From a3388275d0d0e2344c09cfba89f266299bec5f0e Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 13 Aug 2020 20:40:39 -0400 Subject: [PATCH 01/66] Partially functioning port of the flatland default plugins to ros2 --- flatland_plugins/CMakeLists.txt | 2 +- flatland_plugins/plugin_description.xml | 2 +- flatland_plugins/test/bumper_test.cpp | 8 +++-- flatland_plugins/test/diff_drive_test.cpp | 2 +- flatland_plugins/test/gps_test.cpp | 2 +- flatland_plugins/test/laser_test.cpp | 14 +++++--- .../test/model_tf_publisher_test.cpp | 14 +++++--- flatland_plugins/test/tricycle_drive_test.cpp | 2 +- flatland_plugins/test/tween_test.cpp | 11 ++++--- flatland_plugins/test/update_timer_test.cpp | 5 +-- flatland_server/CMakeLists.txt | 33 ++++++++----------- flatland_server/plugin_description.xml | 2 +- flatland_server/src/plugin_manager.cpp | 12 +++++++ .../test/debug_visualization_test.cpp | 2 +- .../test/dummy_model_plugin_test.cpp | 2 +- .../test/dummy_world_plugin_test.cpp | 2 +- flatland_server/test/plugin_manager_test.cpp | 26 ++++++++++----- flatland_server/test/plugin_manager_test.test | 9 ----- flatland_server/test/service_manager_test.cpp | 2 +- .../yaml_preprocessor_test.cpp | 2 +- flatland_viz/src/flatland_viz_node.cpp | 2 +- 21 files changed, 87 insertions(+), 69 deletions(-) delete mode 100644 flatland_server/test/plugin_manager_test.test diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index 482df6b6..dbb5239e 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -93,7 +93,7 @@ ament_package() ## Install ## ############# -pluginlib_export_plugin_description_file(flatland_plugins_lib plugin_description.xml) +pluginlib_export_plugin_description_file(flatland_server plugin_description.xml) install( TARGETS flatland_plugins_lib diff --git a/flatland_plugins/plugin_description.xml b/flatland_plugins/plugin_description.xml index 7b9f985a..555137d2 100644 --- a/flatland_plugins/plugin_description.xml +++ b/flatland_plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + Flatland laser plugin diff --git a/flatland_plugins/test/bumper_test.cpp b/flatland_plugins/test/bumper_test.cpp index 1941d3f9..29f3c825 100644 --- a/flatland_plugins/test/bumper_test.cpp +++ b/flatland_plugins/test/bumper_test.cpp @@ -187,7 +187,8 @@ TEST_F(BumperPluginTest, collision_test) { Timekeeper timekeeper; timekeeper.SetMaxStepSize(0.01); - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); ros::NodeHandle nh; ros::Subscriber sub_1, sub_2, sub_3; @@ -287,7 +288,8 @@ TEST_F(BumperPluginTest, invalid_A) { world_yaml = this_file_dir / fs::path("bumper_tests/invalid_A/world.yaml"); try { - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { std::cmatch match; @@ -305,7 +307,7 @@ TEST_F(BumperPluginTest, invalid_A) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "bumper_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_plugins/test/diff_drive_test.cpp b/flatland_plugins/test/diff_drive_test.cpp index e5e75929..10fccdcd 100644 --- a/flatland_plugins/test/diff_drive_test.cpp +++ b/flatland_plugins/test/diff_drive_test.cpp @@ -64,7 +64,7 @@ TEST(DiffDrivePluginTest, load_test) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "diff_drive_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_plugins/test/gps_test.cpp b/flatland_plugins/test/gps_test.cpp index 7065e5c3..63da15d9 100644 --- a/flatland_plugins/test/gps_test.cpp +++ b/flatland_plugins/test/gps_test.cpp @@ -18,7 +18,7 @@ TEST(GpsPluginTest, load_test) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "gps_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index 963142fb..7f39517f 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/flatland_plugins/test/laser_test.cpp @@ -170,7 +170,8 @@ TEST_F(LaserPluginTest, range_test) { Timekeeper timekeeper; timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); ros::NodeHandle nh; ros::Subscriber sub_1, sub_2, sub_3; @@ -217,7 +218,8 @@ TEST_F(LaserPluginTest, intensity_test) { Timekeeper timekeeper; timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); ros::NodeHandle nh; ros::Subscriber sub_1, sub_2, sub_3; @@ -263,7 +265,8 @@ TEST_F(LaserPluginTest, invalid_A) { world_yaml = this_file_dir / fs::path("laser_tests/invalid_A/world.yaml"); try { - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { @@ -288,7 +291,8 @@ TEST_F(LaserPluginTest, invalid_B) { world_yaml = this_file_dir / fs::path("laser_tests/invalid_B/world.yaml"); try { - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { @@ -307,7 +311,7 @@ TEST_F(LaserPluginTest, invalid_B) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "laser_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_plugins/test/model_tf_publisher_test.cpp b/flatland_plugins/test/model_tf_publisher_test.cpp index befebc3a..e3adc38a 100644 --- a/flatland_plugins/test/model_tf_publisher_test.cpp +++ b/flatland_plugins/test/model_tf_publisher_test.cpp @@ -125,7 +125,8 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_A) { Timekeeper timekeeper; timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); ModelTfPublisher* p = dynamic_cast( w->plugin_manager_.model_plugins_[0].get()); @@ -199,7 +200,8 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_B) { Timekeeper timekeeper; timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); ModelTfPublisher* p = dynamic_cast( w->plugin_manager_.model_plugins_[0].get()); @@ -260,7 +262,8 @@ TEST_F(ModelTfPublisherTest, invalid_A) { this_file_dir / fs::path("model_tf_publisher_tests/invalid_A/world.yaml"); try { - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { @@ -286,7 +289,8 @@ TEST_F(ModelTfPublisherTest, invalid_B) { this_file_dir / fs::path("model_tf_publisher_tests/invalid_B/world.yaml"); try { - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { @@ -306,7 +310,7 @@ TEST_F(ModelTfPublisherTest, invalid_B) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "model_tf_plugin_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_plugins/test/tricycle_drive_test.cpp b/flatland_plugins/test/tricycle_drive_test.cpp index e0cd997d..2d5279e5 100644 --- a/flatland_plugins/test/tricycle_drive_test.cpp +++ b/flatland_plugins/test/tricycle_drive_test.cpp @@ -64,7 +64,7 @@ TEST(TricycleDrivePluginTest, load_test) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "tricycle_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_plugins/test/tween_test.cpp b/flatland_plugins/test/tween_test.cpp index 76c41b71..62271e59 100644 --- a/flatland_plugins/test/tween_test.cpp +++ b/flatland_plugins/test/tween_test.cpp @@ -86,7 +86,8 @@ TEST_F(TweenPluginTest, once_test) { Timekeeper timekeeper; timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(world_yaml.string()); + World* std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); Tween* tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); @@ -120,7 +121,8 @@ TEST_F(TweenPluginTest, yoyo_test) { Timekeeper timekeeper; timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(world_yaml.string()); + World* std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); Tween* tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); @@ -163,7 +165,8 @@ TEST_F(TweenPluginTest, loop_test) { Timekeeper timekeeper; timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(world_yaml.string()); + World* std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); Tween* tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); @@ -201,7 +204,7 @@ TEST_F(TweenPluginTest, loop_test) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "tween_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } \ No newline at end of file diff --git a/flatland_plugins/test/update_timer_test.cpp b/flatland_plugins/test/update_timer_test.cpp index e90b5689..040eb119 100644 --- a/flatland_plugins/test/update_timer_test.cpp +++ b/flatland_plugins/test/update_timer_test.cpp @@ -99,7 +99,8 @@ class UpdateTimerTest : public ::testing::Test { void ExecuteRateTest() { Timekeeper timekeeper; - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); // artificially load a plugin boost::shared_ptr p(new TestPlugin()); @@ -216,7 +217,7 @@ TEST_F(UpdateTimerTest, rate_test_E) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "model_tf_plugin_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index b1c34256..8d369574 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -124,7 +124,7 @@ target_link_libraries(flatland_server ) -ament_export_interfaces(export_flatland_lib HAS_LIBRARY_TARGET) +ament_export_interfaces(flatland_lib HAS_LIBRARY_TARGET) ament_export_libraries(flatland_lib) ament_export_dependencies(rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs) ament_export_include_directories(include) @@ -148,22 +148,17 @@ if(BUILD_TESTING) ament_add_gtest(model_test test/model_test.cpp) target_link_libraries(model_test flatland_lib) -if(FALSE) # fake a block comment + ament_add_gtest(geometry_test test/geometry_test.cpp) + target_link_libraries(geometry_test flatland_lib) - catkin_add_gtest(geometry_test test/geometry_test.cpp) - target_link_libraries(geometry_test - flatland_lib) + ament_add_gtest(collision_filter_registry_test test/collision_filter_registry_test.cpp) + target_link_libraries(collision_filter_registry_test flatland_lib) - catkin_add_gtest(collision_filter_registry_test - test/collision_filter_registry_test.cpp) - target_link_libraries(collision_filter_registry_test - flatland_lib) + ament_add_gtest(plugin_manager_test test/plugin_manager_test.cpp) + target_link_libraries(plugin_manager_test flatland_lib) - add_rostest_gtest(plugin_manager_test - test/plugin_manager_test.test - test/plugin_manager_test.cpp) - target_link_libraries(plugin_manager_test - flatland_lib) + +if(FALSE) # fake a block comment add_rostest_gtest(service_manager_test test/service_manager_test.test @@ -207,11 +202,11 @@ ament_package() ## Install ## ############# -pluginlib_export_plugin_description_file(flatland_lib plugin_description.xml) +pluginlib_export_plugin_description_file(flatland_server plugin_description.xml) install( TARGETS flatland_lib - EXPORT export_flatland_lib + EXPORT flatland_lib LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin @@ -238,8 +233,6 @@ install(DIRECTORY test/conestogo_office_test DESTINATION share/${PROJECT_NAME}/test ) -# TODO + # Install plugins file -#install(FILES flatland_plugins.xml -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -#) +pluginlib_export_plugin_description_file(flatland_lib plugin_description.xml) diff --git a/flatland_server/plugin_description.xml b/flatland_server/plugin_description.xml index 3412bf79..ee373507 100644 --- a/flatland_server/plugin_description.xml +++ b/flatland_server/plugin_description.xml @@ -1,4 +1,4 @@ - + Flatland Dummy Model Plugin, used for testing diff --git a/flatland_server/src/plugin_manager.cpp b/flatland_server/src/plugin_manager.cpp index ba2e37e9..b2dd0e8f 100644 --- a/flatland_server/src/plugin_manager.cpp +++ b/flatland_server/src/plugin_manager.cpp @@ -58,6 +58,18 @@ PluginManager::PluginManager(rclcpp::Node::SharedPtr node) : node_(node) { model_plugin_loader_ = new pluginlib::ClassLoader( "flatland_server", "flatland_server::ModelPlugin"); + RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), "Declared Classes:"); + for(auto a : model_plugin_loader_->getDeclaredClasses()) { + RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a); + } + RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), "Registered Libraries:"); + for(auto a : model_plugin_loader_->getRegisteredLibraries()) { + RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a); + } + RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), "XML Paths:"); + for(auto a : model_plugin_loader_->getPluginXmlPaths()) { + RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a); + } world_plugin_loader_ = new pluginlib::ClassLoader( "flatland_server", "flatland_server::WorldPlugin"); diff --git a/flatland_server/test/debug_visualization_test.cpp b/flatland_server/test/debug_visualization_test.cpp index 9551ba46..e416b858 100644 --- a/flatland_server/test/debug_visualization_test.cpp +++ b/flatland_server/test/debug_visualization_test.cpp @@ -504,7 +504,7 @@ TEST(DebugVizTest, testPublishMarkers) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "debug_visualization_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_server/test/dummy_model_plugin_test.cpp b/flatland_server/test/dummy_model_plugin_test.cpp index aa6f2c71..7e51e743 100644 --- a/flatland_server/test/dummy_model_plugin_test.cpp +++ b/flatland_server/test/dummy_model_plugin_test.cpp @@ -80,7 +80,7 @@ TEST(DummyModelPluginTest, pluginlib_load_test) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "dummy_model_plugin_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_server/test/dummy_world_plugin_test.cpp b/flatland_server/test/dummy_world_plugin_test.cpp index cdabba6b..dfe4b8cd 100644 --- a/flatland_server/test/dummy_world_plugin_test.cpp +++ b/flatland_server/test/dummy_world_plugin_test.cpp @@ -74,7 +74,7 @@ TEST(DummyWorldPluginTest, pluginlib_load_test) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "dummy_world_plugin_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_server/test/plugin_manager_test.cpp b/flatland_server/test/plugin_manager_test.cpp index 195ed95b..670b9832 100644 --- a/flatland_server/test/plugin_manager_test.cpp +++ b/flatland_server/test/plugin_manager_test.cpp @@ -51,6 +51,7 @@ #include #include #include +#include namespace fs = boost::filesystem; using namespace flatland_server; @@ -119,7 +120,6 @@ class PluginManagerTest : public ::testing::Test { protected: boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; - Timekeeper timekeeper; World *w; void SetUp() override { @@ -180,8 +180,11 @@ class PluginManagerTest : public ::testing::Test { TEST_F(PluginManagerTest, collision_test) { world_yaml = this_file_dir / fs::path("plugin_manager_tests/collision_test/world.yaml"); + std::shared_ptr node = rclcpp::Node::make_shared("PluginManagerTest_node"); + Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(world_yaml.string()); + + w = World::MakeWorld(node, world_yaml.string()); Layer *l = w->layers_[0]; Model *m0 = w->models_[0]; Model *m1 = w->models_[1]; @@ -189,7 +192,7 @@ TEST_F(PluginManagerTest, collision_test) { Body *b1 = m1->bodies_[0]; PluginManager *pm = &w->plugin_manager_; std::shared_ptr shared_p(new TestModelPlugin()); - shared_p->Initialize("TestModelPlugin", "test_model_plugin", m0, + shared_p->Initialize(node, "TestModelPlugin", "test_model_plugin", m0, YAML::Node()); pm->model_plugins_.push_back(shared_p); TestModelPlugin *p = shared_p.get(); @@ -302,7 +305,8 @@ TEST_F(PluginManagerTest, load_dummy_test) { world_yaml = this_file_dir / fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); ModelPlugin *p = w->plugin_manager_.model_plugins_[0].get(); @@ -316,7 +320,8 @@ TEST_F(PluginManagerTest, plugin_throws_exception) { fs::path("plugin_manager_tests/plugin_throws_exception/world.yaml"); try { - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException &e) { // do a regex match against error message @@ -340,7 +345,8 @@ TEST_F(PluginManagerTest, nonexistent_plugin) { fs::path("plugin_manager_tests/nonexistent_plugin/world.yaml"); try { - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException &e) { std::cmatch match; @@ -363,7 +369,8 @@ TEST_F(PluginManagerTest, invalid_plugin_yaml) { fs::path("plugin_manager_tests/invalid_plugin_yaml/world.yaml"); try { - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const YAMLException &e) { EXPECT_STREQ( @@ -382,7 +389,8 @@ TEST_F(PluginManagerTest, duplicate_plugin) { fs::path("plugin_manager_tests/duplicate_plugin/world.yaml"); try { - w = World::MakeWorld(world_yaml.string()); + std::shared_ptr node = rclcpp::Node::make_shared("test_node"); + w = World::MakeWorld(node,world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const YAMLException &e) { EXPECT_STREQ( @@ -398,7 +406,7 @@ TEST_F(PluginManagerTest, duplicate_plugin) { // Run all the tests that were declared with TEST() int main(int argc, char **argv) { - ros::init(argc, argv, "plugin_manager_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_server/test/plugin_manager_test.test b/flatland_server/test/plugin_manager_test.test deleted file mode 100644 index 1da47b19..00000000 --- a/flatland_server/test/plugin_manager_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file diff --git a/flatland_server/test/service_manager_test.cpp b/flatland_server/test/service_manager_test.cpp index 43b1c690..97e7a415 100644 --- a/flatland_server/test/service_manager_test.cpp +++ b/flatland_server/test/service_manager_test.cpp @@ -282,7 +282,7 @@ TEST_F(ServiceManagerTest, delete_nonexistent_model) { // Run all the tests that were declared with TEST() int main(int argc, char** argv) { - ros::init(argc, argv, "service_manager_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp index 8fa87cdd..8003dba7 100644 --- a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp +++ b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp @@ -129,7 +129,7 @@ TEST(YamlPreprocTest, testEvalStrings) { // Run all the tests that were declared with TEST() int main(int argc, char **argv) { - ros::init(argc, argv, "yaml_preprocessor_test"); + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_viz/src/flatland_viz_node.cpp b/flatland_viz/src/flatland_viz_node.cpp index 7aea832d..8d7c620f 100644 --- a/flatland_viz/src/flatland_viz_node.cpp +++ b/flatland_viz/src/flatland_viz_node.cpp @@ -69,7 +69,7 @@ void SigintHandler(int sig) { int main(int argc, char** argv) { if (!ros::isInitialized()) { - ros::init(argc, argv, "flatland_viz", ros::init_options::NoSigintHandler); + rclcpp::init(argc, argv); } QApplication app(argc, argv); From e1baf60dcf9e0db4c763292c00837bccef903839 Mon Sep 17 00:00:00 2001 From: Nico Neumann Date: Fri, 14 Aug 2020 13:36:27 +0200 Subject: [PATCH 02/66] fixed plugin dependencies and segmentation faults --- flatland_plugins/CMakeLists.txt | 14 ++++++++++++-- .../include/flatland_plugins/diff_drive.h | 2 +- .../include/flatland_plugins/tricycle_drive.h | 2 +- flatland_plugins/package.xml | 5 +++++ 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index dbb5239e..16b8ffe6 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -29,12 +29,20 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(flatland_server REQUIRED) find_package(flatland_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(interactive_markers REQUIRED) # rostest find_package(Eigen3 REQUIRED)# lua5.1 find_package(Lua 5.1 QUIET) +# Boost +find_package(Boost REQUIRED COMPONENTS date_time system filesystem) + +# OpenCV +find_package(OpenCV REQUIRED) + ############## ## coverage ## ############## @@ -81,13 +89,13 @@ target_include_directories(flatland_plugins_lib PUBLIC target_link_libraries(flatland_plugins_lib ${Eigen3_LIBRARIES} ${LUA_LIBRARIES} + ${OpenCV_LIBRARIES} yaml-cpp ) -ament_export_interfaces(export_flatland_plugins_lib HAS_LIBRARY_TARGET) +ament_export_targets(export_flatland_plugins_lib HAS_LIBRARY_TARGET) # ament_export_libraries(flatland_lib) ament_export_dependencies(rclcpp flatland_server flatland_msgs pluginlib geometry_msgs tf2 tf2_ros tf2_geometry_msgs yaml_cpp_vendor) -ament_package() ############# ## Install ## @@ -160,3 +168,5 @@ if(BUILD_TESTING ) endif() endif() + +ament_package() diff --git a/flatland_plugins/include/flatland_plugins/diff_drive.h b/flatland_plugins/include/flatland_plugins/diff_drive.h index c7621393..ca0d5423 100644 --- a/flatland_plugins/include/flatland_plugins/diff_drive.h +++ b/flatland_plugins/include/flatland_plugins/diff_drive.h @@ -68,7 +68,7 @@ class DiffDrive : public flatland_server::ModelPlugin { rclcpp::Publisher::SharedPtr ground_truth_pub_; rclcpp::Publisher::SharedPtr twist_pub_; Body* body_; - geometry_msgs::msg::Twist::SharedPtr twist_msg_; + geometry_msgs::msg::Twist::SharedPtr twist_msg_ = std::make_shared(); nav_msgs::msg::Odometry odom_msg_; nav_msgs::msg::Odometry ground_truth_msg_; UpdateTimer update_timer_; diff --git a/flatland_plugins/include/flatland_plugins/tricycle_drive.h b/flatland_plugins/include/flatland_plugins/tricycle_drive.h index d4e5d4fd..53e2c933 100644 --- a/flatland_plugins/include/flatland_plugins/tricycle_drive.h +++ b/flatland_plugins/include/flatland_plugins/tricycle_drive.h @@ -78,7 +78,7 @@ class TricycleDrive : public flatland_server::ModelPlugin { double delta_; ///< The current angular offset of the front wheel double d_delta_; ///< The current angular speed of the front wheel - geometry_msgs::msg::Twist::SharedPtr twist_msg_; + geometry_msgs::msg::Twist::SharedPtr twist_msg_ = std::make_shared(); nav_msgs::msg::Odometry odom_msg_; nav_msgs::msg::Odometry ground_truth_msg_; rclcpp::Subscription::SharedPtr twist_sub_; diff --git a/flatland_plugins/package.xml b/flatland_plugins/package.xml index 2e221de1..901b12ab 100644 --- a/flatland_plugins/package.xml +++ b/flatland_plugins/package.xml @@ -28,6 +28,11 @@ flatland_msgs flatland_server lua-dev + visualization_msgs + interactive_markers + libboost-date-time-dev + libboost-filesystem-dev + libboost-dev ament_cmake From 8af48ba94edd103c426fc3aef2895b4ff1cb2bbf Mon Sep 17 00:00:00 2001 From: Nico Neumann Date: Fri, 14 Aug 2020 13:37:04 +0200 Subject: [PATCH 03/66] update flatland_server --- flatland_server/CMakeLists.txt | 7 ++++--- flatland_server/package.xml | 1 + flatland_server/src/flatland_server_node.cpp | 8 ++++---- flatland_server/src/layer.cpp | 2 +- flatland_server/src/world.cpp | 4 ++-- flatland_server/thirdparty/Box2D/CMakeLists.txt | 2 +- 6 files changed, 13 insertions(+), 11 deletions(-) diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 8d369574..6211e60c 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(interactive_markers REQUIRED) find_package(flatland_msgs REQUIRED) @@ -90,7 +91,7 @@ target_include_directories(flatland_lib $ ${LUA_INCLUDE_DIR}) -ament_target_dependencies(flatland_lib rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs visualization_msgs interactive_markers) +ament_target_dependencies(flatland_lib rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs visualization_msgs interactive_markers sensor_msgs) target_link_libraries(flatland_lib ${OpenCV_LIBRARIES} @@ -124,9 +125,9 @@ target_link_libraries(flatland_server ) -ament_export_interfaces(flatland_lib HAS_LIBRARY_TARGET) +ament_export_targets(flatland_lib HAS_LIBRARY_TARGET) ament_export_libraries(flatland_lib) -ament_export_dependencies(rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs) +ament_export_dependencies(rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs sensor_msgs) ament_export_include_directories(include) diff --git a/flatland_server/package.xml b/flatland_server/package.xml index b4f32ccd..15c448f2 100644 --- a/flatland_server/package.xml +++ b/flatland_server/package.xml @@ -23,6 +23,7 @@ tf2_geometry_msgs tf2_ros geometry_msgs + sensor_msgs yaml_cpp_vendor visualization_msgs interactive_markers diff --git a/flatland_server/src/flatland_server_node.cpp b/flatland_server/src/flatland_server_node.cpp index bb274f88..62d86f98 100644 --- a/flatland_server/src/flatland_server_node.cpp +++ b/flatland_server/src/flatland_server_node.cpp @@ -66,8 +66,8 @@ // delete simulation_manager; // simulation_manager = nullptr; // } -// ROS_INFO_STREAM_NAMED("Node", "Beginning ros shutdown"); -// ros::shutdown(); +// RCLCPP_INFO_STREAM_NAMED("Node", "Beginning ros shutdown"); +// rclcpp::shutdown(); // } class FlatlandServerNode : public rclcpp::Node @@ -90,7 +90,7 @@ class FlatlandServerNode : public rclcpp::Node } get_parameter_or("update_rate", update_rate_, 200.0f); get_parameter_or("step_size", step_size_, 1.0f/200.0f); - get_parameter_or("show_viz", show_viz_, 0.0f); + get_parameter_or("show_viz", show_viz_, false); get_parameter_or("viz_pub_rate", viz_pub_rate_, 30.0f); } @@ -112,7 +112,7 @@ class FlatlandServerNode : public rclcpp::Node std::string world_path_; // The file path to the world.yaml file float update_rate_; // The physics update rate (Hz) float step_size_; - float show_viz_; + bool show_viz_; float viz_pub_rate_; std::shared_ptr simulation_manager_; }; diff --git a/flatland_server/src/layer.cpp b/flatland_server/src/layer.cpp index f759c070..f249fa3c 100644 --- a/flatland_server/src/layer.cpp +++ b/flatland_server/src/layer.cpp @@ -162,7 +162,7 @@ Layer *Layer::MakeLayer(std::shared_ptr node, b2World *physics_wor RCLCPP_INFO(rclcpp::get_logger("Layer"), "layer \"%s\" loading image from path=\"%s\"", names[0].c_str(), image_path.string().c_str()); - cv::Mat map = cv::imread(image_path.string(), CV_LOAD_IMAGE_GRAYSCALE); + cv::Mat map = cv::imread(image_path.string(), cv::ImreadModes::IMREAD_GRAYSCALE ); if (map.empty()) { throw YAMLException("Failed to load " + Q(image_path.string()) + " in layer " + Q(names[0])); diff --git a/flatland_server/src/world.cpp b/flatland_server/src/world.cpp index b19c2adf..f4540287 100644 --- a/flatland_server/src/world.cpp +++ b/flatland_server/src/world.cpp @@ -108,7 +108,7 @@ void World::Update(Timekeeper &timekeeper) { timekeeper.StepTime(); plugin_manager_.AfterPhysicsStep(timekeeper); } - //int_marker_manager_.update(); + int_marker_manager_.update(); } void World::BeginContact(b2Contact *contact) { @@ -301,7 +301,7 @@ void World::DeleteModel(const std::string &name) { plugin_manager_.DeleteModelPlugin(models_[i]); delete models_[i]; models_.erase(models_.begin() + i); - //int_marker_manager_.deleteInteractiveMarker(name); + int_marker_manager_.deleteInteractiveMarker(name); found = true; break; } diff --git a/flatland_server/thirdparty/Box2D/CMakeLists.txt b/flatland_server/thirdparty/Box2D/CMakeLists.txt index 13a30c1b..dbc7f2a6 100644 --- a/flatland_server/thirdparty/Box2D/CMakeLists.txt +++ b/flatland_server/thirdparty/Box2D/CMakeLists.txt @@ -146,7 +146,7 @@ set_target_properties(flatland_Box2D PROPERTIES GIT_COMMIT_HASH f655c603ba9d83f07fc566d38d2654ba35739102 ) -ament_export_interfaces(export_flatland_Box2D HAS_LIBRARY_TARGET) +ament_export_targets(export_flatland_Box2D HAS_LIBRARY_TARGET) install( TARGETS flatland_Box2D EXPORT export_flatland_Box2D From a1edcf7db3cb6449396b2b806ec46df20844997d Mon Sep 17 00:00:00 2001 From: Nico Neumann Date: Sun, 16 Aug 2020 13:48:38 +0200 Subject: [PATCH 04/66] update flatland_viz --- flatland_plugins/CMakeLists.txt | 4 +- flatland_viz/CMakeLists.txt | 139 +++++++++++------- flatland_viz/{AMENT_IGNORE => COLCON_IGNORE} | 0 .../include/flatland_viz/flatland_viz.h | 52 ++++--- .../include/flatland_viz/flatland_window.h | 50 +++---- .../include/flatland_viz/load_model_dialog.h | 28 ++-- .../include/flatland_viz/pause_sim_tool.h | 10 +- .../include/flatland_viz/spawn_model_tool.h | 28 ++-- flatland_viz/package.xml | 24 ++- flatland_viz/plugin_description.xml | 4 +- flatland_viz/src/flatland_viz.cpp | 84 ++++++----- flatland_viz/src/flatland_viz_node.cpp | 8 +- flatland_viz/src/flatland_window.cpp | 48 +----- flatland_viz/src/load_model_dialog.cpp | 24 +-- flatland_viz/src/model_dialog.cpp | 12 +- flatland_viz/src/pause_sim_tool.cpp | 13 +- flatland_viz/src/spawn_model_tool.cpp | 73 ++++----- 17 files changed, 299 insertions(+), 302 deletions(-) rename flatland_viz/{AMENT_IGNORE => COLCON_IGNORE} (100%) diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index 16b8ffe6..f1a6c515 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -97,6 +97,8 @@ ament_export_targets(export_flatland_plugins_lib HAS_LIBRARY_TARGET) # ament_export_libraries(flatland_lib) ament_export_dependencies(rclcpp flatland_server flatland_msgs pluginlib geometry_msgs tf2 tf2_ros tf2_geometry_msgs yaml_cpp_vendor) +ament_package() + ############# ## Install ## ############# @@ -168,5 +170,3 @@ if(BUILD_TESTING ) endif() endif() - -ament_package() diff --git a/flatland_viz/CMakeLists.txt b/flatland_viz/CMakeLists.txt index a473ecb8..8243d33b 100644 --- a/flatland_viz/CMakeLists.txt +++ b/flatland_viz/CMakeLists.txt @@ -1,41 +1,45 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(flatland_viz) -# Get the LSB release name (14.04 or 16.04) string into variable ${LSB_RELEASE_VALUE} -FIND_PROGRAM(LSB_RELEASE_EXECUTABLE lsb_release) -EXECUTE_PROCESS(COMMAND ${LSB_RELEASE_EXECUTABLE} -s -r - OUTPUT_VARIABLE LSB_RELEASE_VALUE - OUTPUT_STRIP_TRAILING_WHITESPACE) - -if(${LSB_RELEASE_VALUE} STREQUAL "14.04") -message("Skipping flatland_viz because it is incompatible with 14.04") -return() +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) endif() -# Ensure we're using c++11 -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Find catkin macros and libraries +## Find macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rostest - roscpp - rviz - flatland_server - flatland_msgs -) + +find_package(ament_cmake REQUIRED) + +find_package(rclcpp REQUIRED) +find_package(rviz2 REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(std_srvs REQUIRED) +find_package(flatland_server REQUIRED) +find_package(flatland_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(interactive_markers REQUIRED) + +# Boost +find_package(Boost REQUIRED COMPONENTS date_time system filesystem) ############## ## coverage ## ############## -set(COVERAGE "OFF" CACHE STRING "Enable coverage generation.") +#set(COVERAGE "OFF" CACHE STRING "Enable coverage generation.") -message(STATUS "Using COVERAGE: ${COVERAGE}") -if("${COVERAGE}" STREQUAL "ON") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage -fprofile-arcs -ftest-coverage") -endif() +#message(STATUS "Using COVERAGE: ${COVERAGE}") +#if("${COVERAGE}" STREQUAL "ON") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage -fprofile-arcs -ftest-coverage") +#endif() ######################## ## Ogre Include Stuff ## @@ -55,35 +59,31 @@ endif(NOT OGRE_OV_FOUND) ################################### ## catkin specific configuration ## ################################### -catkin_package( - INCLUDE_DIRS include ${OGRE_OV_INCLUDE_DIRS} - CATKIN_DEPENDS roscpp rviz flatland_server -) +#catkin_package( +# INCLUDE_DIRS include ${OGRE_OV_INCLUDE_DIRS} +# CATKIN_DEPENDS roscpp rviz flatland_server +#) ########### ## Build ## ########### -include_directories(include ${catkin_INCLUDE_DIRS} ${OGRE_OV_INCLUDE_DIRS}) -link_directories(${catkin_LIBRARY_DIRS}) +include_directories(include ${OGRE_OV_INCLUDE_DIRS}) + + + +#link_directories(${catkin_LIBRARY_DIRS}) ## This setting causes Qt's "MOC" generation to happen automatically. set(CMAKE_AUTOMOC ON) ## This plugin includes Qt widgets, so we must include Qt. ## We'll use the version that rviz used so they are compatible. -if(rviz_QT_VERSION VERSION_LESS "5") - message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) - ## pull in all required include dirs, define QT_LIBRARIES, etc. - include(${QT_USE_FILE}) -else() - message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) - ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies - set(QT_LIBRARIES Qt5::Widgets) -endif() +message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") +find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) +## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies +set(QT_LIBRARIES Qt5::Widgets) add_definitions(-DQT_NO_KEYWORDS) @@ -98,14 +98,27 @@ add_executable(flatland_viz include/flatland_viz/load_model_dialog.h src/spawn_model_tool.cpp include/flatland_viz/spawn_model_tool.h - src/pause_sim_tool.cpp - include/flatland_viz/pause_sim_tool.h + src/load_model_dialog.cpp + include/flatland_viz/load_model_dialog.h + #src/pause_sim_tool.cpp + #include/flatland_viz/pause_sim_tool.h ) -add_dependencies(flatland_viz ${catkin_EXPORTED_TARGETS}) + +target_include_directories(flatland_viz + PUBLIC + $ + $ + ${OGRE_OV_INCLUDE_DIRS}) + + +#add_dependencies(flatland_viz ${catkin_EXPORTED_TARGETS}) + +ament_target_dependencies(flatland_viz rclcpp flatland_msgs flatland_server visualization_msgs interactive_markers rviz_common rviz_rendering rviz_default_plugins std_srvs) target_link_libraries(flatland_viz ${QT_LIBRARIES} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${Boost_FILESYSTEM_LIBRARY} ) add_library(flatland_viz_plugins @@ -113,30 +126,44 @@ add_library(flatland_viz_plugins include/flatland_viz/load_model_dialog.h src/spawn_model_tool.cpp include/flatland_viz/spawn_model_tool.h - src/pause_sim_tool.cpp - include/flatland_viz/pause_sim_tool.h) + src/load_model_dialog.cpp + include/flatland_viz/load_model_dialog.h + #src/pause_sim_tool.cpp + #include/flatland_viz/pause_sim_tool.h + ) + +target_include_directories(flatland_viz_plugins + PUBLIC + $ + $ + ${OGRE_OV_INCLUDE_DIRS}) -add_dependencies(flatland_viz_plugins ${catkin_EXPORTED_TARGETS}) +#add_dependencies(flatland_viz_plugins ${catkin_EXPORTED_TARGETS}) +ament_target_dependencies(flatland_viz_plugins rclcpp flatland_msgs flatland_server rviz_common rviz_rendering rviz_default_plugins std_srvs) target_link_libraries(flatland_viz_plugins ${QT_LIBRARIES} - ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + ${Boost_FILESYSTEM_LIBRARY} ) +ament_package() + ############# ## Install ## ############# +pluginlib_export_plugin_description_file(flatland_viz plugin_description.xml) + # Mark executables and/or libraries for installation install(TARGETS flatland_viz flatland_viz_plugins - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION lib/${PROJECT_NAME} ) -install(FILES - plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Mark cpp header files for installation +install( + DIRECTORY include/ + DESTINATION include ) ############# diff --git a/flatland_viz/AMENT_IGNORE b/flatland_viz/COLCON_IGNORE similarity index 100% rename from flatland_viz/AMENT_IGNORE rename to flatland_viz/COLCON_IGNORE diff --git a/flatland_viz/include/flatland_viz/flatland_viz.h b/flatland_viz/include/flatland_viz/flatland_viz.h index c0ae8af4..16969f91 100644 --- a/flatland_viz/include/flatland_viz/flatland_viz.h +++ b/flatland_viz/include/flatland_viz/flatland_viz.h @@ -60,13 +60,21 @@ #include #include -#include "flatland_msgs/DebugTopicList.h" -#include "rviz/config.h" -#include "rviz/panel.h" -#include "rviz/properties/property_tree_widget.h" -#include "rviz/tool.h" -#include "rviz/tool_manager.h" -#include "rviz/window_manager_interface.h" +#include "flatland_msgs/msg/debug_topic_list.hpp" +#include +#include +#include +#include +#include +#include + + +#include +#include +#include +#include "rviz_common/load_resource.hpp" +#include class QSplashScreen; class QAction; @@ -76,7 +84,7 @@ class QDockWidget; class QLabel; class QToolButton; -namespace rviz { +namespace rviz_common { class PanelFactory; class Tool; class Display; @@ -101,23 +109,23 @@ class FlatlandViz : public QWidget { * * @param msg The DebugTopicList message */ - void RecieveDebugTopics(const flatland_msgs::msg::DebugTopicList& msg); + void RecieveDebugTopics(const flatland_msgs::msg::DebugTopicList::SharedPtr msg); /** * @brief Destruct */ virtual ~FlatlandViz(); - rviz::VisualizationManager* manager_; + rviz_common::VisualizationManager* manager_; private: - rviz::RenderPanel* render_panel_; + rviz_common::RenderPanel* render_panel_; - rviz::Display* grid_; - rviz::Display* interactive_markers_; - std::map debug_displays_; + rviz_common::Display* grid_; + rviz_common::Display* interactive_markers_; + std::map debug_displays_; rclcpp::Subscription::SharedPtr debug_topic_subscriber_; - rviz::PropertyTreeWidget* tree_widget_; + rviz_common::properties::PropertyTreeWidget * tree_widget_; FlatlandWindow* parent_; QMenu* file_menu_; @@ -129,8 +137,8 @@ class FlatlandViz : public QWidget { QToolBar* toolbar_; QActionGroup* toolbar_actions_; - std::map action_to_tool_map_; - std::map tool_to_action_map_; + std::map action_to_tool_map_; + std::map tool_to_action_map_; bool show_choose_new_master_option_; QAction* add_tool_action_; @@ -143,10 +151,10 @@ class FlatlandViz : public QWidget { void fullScreenChange(bool hidden); void setDisplayConfigModified(); - void addTool(rviz::Tool*); - void removeTool(rviz::Tool*); - void refreshTool(rviz::Tool*); - void indicateToolIsCurrent(rviz::Tool*); + void addTool(rviz_common::Tool*); + void removeTool(rviz_common::Tool*); + void refreshTool(rviz_common::Tool*); + void indicateToolIsCurrent(rviz_common::Tool*); void onToolbarActionTriggered(QAction* action); void onToolbarRemoveTool(QAction* remove_tool_menu_action); void initToolbars(); @@ -155,4 +163,4 @@ class FlatlandViz : public QWidget { void setFullScreen(bool full_screen); }; -#endif // FLATLAND_VIZ_FLATLAND_VIZ_H \ No newline at end of file +#endif // FLATLAND_VIZ_FLATLAND_VIZ_H diff --git a/flatland_viz/include/flatland_viz/flatland_window.h b/flatland_viz/include/flatland_viz/flatland_window.h index 7bc7f625..b25c6e9d 100644 --- a/flatland_viz/include/flatland_viz/flatland_window.h +++ b/flatland_viz/include/flatland_viz/flatland_window.h @@ -47,49 +47,37 @@ // namespace rviz; #include + #include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include "flatland_viz/flatland_viz.h" -#include "rviz/display.h" -#include "rviz/display_context.h" -#include "rviz/displays_panel.h" -#include "rviz/env_config.h" -#include "rviz/failed_panel.h" -#include "rviz/help_panel.h" -#include "rviz/load_resource.h" -#include "rviz/loading_dialog.h" -#include "rviz/new_object_dialog.h" -#include "rviz/panel_dock_widget.h" -#include "rviz/panel_factory.h" -#include "rviz/properties/status_property.h" -#include "rviz/render_panel.h" -#include "rviz/screenshot_dialog.h" -#include "rviz/selection/selection_manager.h" -#include "rviz/selection_panel.h" -#include "rviz/splash_screen.h" -#include "rviz/time_panel.h" -#include "rviz/tool.h" -#include "rviz/tool_manager.h" -#include "rviz/tool_properties_panel.h" -#include "rviz/view_manager.h" -#include "rviz/views_panel.h" -#include "rviz/visualization_frame.h" -#include "rviz/visualization_manager.h" -#include "rviz/widget_geometry_change_detector.h" -#include "rviz/yaml_config_reader.h" -#include "rviz/yaml_config_writer.h" +#include +#include class FlatlandWindow : public QMainWindow { Q_OBJECT public: FlatlandWindow(QWidget* parent = 0); - rviz::VisualizationManager* visualization_manager_; - rviz::RenderPanel* render_panel_; + rviz_common::VisualizationManager* visualization_manager_; + rviz_common::RenderPanel* render_panel_; - rviz::VisualizationManager* getManager(); + rviz_common::VisualizationManager* getManager(); protected Q_SLOTS: diff --git a/flatland_viz/include/flatland_viz/load_model_dialog.h b/flatland_viz/include/flatland_viz/load_model_dialog.h index 72ec175a..148f0ae8 100644 --- a/flatland_viz/include/flatland_viz/load_model_dialog.h +++ b/flatland_viz/include/flatland_viz/load_model_dialog.h @@ -48,20 +48,6 @@ #ifndef LOAD_MODEL_DIALOG_H #define LOAD_MODEL_DIALOG_H -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - // #include #include #include @@ -72,6 +58,20 @@ #include #include #include +#include +#include + +#include +#include +#include +#include +//#include + +#include + +#include +#include + #include diff --git a/flatland_viz/include/flatland_viz/pause_sim_tool.h b/flatland_viz/include/flatland_viz/pause_sim_tool.h index 5ce0f184..6aec66ae 100644 --- a/flatland_viz/include/flatland_viz/pause_sim_tool.h +++ b/flatland_viz/include/flatland_viz/pause_sim_tool.h @@ -2,8 +2,8 @@ #define PAUSE_SIM_TOOL_H #include -#include -#include +#include +#include namespace flatland_viz { @@ -12,7 +12,7 @@ namespace flatland_viz { * @brief Rviz tool to support pausing and unpausing the * simulation. */ -class PauseSimTool : public rviz::Tool { +class PauseSimTool : public rviz_common::Tool { public: PauseSimTool(); ~PauseSimTool(); @@ -37,9 +37,7 @@ class PauseSimTool : public rviz::Tool { */ virtual void deactivate(); - ros::NodeHandle nh_; ///< NodeHandle to call the pause toggle service - ros::ServiceClient - pause_service_; ///< ServiceClient that calls the pause toggle service + rclcpp::Client::SharedPtr pause_service_; }; } diff --git a/flatland_viz/include/flatland_viz/spawn_model_tool.h b/flatland_viz/include/flatland_viz/spawn_model_tool.h index f837df27..04cce3c7 100644 --- a/flatland_viz/include/flatland_viz/spawn_model_tool.h +++ b/flatland_viz/include/flatland_viz/spawn_model_tool.h @@ -48,27 +48,34 @@ #ifndef SPAWN_MODEL_TOOL_H #define SPAWN_MODEL_TOOL_H -#include +#include #include #include +#include + #include #include #include -#include +#include +//#include #include +#include + #include -#include -#include "rviz/ogre_helpers/arrow.h" +#include +#include +#include "flatland_viz/load_model_dialog.h" + namespace flatland_viz { /** * @name SpawnModelTool * @brief Every tool which can be added to the tool bar is a - * subclass of rviz::Tool. + * subclass of rviz_common::Tool. */ -class SpawnModelTool : public rviz::Tool { +class SpawnModelTool : public rviz_common::Tool { Q_OBJECT public: @@ -119,7 +126,7 @@ class SpawnModelTool : public rviz::Tool { * @brief Main loop of tool * @param event Mouse event */ - virtual int processMouseEvent(rviz::ViewportMouseEvent &event); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent &event); /** * @name SetMovingModelColor * @brief Set the color of the moving model @@ -160,10 +167,9 @@ class SpawnModelTool : public rviz::Tool { static QString model_name; // base file name with path and extension removed protected: - rviz::Arrow *arrow_; // Rviz 3d arrow to show axis of rotation - ros::NodeHandle nh; // ros service node handle - ros::ServiceClient client; // ros service client - std::vector> lines_list_; + rviz_rendering::Arrow *arrow_; // Rviz 3d arrow to show axis of rotation + rclcpp::Client::SharedPtr client; + std::vector> lines_list_; }; } // end namespace flatland_viz diff --git a/flatland_viz/package.xml b/flatland_viz/package.xml index d5df2a50..06a1076d 100644 --- a/flatland_viz/package.xml +++ b/flatland_viz/package.xml @@ -1,7 +1,8 @@ - + + flatland_viz - 1.1.1 + 2.0.0 The flatland gui and visualization BSD https://bitbucket.org/avidbots/flatland @@ -18,12 +19,25 @@ libqt5-gui libqt5-widgets libogre-dev - roscpp - rviz2 + rclcpp + rviz_common + rviz_rendering + rviz_default_plugins + std_srvs flatland_server - flatland_msgs + flatland_msgs + visualization_msgs + interactive_markers + libboost-date-time-dev + libboost-filesystem-dev + libboost-dev + + ament_cmake_gtest + ament_lint_auto + ament_cmake + diff --git a/flatland_viz/plugin_description.xml b/flatland_viz/plugin_description.xml index 9b535527..1f5c43f0 100644 --- a/flatland_viz/plugin_description.xml +++ b/flatland_viz/plugin_description.xml @@ -1,14 +1,14 @@ + base_class_type="rviz_common::Tool"> Tool for spwaning models on the ground plane in flatland. + base_class_type="rviz_common::Tool"> Tool for pausing and unpausing flatland simulation. diff --git a/flatland_viz/src/flatland_viz.cpp b/flatland_viz/src/flatland_viz.cpp index ea67390e..c42cf067 100644 --- a/flatland_viz/src/flatland_viz.cpp +++ b/flatland_viz/src/flatland_viz.cpp @@ -70,11 +70,6 @@ #include #include -#include "rviz/display.h" -#include "rviz/render_panel.h" -#include "rviz/view_manager.h" -#include "rviz/visualization_manager.h" - #include "flatland_viz/flatland_window.h" #include "flatland_viz/flatland_viz.h" @@ -90,7 +85,7 @@ FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { initMenus(); // Construct and lay out render panel. - render_panel_ = new rviz::RenderPanel(); + render_panel_ = new rviz_common::RenderPanel(); QVBoxLayout* main_layout = new QVBoxLayout; main_layout->setMargin(0); main_layout->addWidget(render_panel_); @@ -104,21 +99,21 @@ FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { // holds the main Ogre scene, holds the ViewController, etc. It is // very central and we will probably need one in every usage of // librviz. - manager_ = new rviz::VisualizationManager(render_panel_); + manager_ = new rviz_common::VisualizationManager(render_panel_); render_panel_->initialize(manager_->getSceneManager(), manager_); // bind toolbar events - rviz::ToolManager* tool_man = manager_->getToolManager(); + rviz_common::ToolManager* tool_man = manager_->getToolManager(); connect(manager_, SIGNAL(configChanged()), this, SLOT(setDisplayConfigModified())); - connect(tool_man, &rviz::ToolManager::toolAdded, this, &FlatlandViz::addTool); - connect(tool_man, SIGNAL(toolRemoved(rviz::Tool*)), this, - SLOT(removeTool(rviz::Tool*))); - connect(tool_man, SIGNAL(toolRefreshed(rviz::Tool*)), this, - SLOT(refreshTool(rviz::Tool*))); - connect(tool_man, SIGNAL(toolChanged(rviz::Tool*)), this, - SLOT(indicateToolIsCurrent(rviz::Tool*))); + connect(tool_man, &rviz_common::ToolManager::toolAdded, this, &FlatlandViz::addTool); + connect(tool_man, SIGNAL(toolRemoved(rviz_common::Tool*)), this, + SLOT(removeTool(rviz_common::Tool*))); + connect(tool_man, SIGNAL(toolRefreshed(rviz_common::Tool*)), this, + SLOT(refreshTool(rviz_common::Tool*))); + connect(tool_man, SIGNAL(toolChanged(rviz_common::Tool*)), this, + SLOT(indicateToolIsCurrent(rviz_common::Tool*))); manager_->initialize(); @@ -134,7 +129,7 @@ FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { // Create a Grid display. grid_ = manager_->createDisplay("rviz/Grid", "adjustable grid", true); if (grid_ == nullptr) { - ROS_FATAL("Grid failed to instantiate"); + RCLCPP_WARN(rclcpp::get_logger("flatland_viz"), "Grid failed to instantiate"); exit(1); } @@ -149,16 +144,18 @@ FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { interactive_markers_ = manager_->createDisplay("rviz/InteractiveMarkers", "Move Objects", false); if (interactive_markers_ == nullptr) { - ROS_FATAL("Interactive markers failed to instantiate"); + RCLCPP_WARN(rclcpp::get_logger("flatland_viz"), "Interactive markers failed to instantiate"); exit(1); } interactive_markers_->subProp("Update Topic") ->setValue("/interactive_model_markers/update"); + std::shared_ptr node = rclcpp::Node::make_shared("flatland_viz"); + // Subscribe to debug topics topic - ros::NodeHandle n; - debug_topic_subscriber_ = n.subscribe("/flatland_server/debug/topics", 0, - &FlatlandViz::RecieveDebugTopics, this); + using std::placeholders::_1; + debug_topic_subscriber_ = node->create_subscription( + "/flatland_server/debug/topics", 0, std::bind(&FlatlandViz::RecieveDebugTopics, this, _1)); } // Destructor. @@ -167,7 +164,7 @@ FlatlandViz::~FlatlandViz() { delete manager_; } -void FlatlandViz::indicateToolIsCurrent(rviz::Tool* tool) { +void FlatlandViz::indicateToolIsCurrent(rviz_common::Tool* tool) { QAction* action = tool_to_action_map_[tool]; if (action) { action->setChecked(true); @@ -175,11 +172,11 @@ void FlatlandViz::indicateToolIsCurrent(rviz::Tool* tool) { } void FlatlandViz::setDisplayConfigModified() { - ROS_ERROR("setDisplayConfigModified called"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "setDisplayConfigModified called"); } -void FlatlandViz::addTool(rviz::Tool* tool) { - ROS_ERROR("addTool called"); +void FlatlandViz::addTool(rviz_common::Tool* tool) { + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "addTool called"); QAction* action = new QAction(tool->getName(), toolbar_actions_); action->setIcon(tool->getIcon()); action->setIconText(tool->getName()); @@ -192,10 +189,10 @@ void FlatlandViz::addTool(rviz::Tool* tool) { } void FlatlandViz::onToolbarActionTriggered(QAction* action) { - ROS_ERROR("onToolbarActionTriggered called"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "onToolbarActionTriggered called"); - rviz::Tool* current_tool = manager_->getToolManager()->getCurrentTool(); - rviz::Tool* tool = action_to_tool_map_[action]; + rviz_common::Tool* current_tool = manager_->getToolManager()->getCurrentTool(); + rviz_common::Tool* tool = action_to_tool_map_[action]; if (tool) { manager_->getToolManager()->setCurrentTool(tool); @@ -218,8 +215,8 @@ void FlatlandViz::onToolbarActionTriggered(QAction* action) { } } -void FlatlandViz::removeTool(rviz::Tool* tool) { - ROS_ERROR("removeTool called"); +void FlatlandViz::removeTool(rviz_common::Tool* tool) { + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "removeTool called"); QAction* action = tool_to_action_map_[tool]; if (action) { toolbar_actions_->removeAction(action); @@ -230,7 +227,7 @@ void FlatlandViz::removeTool(rviz::Tool* tool) { QString tool_name = tool->getName(); QList remove_tool_actions = remove_tool_menu_->actions(); for (int i = 0; i < remove_tool_actions.size(); i++) { - ROS_ERROR_STREAM("Removing --------> " << tool_name.toStdString()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("flatland_viz"), "Removing --------> " << tool_name.toStdString()); QAction* removal_action = remove_tool_actions.at(i); if (removal_action->text() == tool_name) { remove_tool_menu_->removeAction(removal_action); @@ -303,7 +300,7 @@ void FlatlandViz::initToolbars() { add_tool_action_ = new QAction("", toolbar_actions_); add_tool_action_->setToolTip("Add a new tool"); - add_tool_action_->setIcon(rviz::loadPixmap("package://rviz/icons/plus.png")); + add_tool_action_->setIcon(rviz_common::loadPixmap("package://rviz_common/icons/plus.png")); toolbar_->addAction(add_tool_action_); connect(add_tool_action_, &QAction::triggered, this, @@ -315,7 +312,7 @@ void FlatlandViz::initToolbars() { remove_tool_button->setPopupMode(QToolButton::InstantPopup); remove_tool_button->setToolTip("Remove a tool from the toolbar"); remove_tool_button->setIcon( - rviz::loadPixmap("package://rviz/icons/minus.png")); + rviz_common::loadPixmap("package://rviz_common/icons/minus.png")); toolbar_->addWidget(remove_tool_button); connect(remove_tool_menu_, &QMenu::triggered, this, @@ -323,13 +320,13 @@ void FlatlandViz::initToolbars() { } void FlatlandViz::openNewToolDialog() { - ROS_ERROR("openNewToolDialog called"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "openNewToolDialog called"); QString class_id; QStringList empty; - rviz::ToolManager* tool_man = manager_->getToolManager(); + rviz_common::ToolManager* tool_man = manager_->getToolManager(); - rviz::NewObjectDialog* dialog = - new rviz::NewObjectDialog(tool_man->getFactory(), "Tool", empty, + rviz_common::NewObjectDialog* dialog = + new rviz_common::NewObjectDialog(tool_man->getFactory(), "Tool", empty, tool_man->getToolClasses(), &class_id); manager_->stopUpdate(); if (dialog->exec() == QDialog::Accepted) { @@ -340,12 +337,13 @@ void FlatlandViz::openNewToolDialog() { } void FlatlandViz::onToolbarRemoveTool(QAction* remove_tool_menu_action) { - ROS_ERROR("onToolbarRemoveTool called"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "onToolbarRemoveTool called"); QString name = remove_tool_menu_action->text(); + for (int i = 0; i < manager_->getToolManager()->numTools(); i++) { - rviz::Tool* tool = manager_->getToolManager()->getTool(i); + rviz_common::Tool* tool = manager_->getToolManager()->getTool(i); if (tool->getName() == name) { - ROS_ERROR_STREAM("Removing --------> " << name.toStdString()); + RCLCPP_ERROR_(rclcpp::get_logger("flatland_viz"), _STREAM("Removing --------> " << name.toStdString()); manager_->getToolManager()->removeTool(i); removeTool(tool); return; @@ -353,7 +351,7 @@ void FlatlandViz::onToolbarRemoveTool(QAction* remove_tool_menu_action) { } } -void FlatlandViz::refreshTool(rviz::Tool* tool) { +void FlatlandViz::refreshTool(rviz_common::Tool* tool) { QAction* action = tool_to_action_map_[tool]; action->setIcon(tool->getIcon()); action->setIconText(tool->getName()); @@ -374,8 +372,8 @@ void FlatlandViz::setFullScreen(bool full_screen) { show(); } -void FlatlandViz::RecieveDebugTopics(const flatland_msgs::msg::DebugTopicList& msg) { - std::vector topics = msg.topics; +void FlatlandViz::RecieveDebugTopics(const flatland_msgs::msg::DebugTopicList::SharedPtr msg) { + std::vector topics = msg->topics; // check for deleted topics for (auto& topic : debug_displays_) { @@ -392,7 +390,7 @@ void FlatlandViz::RecieveDebugTopics(const flatland_msgs::msg::DebugTopicList& m debug_displays_[topic] = manager_->createDisplay( "rviz/MarkerArray", QString::fromLocal8Bit(topic.c_str()), true); if (debug_displays_[topic] == nullptr) { - ROS_FATAL("MarkerArray failed to instantiate"); + RCLCPP_WARN(rclcpp::get_logger("flatland_viz"), "MarkerArray failed to instantiate"); exit(1); } QString topic_qt = QString::fromLocal8Bit( diff --git a/flatland_viz/src/flatland_viz_node.cpp b/flatland_viz/src/flatland_viz_node.cpp index 8d7c620f..502e95e5 100644 --- a/flatland_viz/src/flatland_viz_node.cpp +++ b/flatland_viz/src/flatland_viz_node.cpp @@ -57,18 +57,18 @@ FlatlandWindow* window = nullptr; * @param[in] sig: signal itself */ void SigintHandler(int sig) { - RCLCPP_WARN(rclcpp::get_logger("Node", "*** Shutting down... ***")); + RCLCPP_WARN(rclcpp::get_logger("Node"), "*** Shutting down... ***"); if (window != nullptr) { delete window; window = nullptr; } RCLCPP_INFO_STREAM(rclcpp::get_logger("Flatland Viz"), "Beginning ros shutdown"); - ros::shutdown(); + rclcpp::shutdown(); } int main(int argc, char** argv) { - if (!ros::isInitialized()) { + if (!rclcpp::isInitialized()) { rclcpp::init(argc, argv); } @@ -85,4 +85,4 @@ int main(int argc, char** argv) { delete window; window = nullptr; return 0; -} \ No newline at end of file +} diff --git a/flatland_viz/src/flatland_window.cpp b/flatland_viz/src/flatland_window.cpp index d2e8d78b..580e7574 100644 --- a/flatland_viz/src/flatland_window.cpp +++ b/flatland_viz/src/flatland_window.cpp @@ -47,58 +47,12 @@ #include "flatland_viz/flatland_window.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "rviz/display.h" -#include "rviz/display_context.h" -#include "rviz/displays_panel.h" -#include "rviz/env_config.h" -#include "rviz/failed_panel.h" -#include "rviz/help_panel.h" -#include "rviz/load_resource.h" -#include "rviz/loading_dialog.h" -#include "rviz/new_object_dialog.h" -#include "rviz/panel_dock_widget.h" -#include "rviz/panel_factory.h" -#include "rviz/properties/status_property.h" -#include "rviz/render_panel.h" -#include "rviz/screenshot_dialog.h" -#include "rviz/selection/selection_manager.h" -#include "rviz/selection_panel.h" -#include "rviz/splash_screen.h" -#include "rviz/time_panel.h" -#include "rviz/tool.h" -#include "rviz/tool_manager.h" -#include "rviz/tool_properties_panel.h" -#include "rviz/view_manager.h" -#include "rviz/views_panel.h" -#include "rviz/visualization_frame.h" -#include "rviz/visualization_manager.h" -#include "rviz/widget_geometry_change_detector.h" -#include "rviz/yaml_config_reader.h" -#include "rviz/yaml_config_writer.h" - -#include -#include -//#include - void FlatlandWindow::openNewToolDialog() { QString class_id; QStringList empty; } -rviz::VisualizationManager *FlatlandWindow::getManager() { +rviz_common::VisualizationManager *FlatlandWindow::getManager() { return visualization_manager_; } diff --git a/flatland_viz/src/load_model_dialog.cpp b/flatland_viz/src/load_model_dialog.cpp index 2c54e3e2..6eb96408 100644 --- a/flatland_viz/src/load_model_dialog.cpp +++ b/flatland_viz/src/load_model_dialog.cpp @@ -45,19 +45,23 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "flatland_viz/load_model_dialog.h" + #include #include #include -#include +#include +//#include + +#include -#include +#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -72,10 +76,7 @@ #include -#include "flatland_viz/load_model_dialog.h" #include "flatland_viz/spawn_model_tool.h" -// #include "load_model_dialog.h" -// #include "spawn_model_tool.h" QString LoadModelDialog::path_to_model_file; int LoadModelDialog::count; @@ -178,6 +179,7 @@ void LoadModelDialog::AddNumberAndUpdateName() { boost::filesystem::basename(path_to_model_file.toStdString()); QString name = QString::fromStdString(bsfn); + if (numbering) { name = name.append(QString::number(count++)); } diff --git a/flatland_viz/src/model_dialog.cpp b/flatland_viz/src/model_dialog.cpp index 036651bf..d77ea4bd 100644 --- a/flatland_viz/src/model_dialog.cpp +++ b/flatland_viz/src/model_dialog.cpp @@ -78,7 +78,7 @@ QString ModelDialog::SelectFile() { } ModelDialog::ModelDialog(QWidget *parent) : QDialog(parent) { - ROS_ERROR_STREAM("ModelDialog::ModelDialog"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "ModelDialog::ModelDialog"); path_to_model_file = SelectFile(); QVBoxLayout *v_layout = new QVBoxLayout; @@ -125,11 +125,11 @@ ModelDialog::ModelDialog(QWidget *parent) : QDialog(parent) { static bool first_time = true; QColor color; if (first_time) { - ROS_ERROR_STREAM("firstTime"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "firstTime"); first_time = false; color = Qt::blue; } else { - ROS_ERROR_STREAM("anotherTime"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "anotherTime"); color = saved_color_; } @@ -175,13 +175,13 @@ ModelDialog::ModelDialog(QWidget *parent) : QDialog(parent) { } void ModelDialog::CancelButtonClicked() { - ROS_ERROR_STREAM("Cancel clicked"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "Cancel clicked"); this->close(); } void ModelDialog::OkButtonClicked() { - ROS_ERROR_STREAM("Ok clicked"); - ROS_ERROR_STREAM("connect to ROS model service"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "Ok clicked"); + RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "connect to ROS model service"); ModelDialog::SpawnModelClient(); } diff --git a/flatland_viz/src/pause_sim_tool.cpp b/flatland_viz/src/pause_sim_tool.cpp index 266aae42..491f375c 100644 --- a/flatland_viz/src/pause_sim_tool.cpp +++ b/flatland_viz/src/pause_sim_tool.cpp @@ -5,19 +5,20 @@ namespace flatland_viz { PauseSimTool::PauseSimTool() {} // Disconnect the service client when the tool's destructor is called -PauseSimTool::~PauseSimTool() { pause_service_.shutdown(); } +PauseSimTool::~PauseSimTool() { rclcpp::shutdown(); /*pause_service_->shutdown();*/ } // When the tool is initially loaded, connect to the pause toggle service void PauseSimTool::onInitialize() { - pause_service_ = nh_.serviceClient("toggle_pause"); - setName("Pause/Resume"); + std::shared_ptr node = rclcpp::Node::make_shared("pause_sim_tool"); // TODO + pause_service_ = node->create_client("toggle_pause"); + setName("Pause/Resume"); } // Every time the user presses the tool's Rviz toolbar button, call the pause // toggle service void PauseSimTool::activate() { - std_srvs::Empty empty; - pause_service_.call(empty); + auto request = std::make_shared(); + auto result = pause_service_->async_send_request(request); } void PauseSimTool::deactivate() {} @@ -26,4 +27,4 @@ void PauseSimTool::deactivate() {} // Tell pluginlib about the tool class #include -PLUGINLIB_EXPORT_CLASS(flatland_viz::PauseSimTool, rviz::Tool) +PLUGINLIB_EXPORT_CLASS(flatland_viz::PauseSimTool, rviz_common::Tool) diff --git a/flatland_viz/src/spawn_model_tool.cpp b/flatland_viz/src/spawn_model_tool.cpp index ce2d0ec2..3ec7961d 100644 --- a/flatland_viz/src/spawn_model_tool.cpp +++ b/flatland_viz/src/spawn_model_tool.cpp @@ -45,11 +45,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include -#include - #include #include #include @@ -60,18 +55,20 @@ #include #include -#include +#include "flatland_viz/spawn_model_tool.h" -#include + +#include +//#include +#include +#include + +#include #include #include -#include "flatland_viz/load_model_dialog.h" -#include "flatland_viz/spawn_model_tool.h" -// #include "load_model_dialog.h" -// #include "spawn_model_tool.h" class DialogOptionsWidget; @@ -81,7 +78,7 @@ QString SpawnModelTool::model_name; // Set the "shortcut_key_" member variable defined in the // superclass to declare which key will activate the tool. -SpawnModelTool::SpawnModelTool() : moving_model_node_(NULL) { +SpawnModelTool::SpawnModelTool() : moving_model_node_(nullptr) { shortcut_key_ = 'm'; } @@ -110,7 +107,7 @@ void SpawnModelTool::onInitialize() { model_state = m_hidden; // make an arrow to show axis of rotation - arrow_ = new rviz::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); + arrow_ = new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); arrow_->setColor(0.0f, 0.0f, 1.0f, 1.0f); // blue arrow_->getSceneNode()->setVisible( false); // will only be visible during rotation phase @@ -144,7 +141,7 @@ void SpawnModelTool::onInitialize() { void SpawnModelTool::activate() { RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::activate "); - LoadModelDialog *model_dialog = new LoadModelDialog(NULL, this); + LoadModelDialog *model_dialog = new LoadModelDialog(nullptr, this); model_dialog->setModal(true); model_dialog->show(); } @@ -173,34 +170,35 @@ void SpawnModelTool::deactivate() { } void SpawnModelTool::SpawnModelInFlatland() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::SpawnModelInFlatland name:" << model_name.toStdString()); - flatland_msgs::msg::SpawnModel srv; + auto srv = std::make_shared(); // model names can not have embeded period char model_name = model_name.replace(".", "_", Qt::CaseSensitive); // fill in the service request - srv.request.name = model_name.toStdString(); - srv.request.ns = model_name.toStdString(); - srv.request.yaml_path = path_to_model_file_.toStdString(); - srv.request.pose.x = intersection[0]; - srv.request.pose.y = intersection[1]; - srv.request.pose.theta = initial_angle; + srv->name = model_name.toStdString(); + srv->ns = model_name.toStdString(); + srv->yaml_path = path_to_model_file_.toStdString(); + srv->pose.x = intersection[0]; + srv->pose.y = intersection[1]; + srv->pose.theta = initial_angle; - client = nh.serviceClient("spawn_model"); + std::shared_ptr node = rclcpp::Node::make_shared("spawn_model_tool"); // TODO + client = node->create_client("spawn_model"); // make ros service call - bool client_is_running = client.call(srv); + bool client_is_running = client->call(srv); if (!client_is_running) { QMessageBox msgBox; msgBox.setText("Error: You must have a client running."); msgBox.exec(); } else { - if (!srv.response.success) { + if (!srv->success) { QMessageBox msgBox; - msgBox.setText(srv.response.message.c_str()); + msgBox.setText(srv->message.c_str()); msgBox.exec(); } } @@ -217,14 +215,14 @@ void SpawnModelTool::SetMovingModelColor(QColor c) { m_pMat->getTechnique(0)->getPass(0)->setDiffuse(c.redF(), c.greenF(), c.blueF(), 0); } catch (Ogre::InvalidParametersException e) { - ROS_WARN_STREAM("Invalid preview model"); + //RCLCPP_WARN("Invalid preview model"); } } // processMouseEvent() is sort of the main function of a Tool, because // mouse interactions are the point of Tools. // -// We use the utility function rviz::getPointOnPlaneFromWindowXY() to +// We use the utility function rviz_rendering::getPointOnPlaneFromWindowXY() to // see where on the ground plane the user's mouse is pointing, then // move the moving model to that point and update the VectorProperty. // @@ -233,7 +231,7 @@ void SpawnModelTool::SetMovingModelColor(QColor c) { // place and drop the pointer to the VectorProperty. Dropping the // pointer means when the tool is deactivated the VectorProperty won't // be deleted, which is what we want. -int SpawnModelTool::processMouseEvent(rviz::ViewportMouseEvent &event) { +int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { if (!moving_model_node_) { return Render; } @@ -242,7 +240,7 @@ int SpawnModelTool::processMouseEvent(rviz::ViewportMouseEvent &event) { Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); if (model_state == m_dragging) { - if (rviz::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, + if (rviz_rendering::getPointOnPlaneFromWindowXY (event.viewport, ground_plane, event.x, event.y, intersection)) { moving_model_node_->setVisible(true); moving_model_node_->setPosition(intersection); @@ -262,7 +260,7 @@ int SpawnModelTool::processMouseEvent(rviz::ViewportMouseEvent &event) { } if (model_state == m_rotating) { // model_state is m_rotating - if (rviz::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, + if (rviz_rendering::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, event.y, intersection2)) { if (event.leftDown()) { model_state = m_hidden; @@ -293,8 +291,10 @@ void SpawnModelTool::LoadPreview() { moving_model_node_->removeAllChildren(); lines_list_.clear(); + std::shared_ptr node = rclcpp::Node::make_shared("spawn_model_tool"); // TODO + // Load the bodies list into a model object - flatland_server::YamlReader reader(path_to_model_file_.toStdString()); + flatland_server::YamlReader reader(node, path_to_model_file_.toStdString()); try { flatland_server::YamlReader bodies_reader = @@ -315,7 +315,7 @@ void SpawnModelTool::LoadPreview() { flatland_server::YamlReader footprint = footprints_node.Subnode(j, flatland_server::YamlReader::MAP); - lines_list_.push_back(std::make_shared( + lines_list_.push_back(std::make_shared( context_->getSceneManager(), moving_model_node_)); auto lines = lines_list_.back(); lines->setColor(0.0, 1.0, 0.0, 0.75); // Green @@ -335,7 +335,7 @@ void SpawnModelTool::LoadPreview() { } } } catch (const flatland_server::YAMLException &e) { - ROS_ERROR_STREAM("Couldn't load model bodies for preview" << e.what()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("SpawnModelTool"), "Couldn't load model bodies for preview" << e.what()); } } @@ -349,7 +349,7 @@ void SpawnModelTool::LoadPolygonFootprint( } if (points.size() > 0) { lines->addPoint( - Ogre::Vector3(points.at(0).x, points.at(0).y, 0.)); // Close the box + // Ogre::Vector3(points.at(0).x, points.at(0).y, 0.)); // Close the box } } @@ -358,6 +358,7 @@ void SpawnModelTool::LoadCircleFootprint(flatland_server::YamlReader &footprint, auto lines = lines_list_.back(); auto center = footprint.GetVec2("center", flatland_server::Vec2()); auto radius = footprint.Get("radius", 1.0); + for (float a = 0.; a < M_PI * 2.0; a += M_PI / 8.) { // 16 point circle lines->addPoint(Ogre::Vector3(center.x + radius * cos(a), center.y + radius * sin(a), 0.)); @@ -380,4 +381,4 @@ void SpawnModelTool::SaveName(QString n) { model_name = n; } } // end namespace flatland_viz #include -PLUGINLIB_EXPORT_CLASS(flatland_viz::SpawnModelTool, rviz::Tool) +PLUGINLIB_EXPORT_CLASS(flatland_viz::SpawnModelTool, rviz_common::Tool) From 32e81cdd142e2f9194ee2de128ad152ab1a91f86 Mon Sep 17 00:00:00 2001 From: Nico Neumann Date: Mon, 17 Aug 2020 19:19:28 +0200 Subject: [PATCH 05/66] fixed plugin loading --- flatland_plugins/CMakeLists.txt | 4 ++-- flatland_server/CMakeLists.txt | 7 +------ 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index f1a6c515..16b8ffe6 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -97,8 +97,6 @@ ament_export_targets(export_flatland_plugins_lib HAS_LIBRARY_TARGET) # ament_export_libraries(flatland_lib) ament_export_dependencies(rclcpp flatland_server flatland_msgs pluginlib geometry_msgs tf2 tf2_ros tf2_geometry_msgs yaml_cpp_vendor) -ament_package() - ############# ## Install ## ############# @@ -170,3 +168,5 @@ if(BUILD_TESTING ) endif() endif() + +ament_package() diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 6211e60c..21e13375 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -196,9 +196,6 @@ if(FALSE) # fake a block comment endif() endif() - -ament_package() - ############# ## Install ## ############# @@ -234,6 +231,4 @@ install(DIRECTORY test/conestogo_office_test DESTINATION share/${PROJECT_NAME}/test ) - -# Install plugins file -pluginlib_export_plugin_description_file(flatland_lib plugin_description.xml) +ament_package() From 1c8de6d5e24bba148a72c81a29be1cb382d419e3 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 9 Jun 2022 14:40:28 -0400 Subject: [PATCH 06/66] Fixed a number of ros2 foxy specific compilation issues. Moved box2d out of the flatland_server package into its own --- flatland_box2d/CMakeLists.txt | 174 ++++++++++++++++++ .../Box2D => flatland_box2d}/License.txt | 0 .../include}/Box2D/Box2D.h | 0 .../Box2D/Collision/Shapes/b2ChainShape.h | 0 .../Box2D/Collision/Shapes/b2CircleShape.h | 0 .../Box2D/Collision/Shapes/b2EdgeShape.h | 0 .../Box2D/Collision/Shapes/b2PolygonShape.h | 0 .../include}/Box2D/Collision/Shapes/b2Shape.h | 0 .../include}/Box2D/Collision/b2BroadPhase.h | 0 .../include}/Box2D/Collision/b2Collision.h | 0 .../include}/Box2D/Collision/b2Distance.h | 0 .../include}/Box2D/Collision/b2DynamicTree.h | 0 .../include}/Box2D/Collision/b2TimeOfImpact.h | 0 .../include}/Box2D/Common/b2BlockAllocator.h | 0 .../include}/Box2D/Common/b2Draw.h | 0 .../include}/Box2D/Common/b2GrowableStack.h | 0 .../include}/Box2D/Common/b2Math.h | 0 .../include}/Box2D/Common/b2Settings.h | 0 .../include}/Box2D/Common/b2StackAllocator.h | 0 .../include}/Box2D/Common/b2Timer.h | 0 .../Contacts/b2ChainAndCircleContact.h | 0 .../Contacts/b2ChainAndPolygonContact.h | 0 .../Box2D/Dynamics/Contacts/b2CircleContact.h | 0 .../Box2D/Dynamics/Contacts/b2Contact.h | 0 .../Box2D/Dynamics/Contacts/b2ContactSolver.h | 0 .../Contacts/b2EdgeAndCircleContact.h | 0 .../Contacts/b2EdgeAndPolygonContact.h | 0 .../Contacts/b2PolygonAndCircleContact.h | 0 .../Dynamics/Contacts/b2PolygonContact.h | 0 .../Box2D/Dynamics/Joints/b2DistanceJoint.h | 0 .../Box2D/Dynamics/Joints/b2FrictionJoint.h | 0 .../Box2D/Dynamics/Joints/b2GearJoint.h | 0 .../include}/Box2D/Dynamics/Joints/b2Joint.h | 0 .../Box2D/Dynamics/Joints/b2MotorJoint.h | 0 .../Box2D/Dynamics/Joints/b2MouseJoint.h | 2 +- .../Box2D/Dynamics/Joints/b2PrismaticJoint.h | 0 .../Box2D/Dynamics/Joints/b2PulleyJoint.h | 0 .../Box2D/Dynamics/Joints/b2RevoluteJoint.h | 0 .../Box2D/Dynamics/Joints/b2RopeJoint.h | 0 .../Box2D/Dynamics/Joints/b2WeldJoint.h | 0 .../Box2D/Dynamics/Joints/b2WheelJoint.h | 0 .../include}/Box2D/Dynamics/b2Body.h | 0 .../Box2D/Dynamics/b2ContactManager.h | 0 .../include}/Box2D/Dynamics/b2Fixture.h | 0 .../include}/Box2D/Dynamics/b2Island.h | 0 .../include}/Box2D/Dynamics/b2TimeStep.h | 0 .../include}/Box2D/Dynamics/b2World.h | 0 .../Box2D/Dynamics/b2WorldCallbacks.h | 0 .../include}/Box2D/Rope/b2Rope.h | 0 flatland_box2d/package.xml | 19 ++ .../src}/Collision/Shapes/b2ChainShape.cpp | 0 .../src}/Collision/Shapes/b2CircleShape.cpp | 0 .../src}/Collision/Shapes/b2EdgeShape.cpp | 0 .../src}/Collision/Shapes/b2PolygonShape.cpp | 0 .../src}/Collision/b2BroadPhase.cpp | 0 .../src}/Collision/b2CollideCircle.cpp | 0 .../src}/Collision/b2CollideEdge.cpp | 0 .../src}/Collision/b2CollidePolygon.cpp | 0 .../src}/Collision/b2Collision.cpp | 0 .../src}/Collision/b2Distance.cpp | 0 .../src}/Collision/b2DynamicTree.cpp | 0 .../src}/Collision/b2TimeOfImpact.cpp | 0 .../src}/Common/b2BlockAllocator.cpp | 0 .../src}/Common/b2Draw.cpp | 0 .../src}/Common/b2Math.cpp | 0 .../src}/Common/b2Settings.cpp | 0 .../src}/Common/b2StackAllocator.cpp | 0 .../src}/Common/b2Timer.cpp | 0 .../Contacts/b2ChainAndCircleContact.cpp | 0 .../Contacts/b2ChainAndPolygonContact.cpp | 0 .../Dynamics/Contacts/b2CircleContact.cpp | 0 .../src}/Dynamics/Contacts/b2Contact.cpp | 0 .../Dynamics/Contacts/b2ContactSolver.cpp | 0 .../Contacts/b2EdgeAndCircleContact.cpp | 0 .../Contacts/b2EdgeAndPolygonContact.cpp | 0 .../Contacts/b2PolygonAndCircleContact.cpp | 0 .../Dynamics/Contacts/b2PolygonContact.cpp | 0 .../src}/Dynamics/Joints/b2DistanceJoint.cpp | 0 .../src}/Dynamics/Joints/b2FrictionJoint.cpp | 0 .../src}/Dynamics/Joints/b2GearJoint.cpp | 0 .../src}/Dynamics/Joints/b2Joint.cpp | 0 .../src}/Dynamics/Joints/b2MotorJoint.cpp | 0 .../src}/Dynamics/Joints/b2MouseJoint.cpp | 0 .../src}/Dynamics/Joints/b2PrismaticJoint.cpp | 0 .../src}/Dynamics/Joints/b2PulleyJoint.cpp | 0 .../src}/Dynamics/Joints/b2RevoluteJoint.cpp | 0 .../src}/Dynamics/Joints/b2RopeJoint.cpp | 0 .../src}/Dynamics/Joints/b2WeldJoint.cpp | 0 .../src}/Dynamics/Joints/b2WheelJoint.cpp | 0 .../src}/Dynamics/b2Body.cpp | 0 .../src}/Dynamics/b2ContactManager.cpp | 0 .../src}/Dynamics/b2Fixture.cpp | 0 .../src}/Dynamics/b2Island.cpp | 0 .../src}/Dynamics/b2World.cpp | 0 .../src}/Dynamics/b2WorldCallbacks.cpp | 0 .../src}/Rope/b2Rope.cpp | 0 flatland_plugins/CMakeLists.txt | 7 +- flatland_plugins/package.xml | 5 + flatland_plugins/src/update_timer.cpp | 2 +- flatland_server/CMakeLists.txt | 14 +- flatland_server/package.xml | 2 +- flatland_server/src/flatland_server_node.cpp | 20 +- flatland_server/src/layer.cpp | 2 +- .../thirdparty/Box2D/Box2DConfig.cmake.in | 31 ---- .../thirdparty/Box2D/CMakeLists.txt | 163 ---------------- .../thirdparty/Box2D/UseBox2D.cmake | 9 - 106 files changed, 223 insertions(+), 227 deletions(-) create mode 100644 flatland_box2d/CMakeLists.txt rename {flatland_server/thirdparty/Box2D => flatland_box2d}/License.txt (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Box2D.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/Shapes/b2ChainShape.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/Shapes/b2CircleShape.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/Shapes/b2EdgeShape.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/Shapes/b2PolygonShape.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/Shapes/b2Shape.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/b2BroadPhase.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/b2Collision.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/b2Distance.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/b2DynamicTree.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Collision/b2TimeOfImpact.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Common/b2BlockAllocator.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Common/b2Draw.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Common/b2GrowableStack.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Common/b2Math.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Common/b2Settings.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Common/b2StackAllocator.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Common/b2Timer.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Contacts/b2CircleContact.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Contacts/b2Contact.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Contacts/b2ContactSolver.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Contacts/b2PolygonContact.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2DistanceJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2FrictionJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2GearJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2Joint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2MotorJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2MouseJoint.h (96%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2PrismaticJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2PulleyJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2RevoluteJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2RopeJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2WeldJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/Joints/b2WheelJoint.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/b2Body.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/b2ContactManager.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/b2Fixture.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/b2Island.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/b2TimeStep.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/b2World.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Dynamics/b2WorldCallbacks.h (100%) rename {flatland_server/thirdparty => flatland_box2d/include}/Box2D/Rope/b2Rope.h (100%) create mode 100644 flatland_box2d/package.xml rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/Shapes/b2ChainShape.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/Shapes/b2CircleShape.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/Shapes/b2EdgeShape.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/Shapes/b2PolygonShape.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/b2BroadPhase.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/b2CollideCircle.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/b2CollideEdge.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/b2CollidePolygon.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/b2Collision.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/b2Distance.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/b2DynamicTree.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Collision/b2TimeOfImpact.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Common/b2BlockAllocator.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Common/b2Draw.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Common/b2Math.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Common/b2Settings.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Common/b2StackAllocator.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Common/b2Timer.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Contacts/b2ChainAndCircleContact.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Contacts/b2ChainAndPolygonContact.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Contacts/b2CircleContact.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Contacts/b2Contact.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Contacts/b2ContactSolver.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Contacts/b2EdgeAndCircleContact.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Contacts/b2PolygonAndCircleContact.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Contacts/b2PolygonContact.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2DistanceJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2FrictionJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2GearJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2Joint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2MotorJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2MouseJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2PrismaticJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2PulleyJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2RevoluteJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2RopeJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2WeldJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/Joints/b2WheelJoint.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/b2Body.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/b2ContactManager.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/b2Fixture.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/b2Island.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/b2World.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Dynamics/b2WorldCallbacks.cpp (100%) rename {flatland_server/thirdparty/Box2D => flatland_box2d/src}/Rope/b2Rope.cpp (100%) delete mode 100644 flatland_server/thirdparty/Box2D/Box2DConfig.cmake.in delete mode 100644 flatland_server/thirdparty/Box2D/CMakeLists.txt delete mode 100644 flatland_server/thirdparty/Box2D/UseBox2D.cmake diff --git a/flatland_box2d/CMakeLists.txt b/flatland_box2d/CMakeLists.txt new file mode 100644 index 00000000..3bf42504 --- /dev/null +++ b/flatland_box2d/CMakeLists.txt @@ -0,0 +1,174 @@ + +set(BOX2D_Collision_SRCS + src/Collision/b2BroadPhase.cpp + src/Collision/b2CollideCircle.cpp + src/Collision/b2CollideEdge.cpp + src/Collision/b2CollidePolygon.cpp + src/Collision/b2Collision.cpp + src/Collision/b2Distance.cpp + src/Collision/b2DynamicTree.cpp + src/Collision/b2TimeOfImpact.cpp +) +set(BOX2D_Collision_HDRS + + include/Box2D/Collision/b2Collision.h + include/Box2D/Collision/b2Distance.h + include/Box2D/Collision/b2DynamicTree.h + include/Box2D/Collision/b2TimeOfImpact.h +) +set(BOX2D_Shapes_SRCS + src/Collision/Shapes/b2CircleShape.cpp + src/Collision/Shapes/b2EdgeShape.cpp + src/Collision/Shapes/b2ChainShape.cpp + src/Collision/Shapes/b2PolygonShape.cpp +) +set(BOX2D_Shapes_HDRS + include/Box2D/Collision/Shapes/b2CircleShape.h + include/Box2D/Collision/Shapes/b2EdgeShape.h + include/Box2D/Collision/Shapes/b2ChainShape.h + include/Box2D/Collision/Shapes/b2PolygonShape.h + include/Box2D/Collision/Shapes/b2Shape.h +) +set(BOX2D_Common_SRCS + src/Common/b2BlockAllocator.cpp + src/Common/b2Draw.cpp + src/Common/b2Math.cpp + src/Common/b2Settings.cpp + src/Common/b2StackAllocator.cpp + src/Common/b2Timer.cpp +) +set(BOX2D_Common_HDRS + include/Box2D/Common/b2BlockAllocator.h + include/Box2D/Common/b2Draw.h + include/Box2D/Common/b2GrowableStack.h + include/Box2D/Common/b2Math.h + include/Box2D/Common/b2Settings.h + include/Box2D/Common/b2StackAllocator.h + include/Box2D/Common/b2Timer.h +) +set(BOX2D_Dynamics_SRCS + src/Dynamics/b2Body.cpp + src/Dynamics/b2ContactManager.cpp + src/Dynamics/b2Fixture.cpp + src/Dynamics/b2Island.cpp + src/Dynamics/b2World.cpp + src/Dynamics/b2WorldCallbacks.cpp +) +set(BOX2D_Dynamics_HDRS + include/Box2D/Dynamics/b2Body.h + include/Box2D/Dynamics/b2ContactManager.h + include/Box2D/Dynamics/b2Fixture.h + include/Box2D/Dynamics/b2Island.h + include/Box2D/Dynamics/b2TimeStep.h + include/Box2D/Dynamics/b2World.h + include/Box2D/Dynamics/b2WorldCallbacks.h +) +set(BOX2D_Contacts_SRCS + src/Dynamics/Contacts/b2CircleContact.cpp + src/Dynamics/Contacts/b2Contact.cpp + src/Dynamics/Contacts/b2ContactSolver.cpp + src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp + src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp + src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp + src/Dynamics/Contacts/b2ChainAndCircleContact.cpp + src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp + src/Dynamics/Contacts/b2PolygonContact.cpp +) +set(BOX2D_Contacts_HDRS + include/Box2D/Dynamics/Contacts/b2CircleContact.h + include/Box2D/Dynamics/Contacts/b2Contact.h + include/Box2D/Dynamics/Contacts/b2ContactSolver.h + include/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h + include/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h + include/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h + include/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h + include/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h + include/Box2D/Dynamics/Contacts/b2PolygonContact.h +) +set(BOX2D_Joints_SRCS + src/Dynamics/Joints/b2DistanceJoint.cpp + src/Dynamics/Joints/b2FrictionJoint.cpp + src/Dynamics/Joints/b2GearJoint.cpp + src/Dynamics/Joints/b2Joint.cpp + src/Dynamics/Joints/b2MotorJoint.cpp + src/Dynamics/Joints/b2MouseJoint.cpp + src/Dynamics/Joints/b2PrismaticJoint.cpp + src/Dynamics/Joints/b2PulleyJoint.cpp + src/Dynamics/Joints/b2RevoluteJoint.cpp + src/Dynamics/Joints/b2RopeJoint.cpp + src/Dynamics/Joints/b2WeldJoint.cpp + src/Dynamics/Joints/b2WheelJoint.cpp +) +set(BOX2D_Joints_HDRS + include/Box2D/Dynamics/Joints/b2DistanceJoint.h + include/Box2D/Dynamics/Joints/b2FrictionJoint.h + include/Box2D/Dynamics/Joints/b2GearJoint.h + include/Box2D/Dynamics/Joints/b2Joint.h + include/Box2D/Dynamics/Joints/b2MotorJoint.h + include/Box2D/Dynamics/Joints/b2MouseJoint.h + include/Box2D/Dynamics/Joints/b2PrismaticJoint.h + include/Box2D/Dynamics/Joints/b2PulleyJoint.h + include/Box2D/Dynamics/Joints/b2RevoluteJoint.h + include/Box2D/Dynamics/Joints/b2RopeJoint.h + include/Box2D/Dynamics/Joints/b2WeldJoint.h + include/Box2D/Dynamics/Joints/b2WheelJoint.h +) +set(BOX2D_Rope_SRCS + src/Rope/b2Rope.cpp +) +set(BOX2D_Rope_HDRS + include/Box2D/Rope/b2Rope.h +) +set(BOX2D_General_HDRS + include/Box2D/Box2D.h +) + +cmake_minimum_required(VERSION 3.5) +project(flatland_box2d) + +find_package(ament_cmake REQUIRED) + +include_directories(include) + +add_library(flatland_box2d_lib SHARED + ${BOX2D_General_HDRS} + ${BOX2D_Joints_SRCS} + ${BOX2D_Joints_HDRS} + ${BOX2D_Contacts_SRCS} + ${BOX2D_Contacts_HDRS} + ${BOX2D_Dynamics_SRCS} + ${BOX2D_Dynamics_HDRS} + ${BOX2D_Common_SRCS} + ${BOX2D_Common_HDRS} + ${BOX2D_Shapes_SRCS} + ${BOX2D_Shapes_HDRS} + ${BOX2D_Collision_SRCS} + ${BOX2D_Collision_HDRS} + ${BOX2D_Rope_SRCS} + ${BOX2D_Rope_HDRS} +) +set_target_properties(flatland_box2d_lib PROPERTIES + GIT_COMMIT_HASH f655c603ba9d83f07fc566d38d2654ba35739102 +) + +ament_export_targets(export_flatland_box2d HAS_LIBRARY_TARGET) +ament_export_libraries(flatland_box2d_lib) +ament_export_include_directories(include) + +ament_package() + +install( + TARGETS flatland_box2d_lib + EXPORT export_flatland_box2d + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# Mark cpp header files for installation +install( + DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" +) \ No newline at end of file diff --git a/flatland_server/thirdparty/Box2D/License.txt b/flatland_box2d/License.txt similarity index 100% rename from flatland_server/thirdparty/Box2D/License.txt rename to flatland_box2d/License.txt diff --git a/flatland_server/thirdparty/Box2D/Box2D.h b/flatland_box2d/include/Box2D/Box2D.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Box2D.h rename to flatland_box2d/include/Box2D/Box2D.h diff --git a/flatland_server/thirdparty/Box2D/Collision/Shapes/b2ChainShape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2ChainShape.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/Shapes/b2ChainShape.h rename to flatland_box2d/include/Box2D/Collision/Shapes/b2ChainShape.h diff --git a/flatland_server/thirdparty/Box2D/Collision/Shapes/b2CircleShape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2CircleShape.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/Shapes/b2CircleShape.h rename to flatland_box2d/include/Box2D/Collision/Shapes/b2CircleShape.h diff --git a/flatland_server/thirdparty/Box2D/Collision/Shapes/b2EdgeShape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2EdgeShape.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/Shapes/b2EdgeShape.h rename to flatland_box2d/include/Box2D/Collision/Shapes/b2EdgeShape.h diff --git a/flatland_server/thirdparty/Box2D/Collision/Shapes/b2PolygonShape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2PolygonShape.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/Shapes/b2PolygonShape.h rename to flatland_box2d/include/Box2D/Collision/Shapes/b2PolygonShape.h diff --git a/flatland_server/thirdparty/Box2D/Collision/Shapes/b2Shape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2Shape.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/Shapes/b2Shape.h rename to flatland_box2d/include/Box2D/Collision/Shapes/b2Shape.h diff --git a/flatland_server/thirdparty/Box2D/Collision/b2BroadPhase.h b/flatland_box2d/include/Box2D/Collision/b2BroadPhase.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2BroadPhase.h rename to flatland_box2d/include/Box2D/Collision/b2BroadPhase.h diff --git a/flatland_server/thirdparty/Box2D/Collision/b2Collision.h b/flatland_box2d/include/Box2D/Collision/b2Collision.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2Collision.h rename to flatland_box2d/include/Box2D/Collision/b2Collision.h diff --git a/flatland_server/thirdparty/Box2D/Collision/b2Distance.h b/flatland_box2d/include/Box2D/Collision/b2Distance.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2Distance.h rename to flatland_box2d/include/Box2D/Collision/b2Distance.h diff --git a/flatland_server/thirdparty/Box2D/Collision/b2DynamicTree.h b/flatland_box2d/include/Box2D/Collision/b2DynamicTree.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2DynamicTree.h rename to flatland_box2d/include/Box2D/Collision/b2DynamicTree.h diff --git a/flatland_server/thirdparty/Box2D/Collision/b2TimeOfImpact.h b/flatland_box2d/include/Box2D/Collision/b2TimeOfImpact.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2TimeOfImpact.h rename to flatland_box2d/include/Box2D/Collision/b2TimeOfImpact.h diff --git a/flatland_server/thirdparty/Box2D/Common/b2BlockAllocator.h b/flatland_box2d/include/Box2D/Common/b2BlockAllocator.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2BlockAllocator.h rename to flatland_box2d/include/Box2D/Common/b2BlockAllocator.h diff --git a/flatland_server/thirdparty/Box2D/Common/b2Draw.h b/flatland_box2d/include/Box2D/Common/b2Draw.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2Draw.h rename to flatland_box2d/include/Box2D/Common/b2Draw.h diff --git a/flatland_server/thirdparty/Box2D/Common/b2GrowableStack.h b/flatland_box2d/include/Box2D/Common/b2GrowableStack.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2GrowableStack.h rename to flatland_box2d/include/Box2D/Common/b2GrowableStack.h diff --git a/flatland_server/thirdparty/Box2D/Common/b2Math.h b/flatland_box2d/include/Box2D/Common/b2Math.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2Math.h rename to flatland_box2d/include/Box2D/Common/b2Math.h diff --git a/flatland_server/thirdparty/Box2D/Common/b2Settings.h b/flatland_box2d/include/Box2D/Common/b2Settings.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2Settings.h rename to flatland_box2d/include/Box2D/Common/b2Settings.h diff --git a/flatland_server/thirdparty/Box2D/Common/b2StackAllocator.h b/flatland_box2d/include/Box2D/Common/b2StackAllocator.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2StackAllocator.h rename to flatland_box2d/include/Box2D/Common/b2StackAllocator.h diff --git a/flatland_server/thirdparty/Box2D/Common/b2Timer.h b/flatland_box2d/include/Box2D/Common/b2Timer.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2Timer.h rename to flatland_box2d/include/Box2D/Common/b2Timer.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h rename to flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h rename to flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2CircleContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2CircleContact.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2CircleContact.h rename to flatland_box2d/include/Box2D/Dynamics/Contacts/b2CircleContact.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2Contact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2Contact.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2Contact.h rename to flatland_box2d/include/Box2D/Dynamics/Contacts/b2Contact.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ContactSolver.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ContactSolver.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ContactSolver.h rename to flatland_box2d/include/Box2D/Dynamics/Contacts/b2ContactSolver.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h rename to flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h rename to flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h rename to flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2PolygonContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonContact.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2PolygonContact.h rename to flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonContact.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2DistanceJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2DistanceJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2DistanceJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2DistanceJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2FrictionJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2FrictionJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2FrictionJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2FrictionJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2GearJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2GearJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2GearJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2GearJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2Joint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2Joint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2Joint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2Joint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2MotorJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2MotorJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2MotorJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2MotorJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2MouseJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2MouseJoint.h similarity index 96% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2MouseJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2MouseJoint.h index ea213c7e..2606f2f7 100644 --- a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2MouseJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2MouseJoint.h @@ -93,7 +93,7 @@ class b2MouseJoint : public b2Joint void Dump() override { b2Log("Mouse joint dumping is not supported.\n"); } /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin) override; + void ShiftOrigin(const b2Vec2& newOrigin) override; protected: friend class b2Joint; diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2PrismaticJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2PrismaticJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2PrismaticJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2PulleyJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2PulleyJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2PulleyJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2PulleyJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2RevoluteJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2RevoluteJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2RevoluteJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2RopeJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2RopeJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2RopeJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2RopeJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2WeldJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2WeldJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2WeldJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2WeldJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2WheelJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2WheelJoint.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2WheelJoint.h rename to flatland_box2d/include/Box2D/Dynamics/Joints/b2WheelJoint.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2Body.h b/flatland_box2d/include/Box2D/Dynamics/b2Body.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2Body.h rename to flatland_box2d/include/Box2D/Dynamics/b2Body.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2ContactManager.h b/flatland_box2d/include/Box2D/Dynamics/b2ContactManager.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2ContactManager.h rename to flatland_box2d/include/Box2D/Dynamics/b2ContactManager.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2Fixture.h b/flatland_box2d/include/Box2D/Dynamics/b2Fixture.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2Fixture.h rename to flatland_box2d/include/Box2D/Dynamics/b2Fixture.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2Island.h b/flatland_box2d/include/Box2D/Dynamics/b2Island.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2Island.h rename to flatland_box2d/include/Box2D/Dynamics/b2Island.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2TimeStep.h b/flatland_box2d/include/Box2D/Dynamics/b2TimeStep.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2TimeStep.h rename to flatland_box2d/include/Box2D/Dynamics/b2TimeStep.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2World.h b/flatland_box2d/include/Box2D/Dynamics/b2World.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2World.h rename to flatland_box2d/include/Box2D/Dynamics/b2World.h diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2WorldCallbacks.h b/flatland_box2d/include/Box2D/Dynamics/b2WorldCallbacks.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2WorldCallbacks.h rename to flatland_box2d/include/Box2D/Dynamics/b2WorldCallbacks.h diff --git a/flatland_server/thirdparty/Box2D/Rope/b2Rope.h b/flatland_box2d/include/Box2D/Rope/b2Rope.h similarity index 100% rename from flatland_server/thirdparty/Box2D/Rope/b2Rope.h rename to flatland_box2d/include/Box2D/Rope/b2Rope.h diff --git a/flatland_box2d/package.xml b/flatland_box2d/package.xml new file mode 100644 index 00000000..87f4f649 --- /dev/null +++ b/flatland_box2d/package.xml @@ -0,0 +1,19 @@ + + + + flatland_box2d + 2.0.0 + The flatland Box2D ROS wrapped package + BSD + https://github.com/avidbots/flatland + + Joseph Duchesne + Joseph Duchesne + + ament_cmake + rclcpp + + + ament_cmake + + diff --git a/flatland_server/thirdparty/Box2D/Collision/Shapes/b2ChainShape.cpp b/flatland_box2d/src/Collision/Shapes/b2ChainShape.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/Shapes/b2ChainShape.cpp rename to flatland_box2d/src/Collision/Shapes/b2ChainShape.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/Shapes/b2CircleShape.cpp b/flatland_box2d/src/Collision/Shapes/b2CircleShape.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/Shapes/b2CircleShape.cpp rename to flatland_box2d/src/Collision/Shapes/b2CircleShape.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/Shapes/b2EdgeShape.cpp b/flatland_box2d/src/Collision/Shapes/b2EdgeShape.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/Shapes/b2EdgeShape.cpp rename to flatland_box2d/src/Collision/Shapes/b2EdgeShape.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/Shapes/b2PolygonShape.cpp b/flatland_box2d/src/Collision/Shapes/b2PolygonShape.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/Shapes/b2PolygonShape.cpp rename to flatland_box2d/src/Collision/Shapes/b2PolygonShape.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/b2BroadPhase.cpp b/flatland_box2d/src/Collision/b2BroadPhase.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2BroadPhase.cpp rename to flatland_box2d/src/Collision/b2BroadPhase.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/b2CollideCircle.cpp b/flatland_box2d/src/Collision/b2CollideCircle.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2CollideCircle.cpp rename to flatland_box2d/src/Collision/b2CollideCircle.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/b2CollideEdge.cpp b/flatland_box2d/src/Collision/b2CollideEdge.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2CollideEdge.cpp rename to flatland_box2d/src/Collision/b2CollideEdge.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/b2CollidePolygon.cpp b/flatland_box2d/src/Collision/b2CollidePolygon.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2CollidePolygon.cpp rename to flatland_box2d/src/Collision/b2CollidePolygon.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/b2Collision.cpp b/flatland_box2d/src/Collision/b2Collision.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2Collision.cpp rename to flatland_box2d/src/Collision/b2Collision.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/b2Distance.cpp b/flatland_box2d/src/Collision/b2Distance.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2Distance.cpp rename to flatland_box2d/src/Collision/b2Distance.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/b2DynamicTree.cpp b/flatland_box2d/src/Collision/b2DynamicTree.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2DynamicTree.cpp rename to flatland_box2d/src/Collision/b2DynamicTree.cpp diff --git a/flatland_server/thirdparty/Box2D/Collision/b2TimeOfImpact.cpp b/flatland_box2d/src/Collision/b2TimeOfImpact.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Collision/b2TimeOfImpact.cpp rename to flatland_box2d/src/Collision/b2TimeOfImpact.cpp diff --git a/flatland_server/thirdparty/Box2D/Common/b2BlockAllocator.cpp b/flatland_box2d/src/Common/b2BlockAllocator.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2BlockAllocator.cpp rename to flatland_box2d/src/Common/b2BlockAllocator.cpp diff --git a/flatland_server/thirdparty/Box2D/Common/b2Draw.cpp b/flatland_box2d/src/Common/b2Draw.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2Draw.cpp rename to flatland_box2d/src/Common/b2Draw.cpp diff --git a/flatland_server/thirdparty/Box2D/Common/b2Math.cpp b/flatland_box2d/src/Common/b2Math.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2Math.cpp rename to flatland_box2d/src/Common/b2Math.cpp diff --git a/flatland_server/thirdparty/Box2D/Common/b2Settings.cpp b/flatland_box2d/src/Common/b2Settings.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2Settings.cpp rename to flatland_box2d/src/Common/b2Settings.cpp diff --git a/flatland_server/thirdparty/Box2D/Common/b2StackAllocator.cpp b/flatland_box2d/src/Common/b2StackAllocator.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2StackAllocator.cpp rename to flatland_box2d/src/Common/b2StackAllocator.cpp diff --git a/flatland_server/thirdparty/Box2D/Common/b2Timer.cpp b/flatland_box2d/src/Common/b2Timer.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Common/b2Timer.cpp rename to flatland_box2d/src/Common/b2Timer.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2ChainAndCircleContact.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp rename to flatland_box2d/src/Dynamics/Contacts/b2ChainAndCircleContact.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp rename to flatland_box2d/src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2CircleContact.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2CircleContact.cpp rename to flatland_box2d/src/Dynamics/Contacts/b2CircleContact.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2Contact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2Contact.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2Contact.cpp rename to flatland_box2d/src/Dynamics/Contacts/b2Contact.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/flatland_box2d/src/Dynamics/Contacts/b2ContactSolver.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2ContactSolver.cpp rename to flatland_box2d/src/Dynamics/Contacts/b2ContactSolver.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp rename to flatland_box2d/src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp rename to flatland_box2d/src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp rename to flatland_box2d/src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2PolygonContact.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Contacts/b2PolygonContact.cpp rename to flatland_box2d/src/Dynamics/Contacts/b2PolygonContact.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2DistanceJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2DistanceJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2DistanceJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2FrictionJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2FrictionJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2FrictionJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2GearJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2GearJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2GearJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2GearJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2Joint.cpp b/flatland_box2d/src/Dynamics/Joints/b2Joint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2Joint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2Joint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2MotorJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2MotorJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2MotorJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2MotorJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2MouseJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2MouseJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2MouseJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2PrismaticJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2PrismaticJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2PulleyJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2PulleyJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2PulleyJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2RevoluteJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2RevoluteJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2RopeJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2RopeJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2RopeJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2WeldJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2WeldJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2WeldJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2WheelJoint.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/Joints/b2WheelJoint.cpp rename to flatland_box2d/src/Dynamics/Joints/b2WheelJoint.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2Body.cpp b/flatland_box2d/src/Dynamics/b2Body.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2Body.cpp rename to flatland_box2d/src/Dynamics/b2Body.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2ContactManager.cpp b/flatland_box2d/src/Dynamics/b2ContactManager.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2ContactManager.cpp rename to flatland_box2d/src/Dynamics/b2ContactManager.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2Fixture.cpp b/flatland_box2d/src/Dynamics/b2Fixture.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2Fixture.cpp rename to flatland_box2d/src/Dynamics/b2Fixture.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2Island.cpp b/flatland_box2d/src/Dynamics/b2Island.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2Island.cpp rename to flatland_box2d/src/Dynamics/b2Island.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp b/flatland_box2d/src/Dynamics/b2World.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2World.cpp rename to flatland_box2d/src/Dynamics/b2World.cpp diff --git a/flatland_server/thirdparty/Box2D/Dynamics/b2WorldCallbacks.cpp b/flatland_box2d/src/Dynamics/b2WorldCallbacks.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Dynamics/b2WorldCallbacks.cpp rename to flatland_box2d/src/Dynamics/b2WorldCallbacks.cpp diff --git a/flatland_server/thirdparty/Box2D/Rope/b2Rope.cpp b/flatland_box2d/src/Rope/b2Rope.cpp similarity index 100% rename from flatland_server/thirdparty/Box2D/Rope/b2Rope.cpp rename to flatland_box2d/src/Rope/b2Rope.cpp diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index 482df6b6..078d5ee2 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -21,6 +21,8 @@ find_package(ament_cmake REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(interactive_markers REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(tf2 REQUIRED) @@ -29,12 +31,15 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(flatland_server REQUIRED) find_package(flatland_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) +find_package(OpenCV REQUIRED) # rostest find_package(Eigen3 REQUIRED)# lua5.1 find_package(Lua 5.1 QUIET) +find_package(Boost REQUIRED COMPONENTS date_time system filesystem) + ############## ## coverage ## ############## @@ -84,7 +89,7 @@ target_link_libraries(flatland_plugins_lib yaml-cpp ) -ament_export_interfaces(export_flatland_plugins_lib HAS_LIBRARY_TARGET) +ament_export_targets(export_flatland_plugins_lib HAS_LIBRARY_TARGET) # ament_export_libraries(flatland_lib) ament_export_dependencies(rclcpp flatland_server flatland_msgs pluginlib geometry_msgs tf2 tf2_ros tf2_geometry_msgs yaml_cpp_vendor) ament_package() diff --git a/flatland_plugins/package.xml b/flatland_plugins/package.xml index 2e221de1..b190918f 100644 --- a/flatland_plugins/package.xml +++ b/flatland_plugins/package.xml @@ -27,7 +27,12 @@ yaml_cpp_vendor flatland_msgs flatland_server + visualization_msgs + interactive_markers lua-dev + libboost-date-time-dev + libboost-filesystem-dev + libboost-dev ament_cmake diff --git a/flatland_plugins/src/update_timer.cpp b/flatland_plugins/src/update_timer.cpp index bad5355e..d73c9acc 100644 --- a/flatland_plugins/src/update_timer.cpp +++ b/flatland_plugins/src/update_timer.cpp @@ -50,7 +50,7 @@ namespace flatland_plugins { UpdateTimer::UpdateTimer() - : period_(rclcpp::Duration(0)), last_update_time_(rclcpp::Time(0, 0)) {} + : period_(rclcpp::Duration(0,0)), last_update_time_(rclcpp::Time(0, 0)) {} void UpdateTimer::SetRate(double rate) { if (rate == 0.0) diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index b1c34256..a6f39fdd 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -29,6 +29,7 @@ find_package(visualization_msgs REQUIRED) find_package(interactive_markers REQUIRED) find_package(flatland_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) +find_package(flatland_box2d REQUIRED) # lua5.1 find_package(Lua 5.1 QUIET) @@ -36,9 +37,6 @@ find_package(Lua 5.1 QUIET) # OpenCV find_package(OpenCV REQUIRED) -# Box2D -find_package(Box2D REQUIRED) - # Boost find_package(Boost REQUIRED COMPONENTS date_time system filesystem) find_package(Threads) @@ -90,20 +88,19 @@ target_include_directories(flatland_lib $ ${LUA_INCLUDE_DIR}) -ament_target_dependencies(flatland_lib rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs visualization_msgs interactive_markers) +ament_target_dependencies(flatland_lib flatland_box2d rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs visualization_msgs interactive_markers) target_link_libraries(flatland_lib ${OpenCV_LIBRARIES} ${Boost_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY} ${LUA_LIBRARIES} - ${BOX2D_LIBRARIES} yaml-cpp ) ## Declare a C++ executable add_executable(flatland_server src/flatland_server_node.cpp) -ament_target_dependencies(flatland_server rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs visualization_msgs interactive_markers) +ament_target_dependencies(flatland_server flatland_box2d rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs visualization_msgs interactive_markers) target_include_directories(flatland_server PUBLIC @@ -118,15 +115,14 @@ target_link_libraries(flatland_server ${Boost_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY} ${LUA_LIBRARIES} - ${BOX2D_LIBRARIES} yaml-cpp flatland_lib ) -ament_export_interfaces(export_flatland_lib HAS_LIBRARY_TARGET) +ament_export_targets(export_flatland_lib HAS_LIBRARY_TARGET) ament_export_libraries(flatland_lib) -ament_export_dependencies(rclcpp flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs) +ament_export_dependencies(rclcpp flatland_box2d flatland_msgs yaml_cpp_vendor pluginlib geometry_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs) ament_export_include_directories(include) diff --git a/flatland_server/package.xml b/flatland_server/package.xml index b4f32ccd..7d0078a8 100644 --- a/flatland_server/package.xml +++ b/flatland_server/package.xml @@ -28,10 +28,10 @@ interactive_markers flatland_msgs lua-dev - box2d-dev libboost-date-time-dev libboost-filesystem-dev libboost-dev + flatland_box2d ament_cmake_gtest ament_lint_auto diff --git a/flatland_server/src/flatland_server_node.cpp b/flatland_server/src/flatland_server_node.cpp index bb274f88..51c16f2e 100644 --- a/flatland_server/src/flatland_server_node.cpp +++ b/flatland_server/src/flatland_server_node.cpp @@ -76,11 +76,11 @@ class FlatlandServerNode : public rclcpp::Node FlatlandServerNode() : Node("flatland_server") { - declare_parameter("world_path"); - declare_parameter("update_rate"); - declare_parameter("step_size"); - declare_parameter("show_viz"); - declare_parameter("viz_pub_rate"); + declare_parameter("world_path"); + declare_parameter("update_rate"); + declare_parameter("step_size"); + declare_parameter("show_viz"); + declare_parameter("viz_pub_rate"); // Load parameters if (!get_parameter("world_path", world_path_)) { @@ -88,10 +88,10 @@ class FlatlandServerNode : public rclcpp::Node rclcpp::shutdown(); return; } - get_parameter_or("update_rate", update_rate_, 200.0f); - get_parameter_or("step_size", step_size_, 1.0f/200.0f); - get_parameter_or("show_viz", show_viz_, 0.0f); - get_parameter_or("viz_pub_rate", viz_pub_rate_, 30.0f); + get_parameter_or("update_rate", update_rate_, 200.0f); + get_parameter_or("step_size", step_size_, 1.0f/200.0f); + get_parameter_or("show_viz", show_viz_, false); + get_parameter_or("viz_pub_rate", viz_pub_rate_, 30.0f); } void Run() { @@ -112,7 +112,7 @@ class FlatlandServerNode : public rclcpp::Node std::string world_path_; // The file path to the world.yaml file float update_rate_; // The physics update rate (Hz) float step_size_; - float show_viz_; + bool show_viz_; float viz_pub_rate_; std::shared_ptr simulation_manager_; }; diff --git a/flatland_server/src/layer.cpp b/flatland_server/src/layer.cpp index f759c070..3abac480 100644 --- a/flatland_server/src/layer.cpp +++ b/flatland_server/src/layer.cpp @@ -162,7 +162,7 @@ Layer *Layer::MakeLayer(std::shared_ptr node, b2World *physics_wor RCLCPP_INFO(rclcpp::get_logger("Layer"), "layer \"%s\" loading image from path=\"%s\"", names[0].c_str(), image_path.string().c_str()); - cv::Mat map = cv::imread(image_path.string(), CV_LOAD_IMAGE_GRAYSCALE); + cv::Mat map = cv::imread(image_path.string(), cv::IMREAD_GRAYSCALE); if (map.empty()) { throw YAMLException("Failed to load " + Q(image_path.string()) + " in layer " + Q(names[0])); diff --git a/flatland_server/thirdparty/Box2D/Box2DConfig.cmake.in b/flatland_server/thirdparty/Box2D/Box2DConfig.cmake.in deleted file mode 100644 index 55d62f6c..00000000 --- a/flatland_server/thirdparty/Box2D/Box2DConfig.cmake.in +++ /dev/null @@ -1,31 +0,0 @@ -# -*- cmake -*- -# -# Box2dConfig.cmake(.in) -# - -# Use the following variables to compile and link against Box2d: -# BOX2D_FOUND - True if Box2d was found on your system -# BOX2D_USE_FILE - The file making Box2d usable -# BOX2D_DEFINITIONS - Definitions needed to build with Box2d -# BOX2D_INCLUDE_DIR - Box2d headers location -# BOX2D_INCLUDE_DIRS - List of directories where Box2d header file are -# BOX2D_LIBRARY - Library name -# BOX2D_LIBRARIES - List of libraries to link against -# BOX2D_LIBRARY_DIRS - List of directories containing Box2d libraries -# BOX2D_ROOT_DIR - The base directory of Box2d -# BOX2D_VERSION_STRING - A human-readable string containing the version - -set ( BOX2D_FOUND 1 ) -set ( BOX2D_USE_FILE "@BOX2D_USE_FILE@" ) - -set ( BOX2D_DEFINITIONS "@BOX2D_DEFINITIONS@" ) -set ( BOX2D_INCLUDE_DIR "@BOX2D_INCLUDE_DIR@" ) -set ( Box2D_INCLUDE_DIRS "@BOX2D_INCLUDE_DIRS@" ) # deprecated -set ( BOX2D_INCLUDE_DIRS "@BOX2D_INCLUDE_DIRS@" ) -set ( BOX2D_LIBRARY "@BOX2D_LIBRARY@" ) -set ( BOX2D_LIBRARIES "@BOX2D_LIBRARIES@" ) -set ( BOX2D_LIBRARY_DIRS "@BOX2D_LIBRARY_DIRS@" ) -set ( BOX2D_ROOT_DIR "@CMAKE_INSTALL_PREFIX@" ) - -set ( BOX2D_VERSION_STRING "@BOX2D_VERSION@" ) - diff --git a/flatland_server/thirdparty/Box2D/CMakeLists.txt b/flatland_server/thirdparty/Box2D/CMakeLists.txt deleted file mode 100644 index 13a30c1b..00000000 --- a/flatland_server/thirdparty/Box2D/CMakeLists.txt +++ /dev/null @@ -1,163 +0,0 @@ - -set(BOX2D_Collision_SRCS - Collision/b2BroadPhase.cpp - Collision/b2CollideCircle.cpp - Collision/b2CollideEdge.cpp - Collision/b2CollidePolygon.cpp - Collision/b2Collision.cpp - Collision/b2Distance.cpp - Collision/b2DynamicTree.cpp - Collision/b2TimeOfImpact.cpp -) -set(BOX2D_Collision_HDRS - Collision/b2BroadPhase.h - Collision/b2Collision.h - Collision/b2Distance.h - Collision/b2DynamicTree.h - Collision/b2TimeOfImpact.h -) -set(BOX2D_Shapes_SRCS - Collision/Shapes/b2CircleShape.cpp - Collision/Shapes/b2EdgeShape.cpp - Collision/Shapes/b2ChainShape.cpp - Collision/Shapes/b2PolygonShape.cpp -) -set(BOX2D_Shapes_HDRS - Collision/Shapes/b2CircleShape.h - Collision/Shapes/b2EdgeShape.h - Collision/Shapes/b2ChainShape.h - Collision/Shapes/b2PolygonShape.h - Collision/Shapes/b2Shape.h -) -set(BOX2D_Common_SRCS - Common/b2BlockAllocator.cpp - Common/b2Draw.cpp - Common/b2Math.cpp - Common/b2Settings.cpp - Common/b2StackAllocator.cpp - Common/b2Timer.cpp -) -set(BOX2D_Common_HDRS - Common/b2BlockAllocator.h - Common/b2Draw.h - Common/b2GrowableStack.h - Common/b2Math.h - Common/b2Settings.h - Common/b2StackAllocator.h - Common/b2Timer.h -) -set(BOX2D_Dynamics_SRCS - Dynamics/b2Body.cpp - Dynamics/b2ContactManager.cpp - Dynamics/b2Fixture.cpp - Dynamics/b2Island.cpp - Dynamics/b2World.cpp - Dynamics/b2WorldCallbacks.cpp -) -set(BOX2D_Dynamics_HDRS - Dynamics/b2Body.h - Dynamics/b2ContactManager.h - Dynamics/b2Fixture.h - Dynamics/b2Island.h - Dynamics/b2TimeStep.h - Dynamics/b2World.h - Dynamics/b2WorldCallbacks.h -) -set(BOX2D_Contacts_SRCS - Dynamics/Contacts/b2CircleContact.cpp - Dynamics/Contacts/b2Contact.cpp - Dynamics/Contacts/b2ContactSolver.cpp - Dynamics/Contacts/b2PolygonAndCircleContact.cpp - Dynamics/Contacts/b2EdgeAndCircleContact.cpp - Dynamics/Contacts/b2EdgeAndPolygonContact.cpp - Dynamics/Contacts/b2ChainAndCircleContact.cpp - Dynamics/Contacts/b2ChainAndPolygonContact.cpp - Dynamics/Contacts/b2PolygonContact.cpp -) -set(BOX2D_Contacts_HDRS - Dynamics/Contacts/b2CircleContact.h - Dynamics/Contacts/b2Contact.h - Dynamics/Contacts/b2ContactSolver.h - Dynamics/Contacts/b2PolygonAndCircleContact.h - Dynamics/Contacts/b2EdgeAndCircleContact.h - Dynamics/Contacts/b2EdgeAndPolygonContact.h - Dynamics/Contacts/b2ChainAndCircleContact.h - Dynamics/Contacts/b2ChainAndPolygonContact.h - Dynamics/Contacts/b2PolygonContact.h -) -set(BOX2D_Joints_SRCS - Dynamics/Joints/b2DistanceJoint.cpp - Dynamics/Joints/b2FrictionJoint.cpp - Dynamics/Joints/b2GearJoint.cpp - Dynamics/Joints/b2Joint.cpp - Dynamics/Joints/b2MotorJoint.cpp - Dynamics/Joints/b2MouseJoint.cpp - Dynamics/Joints/b2PrismaticJoint.cpp - Dynamics/Joints/b2PulleyJoint.cpp - Dynamics/Joints/b2RevoluteJoint.cpp - Dynamics/Joints/b2RopeJoint.cpp - Dynamics/Joints/b2WeldJoint.cpp - Dynamics/Joints/b2WheelJoint.cpp -) -set(BOX2D_Joints_HDRS - Dynamics/Joints/b2DistanceJoint.h - Dynamics/Joints/b2FrictionJoint.h - Dynamics/Joints/b2GearJoint.h - Dynamics/Joints/b2Joint.h - Dynamics/Joints/b2MotorJoint.h - Dynamics/Joints/b2MouseJoint.h - Dynamics/Joints/b2PrismaticJoint.h - Dynamics/Joints/b2PulleyJoint.h - Dynamics/Joints/b2RevoluteJoint.h - Dynamics/Joints/b2RopeJoint.h - Dynamics/Joints/b2WeldJoint.h - Dynamics/Joints/b2WheelJoint.h -) -set(BOX2D_Rope_SRCS - Rope/b2Rope.cpp -) -set(BOX2D_Rope_HDRS - Rope/b2Rope.h -) -set(BOX2D_General_HDRS - Box2D.h -) - -include_directories(../) - -add_library(flatland_Box2D SHARED - ${BOX2D_General_HDRS} - ${BOX2D_Joints_SRCS} - ${BOX2D_Joints_HDRS} - ${BOX2D_Contacts_SRCS} - ${BOX2D_Contacts_HDRS} - ${BOX2D_Dynamics_SRCS} - ${BOX2D_Dynamics_HDRS} - ${BOX2D_Common_SRCS} - ${BOX2D_Common_HDRS} - ${BOX2D_Shapes_SRCS} - ${BOX2D_Shapes_HDRS} - ${BOX2D_Collision_SRCS} - ${BOX2D_Collision_HDRS} - ${BOX2D_Rope_SRCS} - ${BOX2D_Rope_HDRS} -) -set_target_properties(flatland_Box2D PROPERTIES - GIT_COMMIT_HASH f655c603ba9d83f07fc566d38d2654ba35739102 -) - -ament_export_interfaces(export_flatland_Box2D HAS_LIBRARY_TARGET) -install( - TARGETS flatland_Box2D - EXPORT export_flatland_Box2D - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -# Mark cpp header files for installation -install(DIRECTORY ./ - DESTINATION include/Box2D/ - FILES_MATCHING PATTERN "*.h" -) \ No newline at end of file diff --git a/flatland_server/thirdparty/Box2D/UseBox2D.cmake b/flatland_server/thirdparty/Box2D/UseBox2D.cmake deleted file mode 100644 index cb9dc8c0..00000000 --- a/flatland_server/thirdparty/Box2D/UseBox2D.cmake +++ /dev/null @@ -1,9 +0,0 @@ -# -*- cmake -*- -# -# UseBox2d.cmake -# - -add_definitions ( ${BOX2D_DEFINITIONS} ) -include_directories ( ${BOX2D_INCLUDE_DIRS} ) -link_directories ( ${BOX2D_LIBRARY_DIRS} ) - From 5b5132a37212dd4e66e6946beb98516cda4111f6 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 9 Jun 2022 14:57:22 -0400 Subject: [PATCH 07/66] ported flatland server launchfile to ros2 format --- flatland_server/launch/server.launch | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/flatland_server/launch/server.launch b/flatland_server/launch/server.launch index a465b1c7..8995a34b 100644 --- a/flatland_server/launch/server.launch +++ b/flatland_server/launch/server.launch @@ -4,31 +4,29 @@ You can override these default values: roslaunch flatland_Server server.launch world_path:="/some/world.yaml" initial_rate:="30.0" --> - + - - - + - + - - - - - - + + + + + + - - - - + From cbe05ba7068125f4c72c0ea702fd2fee04b3eb7e Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 9 Jun 2022 15:09:40 -0400 Subject: [PATCH 08/66] Fixed minor compile error and warning --- flatland_plugins/plugin_description.xml | 2 +- flatland_server/CMakeLists.txt | 2 +- flatland_server/src/plugin_manager.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/flatland_plugins/plugin_description.xml b/flatland_plugins/plugin_description.xml index 555137d2..4c79e5ab 100644 --- a/flatland_plugins/plugin_description.xml +++ b/flatland_plugins/plugin_description.xml @@ -1,4 +1,4 @@ - + Flatland laser plugin diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 74999407..6f6b1a47 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -200,7 +200,7 @@ pluginlib_export_plugin_description_file(flatland_server plugin_description.xml) install( TARGETS flatland_lib - EXPORT flatland_lib + EXPORT export_flatland_lib LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin diff --git a/flatland_server/src/plugin_manager.cpp b/flatland_server/src/plugin_manager.cpp index b2dd0e8f..e4cb2e42 100644 --- a/flatland_server/src/plugin_manager.cpp +++ b/flatland_server/src/plugin_manager.cpp @@ -60,15 +60,15 @@ PluginManager::PluginManager(rclcpp::Node::SharedPtr node) : node_(node) { "flatland_server", "flatland_server::ModelPlugin"); RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), "Declared Classes:"); for(auto a : model_plugin_loader_->getDeclaredClasses()) { - RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a); + RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a.c_str()); } RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), "Registered Libraries:"); for(auto a : model_plugin_loader_->getRegisteredLibraries()) { - RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a); + RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a.c_str()); } RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), "XML Paths:"); for(auto a : model_plugin_loader_->getPluginXmlPaths()) { - RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a); + RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a.c_str()); } world_plugin_loader_ = new pluginlib::ClassLoader( From 543d83d6a2202219e27772b26e6aee6ee3a1b8c9 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 9 Jun 2022 15:22:25 -0400 Subject: [PATCH 09/66] switched CI to try ros2 foxy build --- .travis.yml | 2 +- scripts/ci_postbuild.sh | 8 ++++---- scripts/ci_prebuild.sh | 8 ++++---- scripts/clang_tidy_ignore.yaml | 2 +- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/.travis.yml b/.travis.yml index 79177a3d..a8796444 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,7 +7,7 @@ env: global: - CCACHE_DIR=$HOME/.ccache matrix: - - ROS_DISTRO="eloquent" #ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu BEFORE_SCRIPT='./scripts/ci_prebuild.sh' AFTER_SCRIPT='./scripts/ci_postbuild.sh' + - ROS_DISTRO="foxy" BEFORE_SCRIPT='./scripts/ci_prebuild.sh' AFTER_SCRIPT='./scripts/ci_postbuild.sh' install: - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci -b master script: diff --git a/scripts/ci_postbuild.sh b/scripts/ci_postbuild.sh index 856ff2c7..7e31e9e8 100755 --- a/scripts/ci_postbuild.sh +++ b/scripts/ci_postbuild.sh @@ -1,19 +1,19 @@ #!/bin/bash -e # cd into catkin workspace -cd /root/catkin_ws +cd $WORKSPACE # run static analyzer catkin clean -y catkin build -DCMAKE_EXPORT_COMPILE_COMMANDS=ON echo "running clang tidy on flatland_server..." -run-clang-tidy-3.8.py -j1 -clang-tidy-binary=clang-tidy-3.8 -p=build/flatland_server | /root/catkin_ws/src/flatland/scripts/parse_clang_tidy.py +run-clang-tidy-3.8.py -j1 -clang-tidy-binary=clang-tidy-3.8 -p=build/flatland_server | $WORKSPACE/src/flatland/scripts/parse_clang_tidy.py echo "running clang tidy on flatland_plugins..." -run-clang-tidy-3.8.py -j1 -clang-tidy-binary=clang-tidy-3.8 -p=build/flatland_plugins | /root/catkin_ws/src/flatland/scripts/parse_clang_tidy.py +run-clang-tidy-3.8.py -j1 -clang-tidy-binary=clang-tidy-3.8 -p=build/flatland_plugins | $WORKSPACE/src/flatland/scripts/parse_clang_tidy.py echo "running clang tidy on flatland_viz..." -run-clang-tidy-3.8.py -j1 -clang-tidy-binary=clang-tidy-3.8 -p=build/flatland_viz | /root/catkin_ws/src/flatland/scripts/parse_clang_tidy.py +run-clang-tidy-3.8.py -j1 -clang-tidy-binary=clang-tidy-3.8 -p=build/flatland_viz | $WORKSPACE/src/flatland/scripts/parse_clang_tidy.py echo "ci_postbuild.sh completed." diff --git a/scripts/ci_prebuild.sh b/scripts/ci_prebuild.sh index 6dc520cf..5cd14c9b 100755 --- a/scripts/ci_prebuild.sh +++ b/scripts/ci_prebuild.sh @@ -16,25 +16,25 @@ cd $DIR/../ # check files are correctly formatted echo "running clang format..." -CLANG_CHANGES_CNT=$(git ls-files | grep -E '\.[ch](pp)?$' | grep -v "thirdparty/" | xargs clang-format-3.8 --style=file -output-replacements-xml | grep -c " corresponds to one file ^^^^\n" print_er "Clang Format Error!" echo 'File not formatted correctly, please execute the command below in flatland repo to see what needs to be changed' - echo 'git ls-files | grep -E "\.[ch](pp)?$" | grep -v "thirdparty/" | xargs clang-format-3.8 --style=file -i && git diff --exit-code' + echo 'git ls-files | grep -E "\.[ch](pp)?$" | grep -v "thirdparty/" | grep -v "flatland_box2d/" | xargs clang-format-3.8 --style=file -i && git diff --exit-code' exit 1; fi diff --git a/scripts/clang_tidy_ignore.yaml b/scripts/clang_tidy_ignore.yaml index 9c819842..a561a74c 100644 --- a/scripts/clang_tidy_ignore.yaml +++ b/scripts/clang_tidy_ignore.yaml @@ -1,4 +1,4 @@ -- ".*/flatland/flatland_server/thirdparty/Box2D/.*" +- ".*/flatland/flatland_box2d/.*" - ".*/flatland/flatland_server/.*warning: 'log_deprecated' is deprecated \\[clang-diagnostic-deprecated-declarations\\].*" - ".*/flatland/flatland_plugins/.*warning: 'log_deprecated' is deprecated \\[clang-diagnostic-deprecated-declarations\\].*" - ".*QtCore/qobject.h:235:16.*warning: Potential memory leak.*\\[clang-analyzer-cplusplus.NewDeleteLeaks\\].*" From b37b2f013bd13f6010fa130df7b0c3cb3bdc15b0 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 9 Jun 2022 15:35:29 -0400 Subject: [PATCH 10/66] brought CI inline with master branch --- .travis.yml | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index a8796444..b4e813a1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,11 +1,10 @@ -language: - - cpp - - python +language: generic services: - docker +cache: + directories: + - $HOME/.ccache env: - global: - - CCACHE_DIR=$HOME/.ccache matrix: - ROS_DISTRO="foxy" BEFORE_SCRIPT='./scripts/ci_prebuild.sh' AFTER_SCRIPT='./scripts/ci_postbuild.sh' install: From ad816a9c5a07e52da96003105049b02c15ccec0c Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 9 Jun 2022 15:43:01 -0400 Subject: [PATCH 11/66] added github action --- .github/workflows/industrial_ci_action.yml | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 .github/workflows/industrial_ci_action.yml diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml new file mode 100644 index 00000000..ed878339 --- /dev/null +++ b/.github/workflows/industrial_ci_action.yml @@ -0,0 +1,15 @@ +name: CI + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: foxy, ROS_REPO: main} + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v1 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} \ No newline at end of file From c6adc83b1dee57e1ba1750359f763ec690540df2 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Thu, 9 Jun 2022 15:55:45 -0400 Subject: [PATCH 12/66] fixed ros2 distro name --- .github/workflows/industrial_ci_action.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index ed878339..f42e77d5 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -7,7 +7,7 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: foxy, ROS_REPO: main} + - {ROS_DISTRO: humble, ROS_REPO: main} runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v1 From c760ca2c895ff8d8f6a42f5c27bdd2392abea3ab Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Sat, 19 Nov 2022 15:20:03 +0000 Subject: [PATCH 13/66] Translate flatland_server server launch file to python format. --- flatland_server/launch/server.launch.py | 54 +++++++++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 flatland_server/launch/server.launch.py diff --git a/flatland_server/launch/server.launch.py b/flatland_server/launch/server.launch.py new file mode 100644 index 00000000..786061e3 --- /dev/null +++ b/flatland_server/launch/server.launch.py @@ -0,0 +1,54 @@ +# Helped by: https://github.com/aws-robotics/ros2-launch-file-migrator +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +import launch.conditions as conditions +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + + +def generate_launch_description(): + ld = LaunchDescription( + [ + DeclareLaunchArgument( + name="world_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("flatland_server"), + "test/conestogo_office_test/world.yaml", + ] + ), + ), + DeclareLaunchArgument(name="update_rate", default_value="200.0"), + DeclareLaunchArgument(name="step_size", default_value="0.005"), + DeclareLaunchArgument(name="show_viz", default_value="false"), + DeclareLaunchArgument(name="viz_pub_rate", default_value="30.0"), + DeclareLaunchArgument(name="use_rviz", default_value="false"), + # Node( + # package="flatland_viz", + # executable="flatland_viz", + # name="flatland_viz", + # output="screen", + # condition=conditions.IfCondition("$(var show_viz)"), + # ), + Node( + package="flatland_server", + name="flatland_server", + executable="flatland_server", + output="screen", + parameters=[ + {"world_path": LaunchConfiguration("world_path")}, + {"update_rate": LaunchConfiguration("update_rate")}, + {"step_size": LaunchConfiguration("step_size")}, + {"show_viz": LaunchConfiguration("show_viz")}, + {"viz_pub_rate": LaunchConfiguration("viz_pub_rate")}, + {"use_sim_time": True}, + ], + ), + ] + ) + return ld + + +if __name__ == "__main__": + generate_launch_description() From cd22a3101781eed8766d2d91a296dcc60fb37fc9 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 15:38:45 +0000 Subject: [PATCH 14/66] Fix comparison of integers of different sizes. --- flatland_server/test/collision_filter_registry_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flatland_server/test/collision_filter_registry_test.cpp b/flatland_server/test/collision_filter_registry_test.cpp index 31c19e17..7135e01c 100644 --- a/flatland_server/test/collision_filter_registry_test.cpp +++ b/flatland_server/test/collision_filter_registry_test.cpp @@ -63,7 +63,7 @@ TEST_F(CollisionFilterRegistryTest, empty_test) { EXPECT_EQ(cfr.LookUpLayerId("random_layer"), CFR::LAYER_NOT_EXIST); std::vector layer_names = cfr.GetAllLayers(); - EXPECT_EQ(layer_names.size(), 0); + EXPECT_EQ(layer_names.size(), 0UL); } TEST_F(CollisionFilterRegistryTest, register_collide_test) { From 8c9cef1940105070e7f6db05acdf261da2e7f7e7 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 15:48:09 +0000 Subject: [PATCH 15/66] Fix comparison of integers of different sizes. --- flatland_server/test/collision_filter_registry_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flatland_server/test/collision_filter_registry_test.cpp b/flatland_server/test/collision_filter_registry_test.cpp index 7135e01c..f5e07225 100644 --- a/flatland_server/test/collision_filter_registry_test.cpp +++ b/flatland_server/test/collision_filter_registry_test.cpp @@ -98,7 +98,7 @@ TEST_F(CollisionFilterRegistryTest, register_layers_test) { EXPECT_EQ(cfr.LookUpLayerId("layer5"), 4); std::vector layer_names = cfr.GetAllLayers(); - EXPECT_EQ(layer_names.size(), 5); + EXPECT_EQ(layer_names.size(), 5UL); EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer1") != layer_names.end()); From e1245ee1191ecc52e29261dcba39b062c0ef9537 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 18:20:06 +0000 Subject: [PATCH 16/66] Fix yaml_preprocessor test. Fix usage of get_parameter in YamlPreprocessor. Change integer in yaml out-file to floating point number (result of a calculation in lua). --- flatland_server/CMakeLists.txt | 9 ++---- flatland_server/src/yaml_preprocessor.cpp | 31 +++++++++++-------- .../yaml/eval.strings.out.yaml | 2 +- .../yaml_preprocessor_test.cpp | 17 ++++++++-- 4 files changed, 35 insertions(+), 24 deletions(-) diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 6f6b1a47..03a0d773 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -154,6 +154,8 @@ if(BUILD_TESTING) ament_add_gtest(plugin_manager_test test/plugin_manager_test.cpp) target_link_libraries(plugin_manager_test flatland_lib) + ament_add_gtest(yaml_preprocessor_test test/yaml_preprocessor/yaml_preprocessor_test.cpp) + target_link_libraries(yaml_preprocessor_test flatland_lib yaml-cpp) if(FALSE) # fake a block comment @@ -182,13 +184,6 @@ if(FALSE) # fake a block comment target_link_libraries(dummy_world_plugin_test flatland_lib yaml-cpp) - add_rostest_gtest(yaml_preprocessor_test - test/yaml_preprocessor/yaml_preprocessor_test.test - test/yaml_preprocessor/yaml_preprocessor_test.cpp) - target_link_libraries(yaml_preprocessor_test - flatland_lib yaml-cpp) - - endif() endif() diff --git a/flatland_server/src/yaml_preprocessor.cpp b/flatland_server/src/yaml_preprocessor.cpp index 2a07988b..aadcdbae 100644 --- a/flatland_server/src/yaml_preprocessor.cpp +++ b/flatland_server/src/yaml_preprocessor.cpp @@ -84,7 +84,7 @@ void YamlPreprocessor::ProcessNodes(YAML::Node &node) { } void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) { - std::string value = node.as().substr(5); // omit the $parse + std::string value = node.as().substr(5); // omit the $eval boost::algorithm::trim(value); // trim whitespace RCLCPP_INFO_STREAM(rclcpp::get_logger("YAML Preprocessor"), "Attempting to parse lua " << value); @@ -125,8 +125,8 @@ void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "No lua output for " << value); } } - } catch ( - ...) { /* Something went wrong parsing the lua, or gettings its results */ + } catch (...) { + /* Something went wrong parsing the lua, or gettings its results */ RCLCPP_ERROR_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "Lua error in: " << value); } } @@ -180,12 +180,10 @@ int YamlPreprocessor::LuaGetEnv(lua_State *L) { int YamlPreprocessor::LuaGetParam(lua_State *L) { const char *name = lua_tostring(L, 1); - std::string param_s; - double param_d; - bool param_b; + rclcpp::Parameter param; lua_getglobal(L, "class_pointer"); // push class pointer to the stack - // grab the class pointer and cast it so we can use it + // grab the class pointer and cast it, so we can use it YamlPreprocessor *class_pointer = reinterpret_cast(lua_touserdata(L, lua_gettop(L))); lua_pop(L, 1); // pop that class pointer from the stack @@ -206,12 +204,19 @@ int YamlPreprocessor::LuaGetParam(lua_State *L) { RCLCPP_WARN_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "No rosparam found for: " << name); lua_pushnil(L); } else { - if (class_pointer->ros_node_->get_parameter(name, param_d)) { - lua_pushnumber(L, param_d); - } else if (class_pointer->ros_node_->get_parameter(name, param_s)) { - lua_pushstring(L, param_s.c_str()); - } else if (class_pointer->ros_node_->get_parameter(name, param_b)) { - lua_pushstring(L, param_b ? "true" : "false"); + if (!class_pointer->ros_node_->get_parameter(name, param)) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "Couldn't find a param with name " + << name); + lua_pushnil(L); + } + if (param.get_type() == rclcpp::PARAMETER_DOUBLE) { + lua_pushnumber(L, param.as_double()); + } else if (param.get_type() == rclcpp::PARAMETER_INTEGER) { + lua_pushinteger(L, param.as_int()); + } else if (param.get_type() == rclcpp::PARAMETER_STRING) { + lua_pushstring(L, param.as_string().c_str()); + } else if (param.get_type() == rclcpp::PARAMETER_BOOL) { + lua_pushstring(L, param.as_bool() ? "true" : "false"); } else { RCLCPP_WARN_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "Couldn't load int/double/string value at param " << name); diff --git a/flatland_server/test/yaml_preprocessor/yaml/eval.strings.out.yaml b/flatland_server/test/yaml_preprocessor/yaml/eval.strings.out.yaml index 1281c69a..f35e81d9 100644 --- a/flatland_server/test/yaml_preprocessor/yaml/eval.strings.out.yaml +++ b/flatland_server/test/yaml_preprocessor/yaml/eval.strings.out.yaml @@ -27,7 +27,7 @@ testParam: param3: Foo param4: 10.5 param5: "" - param6: 223 + param6: 223.0 param7: BBBC param8: true param9: true \ No newline at end of file diff --git a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp index 8003dba7..7c0af4bc 100644 --- a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp +++ b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp @@ -44,7 +44,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "flatland_server/yaml_preprocessor.h" +#include #include #include #include @@ -85,12 +85,23 @@ void compareNodes(const char *p1, const char *p2, YAML::Node &a, // Test the bodyToMarkers method on a polygon shape TEST(YamlPreprocTest, testEvalStrings) { + // env vars + setenv("VALIDSTRING", "Bar", 1); + setenv("VALIDNUMBER", "123.4", 1); + + std::shared_ptr node = rclcpp::Node::make_shared("test_yaml_preprocessor"); + // ros params + node->declare_parameter("/string", "Foo"); + node->declare_parameter("/int", 7); + node->declare_parameter("/float", 10.5); + + YamlPreprocessor yamlPreprocessor(node); boost::filesystem::path cwd = fs::path(__FILE__).parent_path(); - YAML::Node in = YamlPreprocessor::LoadParse( + YAML::Node in = yamlPreprocessor.LoadParse( (cwd / fs::path("/yaml/eval.strings.yaml")).string()); - YAML::Node out = YamlPreprocessor::LoadParse( + YAML::Node out = yamlPreprocessor.LoadParse( (cwd / fs::path("/yaml/eval.strings.out.yaml")).string()); // check that the two strings match From 1e92c41c9ff117d769fa3e76101259f4b3e4891b Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 18:35:04 +0000 Subject: [PATCH 17/66] Fix comparison of integers of different sizes. --- flatland_server/test/collision_filter_registry_test.cpp | 2 +- flatland_server/test/dummy_world_plugin_test.test | 9 --------- 2 files changed, 1 insertion(+), 10 deletions(-) delete mode 100644 flatland_server/test/dummy_world_plugin_test.test diff --git a/flatland_server/test/collision_filter_registry_test.cpp b/flatland_server/test/collision_filter_registry_test.cpp index f5e07225..5b029ab7 100644 --- a/flatland_server/test/collision_filter_registry_test.cpp +++ b/flatland_server/test/collision_filter_registry_test.cpp @@ -128,7 +128,7 @@ TEST_F(CollisionFilterRegistryTest, register_layers_test) { EXPECT_TRUE(cfr.IsLayersFull()); layer_names = cfr.GetAllLayers(); - EXPECT_EQ(layer_names.size(), 16); + EXPECT_EQ(layer_names.size(), 16UL); } // Run all the tests that were declared with TEST() diff --git a/flatland_server/test/dummy_world_plugin_test.test b/flatland_server/test/dummy_world_plugin_test.test deleted file mode 100644 index 8e15aa12..00000000 --- a/flatland_server/test/dummy_world_plugin_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file From 90945614d8430a1ef2e0107dd899680bdf913ba4 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 18:35:25 +0000 Subject: [PATCH 18/66] Fix dummy world tests. --- flatland_server/CMakeLists.txt | 9 +++------ flatland_server/test/dummy_world_plugin_test.cpp | 9 ++++----- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 03a0d773..5d1de92f 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -157,6 +157,9 @@ if(BUILD_TESTING) ament_add_gtest(yaml_preprocessor_test test/yaml_preprocessor/yaml_preprocessor_test.cpp) target_link_libraries(yaml_preprocessor_test flatland_lib yaml-cpp) + ament_add_gtest(dummy_world_plugin_test test/dummy_world_plugin_test.cpp) + target_link_libraries(dummy_world_plugin_test flatland_lib yaml-cpp) + if(FALSE) # fake a block comment add_rostest_gtest(service_manager_test @@ -177,12 +180,6 @@ if(FALSE) # fake a block comment test/dummy_model_plugin_test.cpp) target_link_libraries(dummy_model_plugin_test flatland_lib) - - add_rostest_gtest(dummy_world_plugin_test - test/dummy_world_plugin_test.test - test/dummy_world_plugin_test.cpp) - target_link_libraries(dummy_world_plugin_test - flatland_lib yaml-cpp) endif() endif() diff --git a/flatland_server/test/dummy_world_plugin_test.cpp b/flatland_server/test/dummy_world_plugin_test.cpp index dfe4b8cd..d07370dc 100644 --- a/flatland_server/test/dummy_world_plugin_test.cpp +++ b/flatland_server/test/dummy_world_plugin_test.cpp @@ -44,7 +44,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include #include #include @@ -56,17 +55,17 @@ using namespace flatland_server; TEST(DummyWorldPluginTest, pluginlib_load_test) { + std::shared_ptr node = rclcpp::Node::make_shared("test_dummy_world"); pluginlib::ClassLoader loader( "flatland_server", "flatland_server::WorldPlugin"); try { std::shared_ptr plugin = - loader.createInstance("flatland_plugins::DummyWorldPlugin"); + loader.createSharedInstance("flatland_plugins::DummyWorldPlugin"); YAML::Node n = YAML::Node(); - YamlReader reader = YamlReader(); - plugin->Initialize(NULL, "DummyWorldPluginName", "DummyWorldPluginType", n, - reader); + YamlReader reader = YamlReader(node); + plugin->Initialize(node, nullptr, "DummyWorldPluginName", "DummyWorldPluginType", n, reader); } catch (pluginlib::PluginlibException& e) { FAIL() << "Failed to load Dummy World Plugin. " << e.what(); } From 01b54c9fd73ffb1938260eda2bc8065c17a793da Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 18:40:54 +0000 Subject: [PATCH 19/66] Fix dummy model plugin tests. --- flatland_server/CMakeLists.txt | 8 +++----- flatland_server/test/dummy_model_plugin_test.cpp | 7 ++++--- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 5d1de92f..0ee03b0f 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -160,6 +160,9 @@ if(BUILD_TESTING) ament_add_gtest(dummy_world_plugin_test test/dummy_world_plugin_test.cpp) target_link_libraries(dummy_world_plugin_test flatland_lib yaml-cpp) + ament_add_gtest(dummy_model_plugin_test test/dummy_model_plugin_test.cpp) + target_link_libraries(dummy_model_plugin_test flatland_lib) + if(FALSE) # fake a block comment add_rostest_gtest(service_manager_test @@ -175,11 +178,6 @@ if(FALSE) # fake a block comment target_link_libraries(debug_visualization_test flatland_lib) - add_rostest_gtest(dummy_model_plugin_test - test/dummy_model_plugin_test.test - test/dummy_model_plugin_test.cpp) - target_link_libraries(dummy_model_plugin_test - flatland_lib) endif() endif() diff --git a/flatland_server/test/dummy_model_plugin_test.cpp b/flatland_server/test/dummy_model_plugin_test.cpp index 7e51e743..b40d916e 100644 --- a/flatland_server/test/dummy_model_plugin_test.cpp +++ b/flatland_server/test/dummy_model_plugin_test.cpp @@ -57,12 +57,13 @@ * discovered */ TEST(DummyModelPluginTest, pluginlib_load_test) { + std::shared_ptr node = rclcpp::Node::make_shared("test_dummy_model"); pluginlib::ClassLoader loader( "flatland_server", "flatland_server::ModelPlugin"); try { std::shared_ptr plugin = - loader.createInstance("flatland_plugins::DummyModelPlugin"); + loader.createSharedInstance("flatland_plugins::DummyModelPlugin"); YAML::Node n = YAML::Node(); n["dummy_param_float"] = 0.123456; @@ -70,9 +71,9 @@ TEST(DummyModelPluginTest, pluginlib_load_test) { n["dummy_param_int"] = 123456; flatland_server::CollisionFilterRegistry cfr; - flatland_server::Model model(nullptr, &cfr, "", ""); + flatland_server::Model model(node, nullptr, &cfr, "", ""); - plugin->Initialize("DummyModelPlugin", "DummyModelPluginTest", &model, n); + plugin->Initialize(node, "DummyModelPlugin", "DummyModelPluginTest", &model, n); } catch (pluginlib::PluginlibException& e) { FAIL() << "Failed to load Dummy Model Plugin. " << e.what(); } From c02d71660f6efbdf73eda93969bdd9864b083670 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 18:41:46 +0000 Subject: [PATCH 20/66] Remove used test launch file. --- flatland_server/test/dummy_model_plugin_test.test | 9 --------- 1 file changed, 9 deletions(-) delete mode 100644 flatland_server/test/dummy_model_plugin_test.test diff --git a/flatland_server/test/dummy_model_plugin_test.test b/flatland_server/test/dummy_model_plugin_test.test deleted file mode 100644 index efad3cd5..00000000 --- a/flatland_server/test/dummy_model_plugin_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file From 16bca860377abd039112427d52de41e55f0aba73 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 19:43:15 +0000 Subject: [PATCH 21/66] Fix service manager test. --- flatland_server/CMakeLists.txt | 9 +- flatland_server/test/service_manager_test.cpp | 170 ++++++++++-------- 2 files changed, 102 insertions(+), 77 deletions(-) diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 0ee03b0f..5d723bbc 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -163,13 +163,10 @@ if(BUILD_TESTING) ament_add_gtest(dummy_model_plugin_test test/dummy_model_plugin_test.cpp) target_link_libraries(dummy_model_plugin_test flatland_lib) -if(FALSE) # fake a block comment + ament_add_gtest(service_manager_test test/service_manager_test.cpp) + target_link_libraries(service_manager_test flatland_lib) - add_rostest_gtest(service_manager_test - test/service_manager_test.test - test/service_manager_test.cpp) - target_link_libraries(service_manager_test - flatland_lib) +if(FALSE) # fake a block comment add_rostest_gtest(debug_visualization_test diff --git a/flatland_server/test/service_manager_test.cpp b/flatland_server/test/service_manager_test.cpp index 97e7a415..782c3c9a 100644 --- a/flatland_server/test/service_manager_test.cpp +++ b/flatland_server/test/service_manager_test.cpp @@ -44,28 +44,34 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include +#include +#include +#include #include #include #include #include #include #include +#include namespace fs = boost::filesystem; using namespace flatland_server; +using namespace std::chrono; class ServiceManagerTest : public ::testing::Test { + public: + explicit ServiceManagerTest(const std::shared_ptr &node) : timekeeper(node), node(node) {} + + ServiceManagerTest() : ServiceManagerTest(rclcpp::Node::make_shared("test_service_manager")) {} + protected: SimulationManager* sim_man; boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; boost::filesystem::path robot_yaml; Timekeeper timekeeper; - rclcpp::Node::SharedPtr node; // todo - ros::ServiceClient client; + rclcpp::Node::SharedPtr node; std::thread simulation_thread; void SetUp() override { @@ -76,12 +82,12 @@ class ServiceManagerTest : public ::testing::Test { void TearDown() override { StopSimulationThread(); - if (sim_man) delete sim_man; + delete sim_man; } void StartSimulationThread() { sim_man = - new SimulationManager(world_yaml.string(), 1000, 1 / 1000.0, false, 0); + new SimulationManager(node, world_yaml.string(), 1000, 1 / 1000.0, false, 0); simulation_thread = std::thread(&ServiceManagerTest::SimulationThread, dynamic_cast(this)); } @@ -104,28 +110,31 @@ TEST_F(ServiceManagerTest, spawn_valid_model) { robot_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/person.model.yaml"); - flatland_msgs::srv::SpawnModel srv; - - srv.request.name = "service_manager_test_robot"; - srv.request.ns = "robot123"; - srv.request.yaml_path = robot_yaml.string(); - srv.request.pose.x = 101.1; - srv.request.pose.y = 102.1; - srv.request.pose.theta = 0.23; + auto request = std::make_shared(); + request->name = "service_manager_test_robot"; + request->ns = "robot123"; + request->yaml_path = robot_yaml.string(); + request->pose.x = 101.1; + request->pose.y = 102.1; + request->pose.theta = 0.23; - client = nh.serviceClient("spawn_model"); + auto client = node->create_client("spawn_model"); // Threading is required since client.call blocks executing until return StartSimulationThread(); - ros::service::waitForService("spawn_model", 1000); - ASSERT_TRUE(client.call(srv)); + ASSERT_TRUE(client->wait_for_service(1s)); - ASSERT_TRUE(srv.response.success); - ASSERT_STREQ("", srv.response.message.c_str()); + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + FAIL(); + } + auto response = result.get(); + ASSERT_TRUE(response->success); + ASSERT_STREQ("", response->message.c_str()); World* w = sim_man->world_; - ASSERT_EQ(5, w->models_.size()); + ASSERT_EQ(5UL, w->models_.size()); EXPECT_STREQ("service_manager_test_robot", w->models_[4]->name_.c_str()); EXPECT_STREQ("robot123", w->models_[4]->namespace_.c_str()); EXPECT_FLOAT_EQ(101.1, @@ -133,7 +142,7 @@ TEST_F(ServiceManagerTest, spawn_valid_model) { EXPECT_FLOAT_EQ(102.1, w->models_[4]->bodies_[0]->physics_body_->GetPosition().y); EXPECT_FLOAT_EQ(0.23, w->models_[4]->bodies_[0]->physics_body_->GetAngle()); - EXPECT_EQ(1, w->models_[4]->bodies_.size()); + EXPECT_EQ(1UL, w->models_[4]->bodies_.size()); } /** @@ -145,30 +154,33 @@ TEST_F(ServiceManagerTest, spawn_invalid_model) { robot_yaml = this_file_dir / fs::path("random_path/turtlebot.model.yaml"); - flatland_msgs::srv::SpawnModel srv; - - srv.request.name = "service_manager_test_robot"; - srv.request.yaml_path = robot_yaml.string(); - srv.request.pose.x = 1; - srv.request.pose.y = 2; - srv.request.pose.theta = 3; + auto request = std::make_shared(); + request->name = "service_manager_test_robot"; + request->yaml_path = robot_yaml.string(); + request->pose.x = 1; + request->pose.y = 2; + request->pose.theta = 3; - client = nh.serviceClient("spawn_model"); + auto client = node->create_client("spawn_model"); StartSimulationThread(); - ros::service::waitForService("spawn_model", 1000); - ASSERT_TRUE(client.call(srv)); + ASSERT_TRUE(client->wait_for_service(1s)); - ASSERT_FALSE(srv.response.success); + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + FAIL(); + } + auto response = result.get(); + ASSERT_FALSE(response->success); std::cmatch match; std::string regex_str = "Flatland YAML: File does not exist, " "path=\".*/random_path/turtlebot.model.yaml\".*"; std::regex regex(regex_str); - EXPECT_TRUE(std::regex_match(srv.response.message.c_str(), match, regex)) - << "Error Message '" + srv.response.message + "'" + + EXPECT_TRUE(std::regex_match(response->message.c_str(), match, regex)) + << "Error Message '" + response->message + "'" + " did not match against regex '" + regex_str + "'"; } @@ -179,20 +191,24 @@ TEST_F(ServiceManagerTest, move_model) { world_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - flatland_msgs::srv::MoveModel srv; - srv.request.name = "turtlebot1"; - srv.request.pose.x = 5.5; - srv.request.pose.y = 9.9; - srv.request.pose.theta = 0.77; + auto request = std::make_shared(); + request->name = "turtlebot1"; + request->pose.x = 5.5; + request->pose.y = 9.9; + request->pose.theta = 0.77; - client = nh.serviceClient("move_model"); + auto client = node->create_client("move_model"); StartSimulationThread(); - ros::service::waitForService("move_model", 1000); - ASSERT_TRUE(client.call(srv)); + ASSERT_TRUE(client->wait_for_service(1s)); - ASSERT_TRUE(srv.response.success); + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + FAIL(); + } + auto response = result.get(); + ASSERT_TRUE(response->success); World* w = sim_man->world_; EXPECT_NEAR(5.5, w->models_[0]->bodies_[0]->physics_body_->GetPosition().x, @@ -209,24 +225,28 @@ TEST_F(ServiceManagerTest, move_nonexistent_model) { world_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - flatland_msgs::srv::MoveModel srv; - srv.request.name = "not_a_robot"; - srv.request.pose.x = 4; - srv.request.pose.y = 5; - srv.request.pose.theta = 0; + auto request = std::make_shared(); + request->name = "not_a_robot"; + request->pose.x = 4; + request->pose.y = 5; + request->pose.theta = 0; - client = nh.serviceClient("move_model"); + auto client = node->create_client("move_model"); StartSimulationThread(); - ros::service::waitForService("move_model", 1000); - ASSERT_TRUE(client.call(srv)); + ASSERT_TRUE(client->wait_for_service(1s)); - ASSERT_FALSE(srv.response.success); + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + FAIL(); + } + auto response = result.get(); + ASSERT_FALSE(response->success); EXPECT_STREQ( "Flatland World: failed to move model, model with name " "\"not_a_robot\" does not exist", - srv.response.message.c_str()); + response->message.c_str()); } /** @@ -236,24 +256,28 @@ TEST_F(ServiceManagerTest, delete_model) { world_yaml = this_file_dir / fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); - flatland_msgs::srv::DeleteModel srv; - srv.request.name = "turtlebot1"; + auto request = std::make_shared(); + request->name = "turtlebot1"; - client = nh.serviceClient("delete_model"); + auto client = node->create_client("delete_model"); StartSimulationThread(); - ros::service::waitForService("delete_model", 1000); - ASSERT_TRUE(client.call(srv)); + ASSERT_TRUE(client->wait_for_service(1s)); - ASSERT_TRUE(srv.response.success); + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + FAIL(); + } + auto response = result.get(); + ASSERT_TRUE(response->success); World* w = sim_man->world_; // after deleting a mode, there should be one less model, and one less plugin - ASSERT_EQ(w->models_.size(), 0); - ASSERT_EQ(w->plugin_manager_.model_plugins_.size(), 0); - int count = std::count_if(w->models_.begin(), w->models_.end(), + ASSERT_EQ(w->models_.size(), 0UL); + ASSERT_EQ(w->plugin_manager_.model_plugins_.size(), 0UL); + size_t count = std::count_if(w->models_.begin(), w->models_.end(), [](Model* m) { return m->name_ == "turtlebot1"; }); - ASSERT_EQ(count, 0); + ASSERT_EQ(count, 0UL); } /** @@ -263,21 +287,25 @@ TEST_F(ServiceManagerTest, delete_nonexistent_model) { world_yaml = this_file_dir / fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); - flatland_msgs::srv::DeleteModel srv; - srv.request.name = "random_model"; + auto request = std::make_shared(); + request->name = "random_model"; - client = nh.serviceClient("delete_model"); + auto client = node->create_client("delete_model"); StartSimulationThread(); - ros::service::waitForService("delete_model", 1000); - ASSERT_TRUE(client.call(srv)); + ASSERT_TRUE(client->wait_for_service(1s)); - ASSERT_FALSE(srv.response.success); + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + FAIL(); + } + auto response = result.get(); + ASSERT_FALSE(response->success); EXPECT_STREQ( "Flatland World: failed to delete model, model with name " "\"random_model\" does not exist", - srv.response.message.c_str()); + response->message.c_str()); } // Run all the tests that were declared with TEST() From 48292546ca78b49900b4b6b86dea83a7f82b093a Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 19:44:55 +0000 Subject: [PATCH 22/66] Delete unused test launch files. --- flatland_server/test/service_manager_test.test | 9 --------- .../yaml_preprocessor/yaml_preprocessor_test.test | 15 --------------- 2 files changed, 24 deletions(-) delete mode 100644 flatland_server/test/service_manager_test.test delete mode 100644 flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.test diff --git a/flatland_server/test/service_manager_test.test b/flatland_server/test/service_manager_test.test deleted file mode 100644 index 0ce3ec92..00000000 --- a/flatland_server/test/service_manager_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file diff --git a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.test b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.test deleted file mode 100644 index a7e04395..00000000 --- a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - \ No newline at end of file From cca04faecc95bdba1d936f8c707ae44e79655491 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 19:59:58 +0000 Subject: [PATCH 23/66] Change chrono namespace to chrono_literals. --- flatland_server/test/debug_visualization.test | 9 --------- flatland_server/test/service_manager_test.cpp | 2 +- 2 files changed, 1 insertion(+), 10 deletions(-) delete mode 100644 flatland_server/test/debug_visualization.test diff --git a/flatland_server/test/debug_visualization.test b/flatland_server/test/debug_visualization.test deleted file mode 100644 index 4b7f94e6..00000000 --- a/flatland_server/test/debug_visualization.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file diff --git a/flatland_server/test/service_manager_test.cpp b/flatland_server/test/service_manager_test.cpp index 782c3c9a..05f2bd81 100644 --- a/flatland_server/test/service_manager_test.cpp +++ b/flatland_server/test/service_manager_test.cpp @@ -57,7 +57,7 @@ namespace fs = boost::filesystem; using namespace flatland_server; -using namespace std::chrono; +using namespace std::chrono_literals; class ServiceManagerTest : public ::testing::Test { public: From 85d7aa818c820c8d25dfe636d99c06660a8f3683 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 21:35:09 +0000 Subject: [PATCH 24/66] Fix debug visualizaton tests --- flatland_server/CMakeLists.txt | 13 +- .../test/debug_visualization_test.cpp | 141 +++++++++--------- 2 files changed, 76 insertions(+), 78 deletions(-) diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 5d723bbc..51f0c792 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -166,17 +166,8 @@ if(BUILD_TESTING) ament_add_gtest(service_manager_test test/service_manager_test.cpp) target_link_libraries(service_manager_test flatland_lib) -if(FALSE) # fake a block comment - - - add_rostest_gtest(debug_visualization_test - test/debug_visualization.test - test/debug_visualization_test.cpp) - target_link_libraries(debug_visualization_test - flatland_lib) - - -endif() + ament_add_gtest(debug_visualization_test test/debug_visualization_test.cpp) + target_link_libraries(debug_visualization_test flatland_lib) endif() ############# diff --git a/flatland_server/test/debug_visualization_test.cpp b/flatland_server/test/debug_visualization_test.cpp index e416b858..67534f4d 100644 --- a/flatland_server/test/debug_visualization_test.cpp +++ b/flatland_server/test/debug_visualization_test.cpp @@ -49,9 +49,12 @@ #include #include #include -#include +#include +#include #include +using std::placeholders::_1; + // Test the bodyToMarkers method on a polygon shape TEST(DebugVizTest, testBodyToMarkersPolygon) { b2Vec2 gravity(0.0, 0.0); @@ -72,15 +75,16 @@ TEST(DebugVizTest, testBodyToMarkersPolygon) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - flatland_server::DebugVisualization::Get(node_)->BodyToMarkers(markers, body, 1.0, + std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersPolygon"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.5, 0.7); // check that marker was created - ASSERT_EQ(markers.markers.size(), 1); + ASSERT_EQ(markers.markers.size(), 1UL); // Check that ASSERT_EQ(markers.markers[0].header.frame_id, "map"); ASSERT_EQ(markers.markers[0].header.stamp.sec, 0); - ASSERT_EQ(markers.markers[0].header.stamp.nsec, 0); + ASSERT_EQ(markers.markers[0].header.stamp.nanosec, 0UL); // Check color setting ASSERT_NEAR(markers.markers[0].color.r, 1.0, 1e-5); @@ -101,8 +105,8 @@ TEST(DebugVizTest, testBodyToMarkersPolygon) { // Check the marker shape ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_STRIP); - ASSERT_EQ(markers.markers[0].points.size(), - 5); // box as line strip: 0, 1, 2, 3, 0 + // box as line strip: 0, 1, 2, 3, 0 + ASSERT_EQ(markers.markers[0].points.size(), 5UL); ASSERT_NEAR(markers.markers[0].points[0].x, -1.0, 1e-5); ASSERT_NEAR(markers.markers[0].points[0].y, -2.0, 1e-5); ASSERT_NEAR(markers.markers[0].points[1].x, 1.0, 1e-5); @@ -134,10 +138,11 @@ TEST(DebugVizTest, testBodyToMarkersCircle) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - flatland_server::DebugVisualization::Get(node_)->BodyToMarkers(markers, body, 1.0, + std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersCircle"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); // check that marker was created - ASSERT_EQ(markers.markers.size(), 1); + ASSERT_EQ(markers.markers.size(), 1UL); // Check the marker shape ASSERT_EQ(markers.markers[0].type, markers.markers[0].SPHERE_LIST); @@ -162,14 +167,15 @@ TEST(DebugVizTest, testBodyToMarkersEdge) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - flatland_server::DebugVisualization::Get(node_)->BodyToMarkers(markers, body, 1.0, + std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersEdge"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); // check that marker was created - ASSERT_EQ(markers.markers.size(), 1); + ASSERT_EQ(markers.markers.size(), 1UL); // Check the marker shape ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); - ASSERT_EQ(markers.markers[0].points.size(), 2); + ASSERT_EQ(markers.markers[0].points.size(), 2UL); ASSERT_NEAR(markers.markers[0].points[0].x, 0.5, 1e-5); ASSERT_NEAR(markers.markers[0].points[0].y, 1.5, 1e-5); ASSERT_NEAR(markers.markers[0].points[1].x, 3.5, 1e-5); @@ -196,10 +202,11 @@ TEST(DebugVizTest, testBodyToMarkersUnsupported) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - flatland_server::DebugVisualization::Get(node_)->BodyToMarkers(markers, body, 1.0, + std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersUnsupported"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); // check that marker was not created - ASSERT_EQ(markers.markers.size(), 0); + ASSERT_EQ(markers.markers.size(), 0UL); } // test bodyToMarkers with a body with multiple fixtures @@ -225,14 +232,15 @@ TEST(DebugVizTest, testBodyToMarkersMultifixture) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - flatland_server::DebugVisualization::Get(node_)->BodyToMarkers(markers, body, 1.0, + std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersMultifixture"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); // check that one marker was created - ASSERT_EQ(markers.markers.size(), 1); + ASSERT_EQ(markers.markers.size(), 1UL); // Check the 1st marker ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); - ASSERT_EQ(markers.markers[0].points.size(), 4); + ASSERT_EQ(markers.markers[0].points.size(), 4UL); ASSERT_NEAR(markers.markers[0].points[0].x, 0.0, 1e-5); ASSERT_NEAR(markers.markers[0].points[0].y, 1.0, 1e-5); ASSERT_NEAR(markers.markers[0].points[1].x, 1.0, 1e-5); @@ -268,16 +276,17 @@ TEST(DebugVizTest, testBodyToMarkersMultibody) { body2->CreateFixture(&fixtureDef2); visualization_msgs::msg::MarkerArray markers; - flatland_server::DebugVisualization::Get(node_)->BodyToMarkers(markers, body, 1.0, + std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersMultibody"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get(node_)->BodyToMarkers(markers, body2, 1.0, + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body2, 1.0, 0.0, 0.0, 1.0); // check that marker was created - ASSERT_EQ(markers.markers.size(), 1); + ASSERT_EQ(markers.markers.size(), 1UL); // Check the 1st marker ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); - ASSERT_EQ(markers.markers[0].points.size(), 4); + ASSERT_EQ(markers.markers[0].points.size(), 4UL); ASSERT_NEAR(markers.markers[0].points[0].x, 0.0, 1e-5); ASSERT_NEAR(markers.markers[0].points[0].y, 1.0, 1e-5); ASSERT_NEAR(markers.markers[0].points[1].x, 1.0, 1e-5); @@ -313,16 +322,17 @@ TEST(DebugVizTest, testJointToMarkersMultiJoint) { b2Joint* j2 = world.CreateJoint(&jd2); visualization_msgs::msg::MarkerArray markers; - flatland_server::DebugVisualization::Get(node_)->JointToMarkers(markers, j1, 0.1, + std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_JointToMarkersMultiJoint"); + flatland_server::DebugVisualization::Get(node)->JointToMarkers(markers, j1, 0.1, 0.2, 0.3, 0.4); - flatland_server::DebugVisualization::Get(node_)->JointToMarkers(markers, j2, 0.5, + flatland_server::DebugVisualization::Get(node)->JointToMarkers(markers, j2, 0.5, 0.6, 0.7, 0.8); // check that marker was created - ASSERT_EQ(markers.markers.size(), 4); + ASSERT_EQ(markers.markers.size(), 4UL); // Check the 1st marker ASSERT_EQ(markers.markers[0].type, markers.markers[0].LINE_LIST); - ASSERT_EQ(markers.markers[0].points.size(), 6); + ASSERT_EQ(markers.markers[0].points.size(), 6UL); for (unsigned int i = 0; i < 6; i++) { ASSERT_FLOAT_EQ(markers.markers[0].points[i].x, 0.0) << "index: " << i; @@ -336,7 +346,7 @@ TEST(DebugVizTest, testJointToMarkersMultiJoint) { // Check the 2nd marker ASSERT_EQ(markers.markers[1].type, markers.markers[1].CUBE_LIST); - ASSERT_EQ(markers.markers[1].points.size(), 4); + ASSERT_EQ(markers.markers[1].points.size(), 4UL); for (unsigned int i = 0; i < 4; i++) { ASSERT_FLOAT_EQ(markers.markers[1].points[i].x, 0.0) << "index: " << i; @@ -345,7 +355,7 @@ TEST(DebugVizTest, testJointToMarkersMultiJoint) { // Check the 3rd marker ASSERT_EQ(markers.markers[2].type, markers.markers[3].LINE_LIST); - ASSERT_EQ(markers.markers[2].points.size(), 6); + ASSERT_EQ(markers.markers[2].points.size(), 6UL); ASSERT_FLOAT_EQ(markers.markers[2].points[0].x, 0.0); ASSERT_FLOAT_EQ(markers.markers[2].points[0].y, 0.0); ASSERT_FLOAT_EQ(markers.markers[2].points[1].x, 1.0); @@ -363,7 +373,7 @@ TEST(DebugVizTest, testJointToMarkersMultiJoint) { // Check the 4th marker ASSERT_EQ(markers.markers[3].type, markers.markers[3].CUBE_LIST); - ASSERT_EQ(markers.markers[3].points.size(), 4); + ASSERT_EQ(markers.markers[3].points.size(), 4UL); ASSERT_FLOAT_EQ(markers.markers[3].points[0].x, 1.0); ASSERT_FLOAT_EQ(markers.markers[3].points[0].y, 2.0); ASSERT_FLOAT_EQ(markers.markers[3].points[1].x, 3.0); @@ -376,19 +386,21 @@ TEST(DebugVizTest, testJointToMarkersMultiJoint) { // A helper class to accept MarkerArray message callbacks struct MarkerArraySubscriptionHelper { + std::shared_ptr node_; visualization_msgs::msg::MarkerArray markers_; int count_; - MarkerArraySubscriptionHelper() : count_(0) {} + explicit MarkerArraySubscriptionHelper(std::shared_ptr node) : node_(std::move(node)), count_(0) {} /** * @brief callback that stores the last message and total message count * @param msg The input message pointer */ - void callback(const visualization_msgs::msg::MarkerArrayConstPtr& msg) { + void callback(const visualization_msgs::msg::MarkerArray& msg) { + //void callback(const visualization_msgs::msg::MarkerArrayConstPtr& msg) { ++count_; RCLCPP_INFO(rclcpp::get_logger("Debug Visualization Test"), "GOT ONE"); - markers_ = visualization_msgs::msg::MarkerArray(*msg); // Copy the message + markers_ = visualization_msgs::msg::MarkerArray(msg); // Copy the message } /** @@ -398,10 +410,10 @@ struct MarkerArraySubscriptionHelper { * * @return true if successful */ - bool waitForMessageCount(int count) { - ros::Rate rate(10); // throttle check to 10Hz + bool waitForMessageCount(int count) const { + rclcpp::Rate rate(10); // throttle check to 10Hz for (unsigned int i = 0; i < 20; i++) { - ros::spinOnce(); + rclcpp::spin_some(node_); if (count_ >= count) return true; rate.sleep(); } @@ -411,7 +423,8 @@ struct MarkerArraySubscriptionHelper { // Test the bodyToMarkers method on an unsupported shape TEST(DebugVizTest, testPublishMarkers) { - flatland_server::Timekeeper timekeeper; + std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_PublishMarkers"); + flatland_server::Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.01); b2Vec2 gravity(0.0, 0.0); @@ -436,70 +449,64 @@ TEST(DebugVizTest, testPublishMarkers) { b2Joint* joint = world.CreateJoint(&joint_def); // Set up helper class subscribing to rostopic - rclcpp::Node::SharedPtr node_; // TODO - MarkerArraySubscriptionHelper helper; - ros::Subscriber sub = - nh.subscribe("/debug_visualization_test/debug/example", 0, - &MarkerArraySubscriptionHelper::callback, &helper); + MarkerArraySubscriptionHelper helper(node); + std::string topicName = "example"; + auto sub = + node->create_subscription( + topicName, 0, + std::bind(&MarkerArraySubscriptionHelper::callback, &helper, _1)); - flatland_server::DebugVisualization::Get(node_)->Visualize("example", body, 1.0, - 0.0, 0.0, 1.0); + auto debugVis = flatland_server::DebugVisualization::Get(node); + debugVis->Visualize(topicName, body, 1.0, 0.0, 0.0, 1.0); // Check pre publish conditions - EXPECT_EQ(flatland_server::DebugVisualization::Get(node_)->topics_.size(), 1); - ros::spinOnce(); + EXPECT_EQ(debugVis->topics_.size(), 1UL); + rclcpp::spin_some(node); EXPECT_EQ(helper.count_, 0); - EXPECT_EQ(flatland_server::DebugVisualization::Get(node_) - .topics_["example"] - .needs_publishing, - true); + EXPECT_EQ(debugVis->topics_["example"].needs_publishing, true); // Check that there is a publisher - EXPECT_EQ(sub.getNumPublishers(), 1); + EXPECT_EQ(sub->get_publisher_count(), 1UL); // Publish - flatland_server::DebugVisualization::Get(node_)->Publish(timekeeper); + debugVis->Publish(timekeeper); // Verify that message was published EXPECT_TRUE(helper.waitForMessageCount(1)); - EXPECT_EQ(helper.markers_.markers.size(), 1); + EXPECT_EQ(helper.markers_.markers.size(), 1UL); - // Publish again (should have no change- nothing needs publishing) - flatland_server::DebugVisualization::Get(node_)->Publish(timekeeper); + // Publish again (should have no change - nothing needs publishing) + debugVis->Publish(timekeeper); // Verify that message was published EXPECT_TRUE(helper.waitForMessageCount(1)); - EXPECT_EQ(1, helper.markers_.markers.size()); + EXPECT_EQ(1UL, helper.markers_.markers.size()); // Publish some more markers - flatland_server::DebugVisualization::Get(node_)->Visualize("example", body, 1.0, - 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get(node_)->Visualize("example", body, 1.0, - 0.0, 0.0, 1.0); + debugVis->Visualize("example", body, 1.0, 0.0, 0.0, 1.0); + debugVis->Visualize("example", body, 1.0, 0.0, 0.0, 1.0); // inserts two markers - flatland_server::DebugVisualization::Get(node_)->Visualize("example", joint, 1.0, - 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get(node_)->Publish(timekeeper); + debugVis->Visualize("example", joint, 1.0, 0.0, 0.0, 1.0); + debugVis->Publish(timekeeper); // Verify that message was published EXPECT_TRUE(helper.waitForMessageCount(2)); // Published twice - EXPECT_EQ(5, helper.markers_.markers.size()); // 5 markers in latest msg + EXPECT_EQ(5UL, helper.markers_.markers.size()); // 5 markers in latest msg // Reset marker list, this empties the markers array, and topics having // empty markers are automatically deleted - flatland_server::DebugVisualization::Get(node_)->Reset("example"); - flatland_server::DebugVisualization::Get(node_)->Publish(timekeeper); + debugVis->Reset("example"); + debugVis->Publish(timekeeper); // Verify that message was published EXPECT_TRUE(helper.waitForMessageCount(2)); // Published two times // publish again with some contents, and the topic is created again - flatland_server::DebugVisualization::Get(node_)->Visualize("example", joint, 1.0, - 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get(node_)->Publish(timekeeper); + debugVis->Visualize("example", joint, 1.0, 0.0, 0.0, 1.0); + debugVis->Publish(timekeeper); EXPECT_TRUE(helper.waitForMessageCount(3)); - EXPECT_EQ(2, helper.markers_.markers.size()); + EXPECT_EQ(2UL, helper.markers_.markers.size()); } // Run all the tests that were declared with TEST() From 9680a94419a01c8a5a27b5e15a0f5a930b7ebcf3 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 21:45:23 +0000 Subject: [PATCH 25/66] Format test files --- .../test/collision_filter_registry_test.cpp | 1 + .../test/debug_visualization_test.cpp | 78 +++++++++++-------- .../test/dummy_model_plugin_test.cpp | 9 ++- .../test/dummy_world_plugin_test.cpp | 9 ++- flatland_server/test/geometry_test.cpp | 2 + flatland_server/test/load_world_test.cpp | 12 ++- flatland_server/test/model_test.cpp | 9 ++- flatland_server/test/plugin_manager_test.cpp | 18 +++-- flatland_server/test/service_manager_test.cpp | 60 ++++++++------ .../yaml_preprocessor_test.cpp | 6 +- 10 files changed, 126 insertions(+), 78 deletions(-) diff --git a/flatland_server/test/collision_filter_registry_test.cpp b/flatland_server/test/collision_filter_registry_test.cpp index 5b029ab7..ded183f7 100644 --- a/flatland_server/test/collision_filter_registry_test.cpp +++ b/flatland_server/test/collision_filter_registry_test.cpp @@ -46,6 +46,7 @@ #include #include + #include #include diff --git a/flatland_server/test/debug_visualization_test.cpp b/flatland_server/test/debug_visualization_test.cpp index 67534f4d..07837b87 100644 --- a/flatland_server/test/debug_visualization_test.cpp +++ b/flatland_server/test/debug_visualization_test.cpp @@ -45,13 +45,15 @@ */ #include "flatland_server/debug_visualization.h" + #include #include #include + +#include #include #include #include -#include using std::placeholders::_1; @@ -75,9 +77,10 @@ TEST(DebugVizTest, testBodyToMarkersPolygon) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersPolygon"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, - 0.0, 0.5, 0.7); + std::shared_ptr node = rclcpp::Node::make_shared( + "test_debug_visualization_BodyToMarkersPolygon"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers( + markers, body, 1.0, 0.0, 0.5, 0.7); // check that marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -138,9 +141,10 @@ TEST(DebugVizTest, testBodyToMarkersCircle) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersCircle"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); + std::shared_ptr node = + rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersCircle"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers( + markers, body, 1.0, 0.0, 0.0, 1.0); // check that marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -167,9 +171,10 @@ TEST(DebugVizTest, testBodyToMarkersEdge) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersEdge"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); + std::shared_ptr node = + rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersEdge"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers( + markers, body, 1.0, 0.0, 0.0, 1.0); // check that marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -202,9 +207,10 @@ TEST(DebugVizTest, testBodyToMarkersUnsupported) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersUnsupported"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); + std::shared_ptr node = rclcpp::Node::make_shared( + "test_debug_visualization_BodyToMarkersUnsupported"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers( + markers, body, 1.0, 0.0, 0.0, 1.0); // check that marker was not created ASSERT_EQ(markers.markers.size(), 0UL); } @@ -232,9 +238,10 @@ TEST(DebugVizTest, testBodyToMarkersMultifixture) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersMultifixture"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); + std::shared_ptr node = rclcpp::Node::make_shared( + "test_debug_visualization_BodyToMarkersMultifixture"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers( + markers, body, 1.0, 0.0, 0.0, 1.0); // check that one marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -276,11 +283,12 @@ TEST(DebugVizTest, testBodyToMarkersMultibody) { body2->CreateFixture(&fixtureDef2); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersMultibody"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, - 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body2, 1.0, - 0.0, 0.0, 1.0); + std::shared_ptr node = rclcpp::Node::make_shared( + "test_debug_visualization_BodyToMarkersMultibody"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers( + markers, body, 1.0, 0.0, 0.0, 1.0); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers( + markers, body2, 1.0, 0.0, 0.0, 1.0); // check that marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -322,11 +330,12 @@ TEST(DebugVizTest, testJointToMarkersMultiJoint) { b2Joint* j2 = world.CreateJoint(&jd2); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_JointToMarkersMultiJoint"); - flatland_server::DebugVisualization::Get(node)->JointToMarkers(markers, j1, 0.1, - 0.2, 0.3, 0.4); - flatland_server::DebugVisualization::Get(node)->JointToMarkers(markers, j2, 0.5, - 0.6, 0.7, 0.8); + std::shared_ptr node = rclcpp::Node::make_shared( + "test_debug_visualization_JointToMarkersMultiJoint"); + flatland_server::DebugVisualization::Get(node)->JointToMarkers( + markers, j1, 0.1, 0.2, 0.3, 0.4); + flatland_server::DebugVisualization::Get(node)->JointToMarkers( + markers, j2, 0.5, 0.6, 0.7, 0.8); // check that marker was created ASSERT_EQ(markers.markers.size(), 4UL); @@ -390,14 +399,15 @@ struct MarkerArraySubscriptionHelper { visualization_msgs::msg::MarkerArray markers_; int count_; - explicit MarkerArraySubscriptionHelper(std::shared_ptr node) : node_(std::move(node)), count_(0) {} + explicit MarkerArraySubscriptionHelper(std::shared_ptr node) + : node_(std::move(node)), count_(0) {} /** * @brief callback that stores the last message and total message count * @param msg The input message pointer */ void callback(const visualization_msgs::msg::MarkerArray& msg) { - //void callback(const visualization_msgs::msg::MarkerArrayConstPtr& msg) { + // void callback(const visualization_msgs::msg::MarkerArrayConstPtr& msg) { ++count_; RCLCPP_INFO(rclcpp::get_logger("Debug Visualization Test"), "GOT ONE"); markers_ = visualization_msgs::msg::MarkerArray(msg); // Copy the message @@ -423,7 +433,8 @@ struct MarkerArraySubscriptionHelper { // Test the bodyToMarkers method on an unsupported shape TEST(DebugVizTest, testPublishMarkers) { - std::shared_ptr node = rclcpp::Node::make_shared("test_debug_visualization_PublishMarkers"); + std::shared_ptr node = + rclcpp::Node::make_shared("test_debug_visualization_PublishMarkers"); flatland_server::Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.01); @@ -451,10 +462,9 @@ TEST(DebugVizTest, testPublishMarkers) { // Set up helper class subscribing to rostopic MarkerArraySubscriptionHelper helper(node); std::string topicName = "example"; - auto sub = - node->create_subscription( - topicName, 0, - std::bind(&MarkerArraySubscriptionHelper::callback, &helper, _1)); + auto sub = node->create_subscription( + topicName, 0, + std::bind(&MarkerArraySubscriptionHelper::callback, &helper, _1)); auto debugVis = flatland_server::DebugVisualization::Get(node); debugVis->Visualize(topicName, body, 1.0, 0.0, 0.0, 1.0); @@ -490,7 +500,7 @@ TEST(DebugVizTest, testPublishMarkers) { debugVis->Publish(timekeeper); // Verify that message was published - EXPECT_TRUE(helper.waitForMessageCount(2)); // Published twice + EXPECT_TRUE(helper.waitForMessageCount(2)); // Published twice EXPECT_EQ(5UL, helper.markers_.markers.size()); // 5 markers in latest msg // Reset marker list, this empties the markers array, and topics having diff --git a/flatland_server/test/dummy_model_plugin_test.cpp b/flatland_server/test/dummy_model_plugin_test.cpp index b40d916e..c4712723 100644 --- a/flatland_server/test/dummy_model_plugin_test.cpp +++ b/flatland_server/test/dummy_model_plugin_test.cpp @@ -48,16 +48,18 @@ #include #include #include +#include + #include #include -#include /** * Test the pluginlib is configured correctly so that the model can be * discovered */ TEST(DummyModelPluginTest, pluginlib_load_test) { - std::shared_ptr node = rclcpp::Node::make_shared("test_dummy_model"); + std::shared_ptr node = + rclcpp::Node::make_shared("test_dummy_model"); pluginlib::ClassLoader loader( "flatland_server", "flatland_server::ModelPlugin"); @@ -73,7 +75,8 @@ TEST(DummyModelPluginTest, pluginlib_load_test) { flatland_server::CollisionFilterRegistry cfr; flatland_server::Model model(node, nullptr, &cfr, "", ""); - plugin->Initialize(node, "DummyModelPlugin", "DummyModelPluginTest", &model, n); + plugin->Initialize(node, "DummyModelPlugin", "DummyModelPluginTest", &model, + n); } catch (pluginlib::PluginlibException& e) { FAIL() << "Failed to load Dummy Model Plugin. " << e.what(); } diff --git a/flatland_server/test/dummy_world_plugin_test.cpp b/flatland_server/test/dummy_world_plugin_test.cpp index d07370dc..df72f1c4 100644 --- a/flatland_server/test/dummy_world_plugin_test.cpp +++ b/flatland_server/test/dummy_world_plugin_test.cpp @@ -48,14 +48,16 @@ #include #include #include +#include + #include #include -#include using namespace flatland_server; TEST(DummyWorldPluginTest, pluginlib_load_test) { - std::shared_ptr node = rclcpp::Node::make_shared("test_dummy_world"); + std::shared_ptr node = + rclcpp::Node::make_shared("test_dummy_world"); pluginlib::ClassLoader loader( "flatland_server", "flatland_server::WorldPlugin"); @@ -65,7 +67,8 @@ TEST(DummyWorldPluginTest, pluginlib_load_test) { YAML::Node n = YAML::Node(); YamlReader reader = YamlReader(node); - plugin->Initialize(node, nullptr, "DummyWorldPluginName", "DummyWorldPluginType", n, reader); + plugin->Initialize(node, nullptr, "DummyWorldPluginName", + "DummyWorldPluginType", n, reader); } catch (pluginlib::PluginlibException& e) { FAIL() << "Failed to load Dummy World Plugin. " << e.what(); } diff --git a/flatland_server/test/geometry_test.cpp b/flatland_server/test/geometry_test.cpp index c101be87..1df2cfb0 100644 --- a/flatland_server/test/geometry_test.cpp +++ b/flatland_server/test/geometry_test.cpp @@ -45,7 +45,9 @@ */ #include "flatland_server/geometry.h" + #include + #include // Test the CreateTransform method diff --git a/flatland_server/test/load_world_test.cpp b/flatland_server/test/load_world_test.cpp index bff24339..0ae1e936 100644 --- a/flatland_server/test/load_world_test.cpp +++ b/flatland_server/test/load_world_test.cpp @@ -51,9 +51,10 @@ #include #include #include -#include #include + #include +#include #include #include @@ -85,7 +86,8 @@ class LoadWorldTest : public ::testing::Test { std::regex regex(regex_str); try { - std::shared_ptr node = rclcpp::Node::make_shared("test_yaml_fail_node"); + std::shared_ptr node = + rclcpp::Node::make_shared("test_yaml_fail_node"); w = World::MakeWorld(node, world_yaml.string()); ADD_FAILURE() << "Expected an exception, but none were raised"; } catch (const Exception &e) { @@ -381,7 +383,8 @@ class LoadWorldTest : public ::testing::Test { if (j->GetBodyA() != body_A->physics_body_) { printf("BodyA ptr Actual %p != Expected:%p\n", - (void *)joint->physics_joint_->GetBodyA(), (void *)body_A->physics_body_); + (void *)joint->physics_joint_->GetBodyA(), + (void *)body_A->physics_body_); return false; } @@ -516,7 +519,8 @@ class LoadWorldTest : public ::testing::Test { TEST_F(LoadWorldTest, simple_test_A) { world_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - std::shared_ptr node = rclcpp::Node::make_shared("simple_test_A_node"); + std::shared_ptr node = + rclcpp::Node::make_shared("simple_test_A_node"); w = World::MakeWorld(node, world_yaml.string()); EXPECT_EQ(w->physics_velocity_iterations_, 11); diff --git a/flatland_server/test/model_test.cpp b/flatland_server/test/model_test.cpp index fc0396f7..684f131b 100644 --- a/flatland_server/test/model_test.cpp +++ b/flatland_server/test/model_test.cpp @@ -45,14 +45,18 @@ */ #include "flatland_server/model.h" + #include + #include #include + #include "flatland_server/collision_filter_registry.h" // Test the NameSpaceTF method TEST(TestSuite, testNameSpaceTF) { - std::shared_ptr node = rclcpp::Node::make_shared("testNameSpaceTF_node"); + std::shared_ptr node = + rclcpp::Node::make_shared("testNameSpaceTF_node"); flatland_server::Model has_ns(node, nullptr, nullptr, std::string("foo"), std::string("has_ns")); // namespace "foo" onto tf "bar" => foo_bar @@ -70,7 +74,8 @@ TEST(TestSuite, testNameSpaceTF) { // Test the NameSpaceTopic method TEST(TestSuite, testNameSpaceTopic) { - std::shared_ptr node = rclcpp::Node::make_shared("testNameSpaceTopic_node"); + std::shared_ptr node = + rclcpp::Node::make_shared("testNameSpaceTopic_node"); flatland_server::Model has_ns(node, nullptr, nullptr, std::string("foo"), std::string("has_ns")); // namespace "foo" onto tf "bar" => foo_bar diff --git a/flatland_server/test/plugin_manager_test.cpp b/flatland_server/test/plugin_manager_test.cpp index 670b9832..e1d4f686 100644 --- a/flatland_server/test/plugin_manager_test.cpp +++ b/flatland_server/test/plugin_manager_test.cpp @@ -50,8 +50,9 @@ #include #include #include -#include + #include +#include namespace fs = boost::filesystem; using namespace flatland_server; @@ -180,10 +181,11 @@ class PluginManagerTest : public ::testing::Test { TEST_F(PluginManagerTest, collision_test) { world_yaml = this_file_dir / fs::path("plugin_manager_tests/collision_test/world.yaml"); - std::shared_ptr node = rclcpp::Node::make_shared("PluginManagerTest_node"); + std::shared_ptr node = + rclcpp::Node::make_shared("PluginManagerTest_node"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); - + w = World::MakeWorld(node, world_yaml.string()); Layer *l = w->layers_[0]; Model *m0 = w->models_[0]; @@ -306,7 +308,7 @@ TEST_F(PluginManagerTest, load_dummy_test) { fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); ModelPlugin *p = w->plugin_manager_.model_plugins_[0].get(); @@ -321,7 +323,7 @@ TEST_F(PluginManagerTest, plugin_throws_exception) { try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException &e) { // do a regex match against error message @@ -346,7 +348,7 @@ TEST_F(PluginManagerTest, nonexistent_plugin) { try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException &e) { std::cmatch match; @@ -370,7 +372,7 @@ TEST_F(PluginManagerTest, invalid_plugin_yaml) { try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const YAMLException &e) { EXPECT_STREQ( @@ -390,7 +392,7 @@ TEST_F(PluginManagerTest, duplicate_plugin) { try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const YAMLException &e) { EXPECT_STREQ( diff --git a/flatland_server/test/service_manager_test.cpp b/flatland_server/test/service_manager_test.cpp index 05f2bd81..25a0d8cc 100644 --- a/flatland_server/test/service_manager_test.cpp +++ b/flatland_server/test/service_manager_test.cpp @@ -44,16 +44,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include #include #include #include #include + +#include +#include +#include +#include #include #include -#include namespace fs = boost::filesystem; using namespace flatland_server; @@ -61,9 +62,11 @@ using namespace std::chrono_literals; class ServiceManagerTest : public ::testing::Test { public: - explicit ServiceManagerTest(const std::shared_ptr &node) : timekeeper(node), node(node) {} + explicit ServiceManagerTest(const std::shared_ptr& node) + : timekeeper(node), node(node) {} - ServiceManagerTest() : ServiceManagerTest(rclcpp::Node::make_shared("test_service_manager")) {} + ServiceManagerTest() + : ServiceManagerTest(rclcpp::Node::make_shared("test_service_manager")) {} protected: SimulationManager* sim_man; @@ -86,8 +89,8 @@ class ServiceManagerTest : public ::testing::Test { } void StartSimulationThread() { - sim_man = - new SimulationManager(node, world_yaml.string(), 1000, 1 / 1000.0, false, 0); + sim_man = new SimulationManager(node, world_yaml.string(), 1000, 1 / 1000.0, + false, 0); simulation_thread = std::thread(&ServiceManagerTest::SimulationThread, dynamic_cast(this)); } @@ -118,7 +121,8 @@ TEST_F(ServiceManagerTest, spawn_valid_model) { request->pose.y = 102.1; request->pose.theta = 0.23; - auto client = node->create_client("spawn_model"); + auto client = + node->create_client("spawn_model"); // Threading is required since client.call blocks executing until return StartSimulationThread(); @@ -126,7 +130,8 @@ TEST_F(ServiceManagerTest, spawn_valid_model) { ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != + rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); @@ -161,14 +166,16 @@ TEST_F(ServiceManagerTest, spawn_invalid_model) { request->pose.y = 2; request->pose.theta = 3; - auto client = node->create_client("spawn_model"); + auto client = + node->create_client("spawn_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != + rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); @@ -197,14 +204,16 @@ TEST_F(ServiceManagerTest, move_model) { request->pose.y = 9.9; request->pose.theta = 0.77; - auto client = node->create_client("move_model"); + auto client = + node->create_client("move_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != + rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); @@ -231,14 +240,16 @@ TEST_F(ServiceManagerTest, move_nonexistent_model) { request->pose.y = 5; request->pose.theta = 0; - auto client = node->create_client("move_model"); + auto client = + node->create_client("move_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != + rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); @@ -259,14 +270,16 @@ TEST_F(ServiceManagerTest, delete_model) { auto request = std::make_shared(); request->name = "turtlebot1"; - auto client = node->create_client("delete_model"); + auto client = + node->create_client("delete_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != + rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); @@ -275,8 +288,9 @@ TEST_F(ServiceManagerTest, delete_model) { // after deleting a mode, there should be one less model, and one less plugin ASSERT_EQ(w->models_.size(), 0UL); ASSERT_EQ(w->plugin_manager_.model_plugins_.size(), 0UL); - size_t count = std::count_if(w->models_.begin(), w->models_.end(), - [](Model* m) { return m->name_ == "turtlebot1"; }); + size_t count = + std::count_if(w->models_.begin(), w->models_.end(), + [](Model* m) { return m->name_ == "turtlebot1"; }); ASSERT_EQ(count, 0UL); } @@ -290,14 +304,16 @@ TEST_F(ServiceManagerTest, delete_nonexistent_model) { auto request = std::make_shared(); request->name = "random_model"; - auto client = node->create_client("delete_model"); + auto client = + node->create_client("delete_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != + rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); diff --git a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp index 7c0af4bc..4591ba76 100644 --- a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp +++ b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp @@ -46,8 +46,9 @@ #include #include -#include + #include +#include namespace fs = boost::filesystem; using namespace flatland_server; @@ -89,7 +90,8 @@ TEST(YamlPreprocTest, testEvalStrings) { setenv("VALIDSTRING", "Bar", 1); setenv("VALIDNUMBER", "123.4", 1); - std::shared_ptr node = rclcpp::Node::make_shared("test_yaml_preprocessor"); + std::shared_ptr node = + rclcpp::Node::make_shared("test_yaml_preprocessor"); // ros params node->declare_parameter("/string", "Foo"); node->declare_parameter("/int", 7); From bce1f244af8c6adc632ef7df875650850124b3ae Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 22:13:11 +0000 Subject: [PATCH 26/66] Fix diff drive tests. --- flatland_plugins/CMakeLists.txt | 13 ++++++------- flatland_plugins/test/diff_drive_test.cpp | 9 +++++---- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index 976115d5..beb15957 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -128,9 +128,13 @@ install( ## Add folders to be run by python nosetests # catkin_add_nosetests(test) -if(FALSE) # fake a block comment -if(BUILD_TESTING ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(diff_drive_test test/diff_drive_test.cpp) + target_link_libraries(diff_drive_test flatland_plugins_lib) +if(FALSE) # fake a block comment add_rostest_gtest(laser_test test/laser_test.test test/laser_test.cpp) target_link_libraries(laser_test flatland_plugins_lib) @@ -139,10 +143,6 @@ if(BUILD_TESTING ) test/tricycle_drive_test.cpp) target_link_libraries(tricycle_drive_test flatland_plugins_lib) - add_rostest_gtest(diff_drive_test test/diff_drive_test.test - test/diff_drive_test.cpp) - target_link_libraries(diff_drive_test flatland_plugins_lib) - add_rostest_gtest(bumper_test test/bumper_test.test test/bumper_test.cpp) target_link_libraries(bumper_test flatland_plugins_lib) @@ -162,7 +162,6 @@ if(BUILD_TESTING ) add_rostest_gtest(gps_test test/gps_test.test test/gps_test.cpp) target_link_libraries(gps_test flatland_plugins_lib) - endif() endif() diff --git a/flatland_plugins/test/diff_drive_test.cpp b/flatland_plugins/test/diff_drive_test.cpp index 10fccdcd..4c371dfb 100644 --- a/flatland_plugins/test/diff_drive_test.cpp +++ b/flatland_plugins/test/diff_drive_test.cpp @@ -44,19 +44,20 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include #include -#include +#include #include TEST(DiffDrivePluginTest, load_test) { + std::shared_ptr node = + rclcpp::Node::make_shared("test_diff_drive_plugin"); pluginlib::ClassLoader loader( "flatland_server", "flatland_server::ModelPlugin"); try { - boost::shared_ptr plugin = - loader.createInstance("flatland_plugins::DiffDrive"); + std::shared_ptr plugin = + loader.createSharedInstance("flatland_plugins::DiffDrive"); } catch (pluginlib::PluginlibException& e) { FAIL() << "Failed to load diff drive Drive plugin. " << e.what(); } From 17b78808d0fb4298112f3bfae6e833678dfb9537 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 22:17:37 +0000 Subject: [PATCH 27/66] Fix tricycle drive test. --- flatland_plugins/CMakeLists.txt | 7 +++---- flatland_plugins/test/diff_drive_test.test | 9 --------- flatland_plugins/test/tricycle_drive_test.cpp | 9 +++++---- flatland_plugins/test/tricycle_drive_test.test | 9 --------- 4 files changed, 8 insertions(+), 26 deletions(-) delete mode 100644 flatland_plugins/test/diff_drive_test.test delete mode 100644 flatland_plugins/test/tricycle_drive_test.test diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index beb15957..46557306 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -134,15 +134,14 @@ if(BUILD_TESTING) ament_add_gtest(diff_drive_test test/diff_drive_test.cpp) target_link_libraries(diff_drive_test flatland_plugins_lib) + ament_add_gtest(tricycle_drive_test test/tricycle_drive_test.cpp) + target_link_libraries(tricycle_drive_test flatland_plugins_lib) + if(FALSE) # fake a block comment add_rostest_gtest(laser_test test/laser_test.test test/laser_test.cpp) target_link_libraries(laser_test flatland_plugins_lib) - add_rostest_gtest(tricycle_drive_test test/tricycle_drive_test.test - test/tricycle_drive_test.cpp) - target_link_libraries(tricycle_drive_test flatland_plugins_lib) - add_rostest_gtest(bumper_test test/bumper_test.test test/bumper_test.cpp) target_link_libraries(bumper_test flatland_plugins_lib) diff --git a/flatland_plugins/test/diff_drive_test.test b/flatland_plugins/test/diff_drive_test.test deleted file mode 100644 index 1c460beb..00000000 --- a/flatland_plugins/test/diff_drive_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file diff --git a/flatland_plugins/test/tricycle_drive_test.cpp b/flatland_plugins/test/tricycle_drive_test.cpp index 2d5279e5..c0189e4f 100644 --- a/flatland_plugins/test/tricycle_drive_test.cpp +++ b/flatland_plugins/test/tricycle_drive_test.cpp @@ -44,19 +44,20 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include #include -#include +#include #include TEST(TricycleDrivePluginTest, load_test) { + std::shared_ptr node = + rclcpp::Node::make_shared("test_tricycle_drive_plugin"); pluginlib::ClassLoader loader( "flatland_server", "flatland_server::ModelPlugin"); try { - boost::shared_ptr plugin = - loader.createInstance("flatland_plugins::TricycleDrive"); + std::shared_ptr plugin = + loader.createSharedInstance("flatland_plugins::TricycleDrive"); } catch (pluginlib::PluginlibException& e) { FAIL() << "Failed to load Tricycle Drive plugin. " << e.what(); } diff --git a/flatland_plugins/test/tricycle_drive_test.test b/flatland_plugins/test/tricycle_drive_test.test deleted file mode 100644 index 7cc10c40..00000000 --- a/flatland_plugins/test/tricycle_drive_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file From 07d30bf30d56a8669c8f789083ff5a75af5bed9e Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 22:37:18 +0000 Subject: [PATCH 28/66] Fix model tf publisher tests. --- flatland_plugins/CMakeLists.txt | 7 ++-- .../test/model_tf_publisher_test.cpp | 35 ++++++++++--------- .../test/model_tf_publisher_test.test | 9 ----- 3 files changed, 22 insertions(+), 29 deletions(-) delete mode 100644 flatland_plugins/test/model_tf_publisher_test.test diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index 46557306..fd5aa566 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -137,6 +137,9 @@ if(BUILD_TESTING) ament_add_gtest(tricycle_drive_test test/tricycle_drive_test.cpp) target_link_libraries(tricycle_drive_test flatland_plugins_lib) + ament_add_gtest(model_tf_publisher_test test/model_tf_publisher_test.cpp) + target_link_libraries(model_tf_publisher_test flatland_plugins_lib) + if(FALSE) # fake a block comment add_rostest_gtest(laser_test test/laser_test.test test/laser_test.cpp) @@ -146,10 +149,6 @@ if(FALSE) # fake a block comment test/bumper_test.cpp) target_link_libraries(bumper_test flatland_plugins_lib) - add_rostest_gtest(model_tf_publisher_test test/model_tf_publisher_test.test - test/model_tf_publisher_test.cpp) - target_link_libraries(model_tf_publisher_test flatland_plugins_lib) - add_rostest_gtest(update_timer_test test/update_timer_test.test test/update_timer_test.cpp) target_link_libraries(update_timer_test flatland_plugins_lib) diff --git a/flatland_plugins/test/model_tf_publisher_test.cpp b/flatland_plugins/test/model_tf_publisher_test.cpp index e3adc38a..5f44e3c6 100644 --- a/flatland_plugins/test/model_tf_publisher_test.cpp +++ b/flatland_plugins/test/model_tf_publisher_test.cpp @@ -53,11 +53,14 @@ #include #include #include +#include +#include #include namespace fs = boost::filesystem; using namespace flatland_server; using namespace flatland_plugins; +using namespace std::chrono_literals; class ModelTfPublisherTest : public ::testing::Test { public: @@ -71,9 +74,7 @@ class ModelTfPublisherTest : public ::testing::Test { } void TearDown() override { - if (w != nullptr) { - delete w; - } + delete w; } static bool fltcmp(const double& n1, const double& n2) { @@ -93,8 +94,8 @@ class ModelTfPublisherTest : public ::testing::Test { bool TfEq(const geometry_msgs::msg::TransformStamped& tf, float x, float y, float a) { tf2::Quaternion q; - tf::quaternionMsgToTF(tf.transform.rotation, q); - tf::Matrix3x3 rot_matrix(q); + tf2::fromMsg(tf.transform.rotation, q); + tf2::Matrix3x3 rot_matrix(q); double roll, pitch, yaw; rot_matrix.getRPY(roll, pitch, yaw); @@ -123,17 +124,18 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_A) { this_file_dir / fs::path("model_tf_publisher_tests/tf_publish_test_A/world.yaml"); - Timekeeper timekeeper; + std::shared_ptr node = + rclcpp::Node::make_shared("test_tf_publisher_tf_publish_test_A"); + Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); - std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node,world_yaml.string()); - ModelTfPublisher* p = dynamic_cast( + auto* p = dynamic_cast( w->plugin_manager_.model_plugins_[0].get()); EXPECT_DOUBLE_EQ(5000.0, p->update_rate_); EXPECT_STREQ("antenna", p->reference_body_->name_.c_str()); - tf2_ros::Buffer tf_buffer; + tf2_ros::Buffer tf_buffer(node->get_clock()); tf2_ros::TransformListener tf_listener(tf_buffer); geometry_msgs::msg::TransformStamped tf_world_to_base; geometry_msgs::msg::TransformStamped tf_world_to_antenna; @@ -143,10 +145,10 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_A) { geometry_msgs::msg::TransformStamped tf_base_to_rear_bumper; // let it spin for 10 times to make sure the message gets through - ros::WallRate rate(500); + rclcpp::WallRate rate(500ns); for (unsigned int i = 0; i < 100; i++) { w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); rate.sleep(); } @@ -198,9 +200,10 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_B) { this_file_dir / fs::path("model_tf_publisher_tests/tf_publish_test_B/world.yaml"); - Timekeeper timekeeper; + std::shared_ptr node = + rclcpp::Node::make_shared("test_tf_publisher_tf_publish_test_B"); + Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); - std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node,world_yaml.string()); ModelTfPublisher* p = dynamic_cast( w->plugin_manager_.model_plugins_[0].get()); @@ -208,7 +211,7 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_B) { EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), p->update_rate_); EXPECT_STREQ("base", p->reference_body_->name_.c_str()); - tf2_ros::Buffer tf_buffer; + tf2_ros::Buffer tf_buffer(node->get_clock()); tf2_ros::TransformListener tf_listener(tf_buffer); geometry_msgs::msg::TransformStamped tf_map_to_base; geometry_msgs::msg::TransformStamped tf_base_to_antenna; @@ -218,10 +221,10 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_B) { geometry_msgs::msg::TransformStamped tf_base_to_rear_bumper; // let it spin for 10 times to make sure the message gets through - ros::WallRate rate(500); + rclcpp::WallRate rate(500ns); for (unsigned int i = 0; i < 100; i++) { w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); rate.sleep(); } diff --git a/flatland_plugins/test/model_tf_publisher_test.test b/flatland_plugins/test/model_tf_publisher_test.test deleted file mode 100644 index b7302255..00000000 --- a/flatland_plugins/test/model_tf_publisher_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file From 5f7409a372062e4b099ff394be1e0237f9572f37 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 22:46:40 +0000 Subject: [PATCH 29/66] Fix tween test --- flatland_plugins/CMakeLists.txt | 7 +++---- flatland_plugins/test/tween_test.cpp | 21 ++++++++++++--------- flatland_plugins/test/tween_test.test | 9 --------- 3 files changed, 15 insertions(+), 22 deletions(-) delete mode 100644 flatland_plugins/test/tween_test.test diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index fd5aa566..e5fc3c66 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -140,6 +140,9 @@ if(BUILD_TESTING) ament_add_gtest(model_tf_publisher_test test/model_tf_publisher_test.cpp) target_link_libraries(model_tf_publisher_test flatland_plugins_lib) + ament_add_gtest(tween_test test/tween_test.cpp) + target_link_libraries(tween_test flatland_plugins_lib) + if(FALSE) # fake a block comment add_rostest_gtest(laser_test test/laser_test.test test/laser_test.cpp) @@ -153,10 +156,6 @@ if(FALSE) # fake a block comment test/update_timer_test.cpp) target_link_libraries(update_timer_test flatland_plugins_lib) - add_rostest_gtest(tween_test test/tween_test.test - test/tween_test.cpp) - target_link_libraries(tween_test flatland_plugins_lib ${catkin_LIBRARIES}) - add_rostest_gtest(gps_test test/gps_test.test test/gps_test.cpp) target_link_libraries(gps_test flatland_plugins_lib) diff --git a/flatland_plugins/test/tween_test.cpp b/flatland_plugins/test/tween_test.cpp index 62271e59..0235e160 100644 --- a/flatland_plugins/test/tween_test.cpp +++ b/flatland_plugins/test/tween_test.cpp @@ -84,10 +84,11 @@ class TweenPluginTest : public ::testing::Test { TEST_F(TweenPluginTest, once_test) { world_yaml = this_file_dir / fs::path("tween_tests/once.world.yaml"); - Timekeeper timekeeper; + std::shared_ptr node = + rclcpp::Node::make_shared("test_tween_once_test"); + Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.5); - World* std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + World *w = World::MakeWorld(node, world_yaml.string()); Tween* tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); @@ -119,10 +120,11 @@ TEST_F(TweenPluginTest, once_test) { TEST_F(TweenPluginTest, yoyo_test) { world_yaml = this_file_dir / fs::path("tween_tests/yoyo.world.yaml"); - Timekeeper timekeeper; + std::shared_ptr node = + rclcpp::Node::make_shared("test_tween_yoyo_test"); + Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.5); - World* std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + World *w = World::MakeWorld(node, world_yaml.string()); Tween* tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); @@ -163,10 +165,11 @@ TEST_F(TweenPluginTest, yoyo_test) { TEST_F(TweenPluginTest, loop_test) { world_yaml = this_file_dir / fs::path("tween_tests/loop.world.yaml"); - Timekeeper timekeeper; + std::shared_ptr node = + rclcpp::Node::make_shared("test_tween_loop_test"); + Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.5); - World* std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + World* w = World::MakeWorld(node,world_yaml.string()); Tween* tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); diff --git a/flatland_plugins/test/tween_test.test b/flatland_plugins/test/tween_test.test deleted file mode 100644 index 2c075752..00000000 --- a/flatland_plugins/test/tween_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file From 4191274bab1ee93f8c2c1d5f872f876236198d2c Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 22:49:57 +0000 Subject: [PATCH 30/66] Fix gps test --- flatland_plugins/CMakeLists.txt | 7 +++---- flatland_plugins/test/gps_test.cpp | 9 +++++---- flatland_plugins/test/gps_test.test | 4 ---- 3 files changed, 8 insertions(+), 12 deletions(-) delete mode 100644 flatland_plugins/test/gps_test.test diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index e5fc3c66..6eb1108d 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -143,6 +143,9 @@ if(BUILD_TESTING) ament_add_gtest(tween_test test/tween_test.cpp) target_link_libraries(tween_test flatland_plugins_lib) + ament_add_gtest(gps_test test/gps_test.cpp) + target_link_libraries(gps_test flatland_plugins_lib) + if(FALSE) # fake a block comment add_rostest_gtest(laser_test test/laser_test.test test/laser_test.cpp) @@ -155,10 +158,6 @@ if(FALSE) # fake a block comment add_rostest_gtest(update_timer_test test/update_timer_test.test test/update_timer_test.cpp) target_link_libraries(update_timer_test flatland_plugins_lib) - - add_rostest_gtest(gps_test test/gps_test.test - test/gps_test.cpp) - target_link_libraries(gps_test flatland_plugins_lib) endif() endif() diff --git a/flatland_plugins/test/gps_test.cpp b/flatland_plugins/test/gps_test.cpp index 63da15d9..d128b38d 100644 --- a/flatland_plugins/test/gps_test.cpp +++ b/flatland_plugins/test/gps_test.cpp @@ -1,16 +1,17 @@ -#include #include #include -#include +#include #include TEST(GpsPluginTest, load_test) { + std::shared_ptr node = + rclcpp::Node::make_shared("test_gps_plugin"); pluginlib::ClassLoader loader( "flatland_server", "flatland_server::ModelPlugin"); try { - boost::shared_ptr plugin = - loader.createInstance("flatland_plugins::Gps"); + std::shared_ptr plugin = + loader.createSharedInstance("flatland_plugins::Gps"); } catch (pluginlib::PluginlibException& e) { FAIL() << "Failed to load GPS plugin. " << e.what(); } diff --git a/flatland_plugins/test/gps_test.test b/flatland_plugins/test/gps_test.test deleted file mode 100644 index 5bfa16a3..00000000 --- a/flatland_plugins/test/gps_test.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file From 288a9f429e7679baea7dc73a461b4a8ce3822e53 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 22:53:58 +0000 Subject: [PATCH 31/66] Fix wall rate value on model tf publisher test. --- flatland_plugins/test/bumper_test.test | 9 --------- flatland_plugins/test/model_tf_publisher_test.cpp | 6 ++---- 2 files changed, 2 insertions(+), 13 deletions(-) delete mode 100644 flatland_plugins/test/bumper_test.test diff --git a/flatland_plugins/test/bumper_test.test b/flatland_plugins/test/bumper_test.test deleted file mode 100644 index 173a7e4e..00000000 --- a/flatland_plugins/test/bumper_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file diff --git a/flatland_plugins/test/model_tf_publisher_test.cpp b/flatland_plugins/test/model_tf_publisher_test.cpp index 5f44e3c6..950cc3c8 100644 --- a/flatland_plugins/test/model_tf_publisher_test.cpp +++ b/flatland_plugins/test/model_tf_publisher_test.cpp @@ -46,7 +46,6 @@ #include #include -#include #include #include #include @@ -60,7 +59,6 @@ namespace fs = boost::filesystem; using namespace flatland_server; using namespace flatland_plugins; -using namespace std::chrono_literals; class ModelTfPublisherTest : public ::testing::Test { public: @@ -145,7 +143,7 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_A) { geometry_msgs::msg::TransformStamped tf_base_to_rear_bumper; // let it spin for 10 times to make sure the message gets through - rclcpp::WallRate rate(500ns); + rclcpp::WallRate rate(500); for (unsigned int i = 0; i < 100; i++) { w->Update(timekeeper); rclcpp::spin_some(node); @@ -221,7 +219,7 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_B) { geometry_msgs::msg::TransformStamped tf_base_to_rear_bumper; // let it spin for 10 times to make sure the message gets through - rclcpp::WallRate rate(500ns); + rclcpp::WallRate rate(500); for (unsigned int i = 0; i < 100; i++) { w->Update(timekeeper); rclcpp::spin_some(node); From 5364f391225d750003dd73c5afb2ff6a83220eb0 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 23:07:48 +0000 Subject: [PATCH 32/66] Fix bumber plugin tests. --- flatland_plugins/CMakeLists.txt | 7 ++-- flatland_plugins/test/bumper_test.cpp | 52 ++++++++++++++------------- 2 files changed, 31 insertions(+), 28 deletions(-) diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index 6eb1108d..c1c61b7f 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -146,15 +146,14 @@ if(BUILD_TESTING) ament_add_gtest(gps_test test/gps_test.cpp) target_link_libraries(gps_test flatland_plugins_lib) + ament_add_gtest(bumper_test test/bumper_test.cpp) + target_link_libraries(bumper_test flatland_plugins_lib) + if(FALSE) # fake a block comment add_rostest_gtest(laser_test test/laser_test.test test/laser_test.cpp) target_link_libraries(laser_test flatland_plugins_lib) - add_rostest_gtest(bumper_test test/bumper_test.test - test/bumper_test.cpp) - target_link_libraries(bumper_test flatland_plugins_lib) - add_rostest_gtest(update_timer_test test/update_timer_test.test test/update_timer_test.cpp) target_link_libraries(update_timer_test flatland_plugins_lib) diff --git a/flatland_plugins/test/bumper_test.cpp b/flatland_plugins/test/bumper_test.cpp index 29f3c825..168264ae 100644 --- a/flatland_plugins/test/bumper_test.cpp +++ b/flatland_plugins/test/bumper_test.cpp @@ -44,9 +44,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include -#include #include #include #include @@ -57,7 +56,8 @@ namespace fs = boost::filesystem; using namespace flatland_server; using namespace flatland_plugins; -using namespace flatland_msgs; +using namespace flatland_msgs::msg; +using std::placeholders::_1; class BumperPluginTest : public ::testing::Test { public: @@ -65,6 +65,9 @@ class BumperPluginTest : public ::testing::Test { boost::filesystem::path world_yaml; flatland_msgs::msg::Collisions msg1, msg2; World* w; + std::shared_ptr node; + + BumperPluginTest() : node(rclcpp::Node::make_shared("test_bumper_plugin")) {} void SetUp() override { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); @@ -112,12 +115,12 @@ class BumperPluginTest : public ::testing::Test { } bool CollisionsEq(const Collisions& collisions, const std::string& frame_id, - int num_collisions) { + size_t num_collisions) { if (!StringEq("frame_id", collisions.header.frame_id, frame_id)) return false; if (num_collisions != collisions.collisions.size()) { - printf("Num collisions Actual:%lu != Expected:%d\n", + printf("Num collisions Actual:%lu != Expected:%lu\n", collisions.collisions.size(), num_collisions); return false; } @@ -128,7 +131,7 @@ class BumperPluginTest : public ::testing::Test { // check the received scan data is as expected bool CollisionEq(const Collision& collision, const std::string& entity_a, const std::string& body_A, const std::string& entity_b, - const std::string& body_B, int return_size, + const std::string& body_B, size_t return_size, const std::pair& normal) { if (!StringEq("entity_a", collision.entity_a, entity_a)) return false; if (!StringEq("body_A", collision.body_a, body_A)) return false; @@ -142,7 +145,7 @@ class BumperPluginTest : public ::testing::Test { collision.contact_positions.size() == return_size && collision.contact_normals.size() == return_size)) { printf( - "Vector sizes are expected to be all the same and have sizes %d, " + "Vector sizes are expected to be all the same and have sizes %lu, " "magnitude_forces=%lu contact_positions=%lu contact_normals=%lu\n", return_size, collision.magnitude_forces.size(), collision.contact_positions.size(), collision.contact_normals.size()); @@ -165,14 +168,14 @@ class BumperPluginTest : public ::testing::Test { return true; } - void CollisionCb_A(const flatland_msgs::msg::Collisions& msg) { msg1 = msg; } + void CollisionCb_A(const Collisions& msg) { msg1 = msg; } - void CollisionCb_B(const flatland_msgs::msg::Collisions& msg) { msg2 = msg; } + void CollisionCb_B(const Collisions& msg) { msg2 = msg; } - void SpinRos(float hz, int iterations) { - ros::WallRate rate(hz); + void SpinRos(float hz, unsigned iterations) { + rclcpp::WallRate rate(hz); for (unsigned int i = 0; i < iterations; i++) { - ros::spinOnce(); + rclcpp::spin_some(node); rate.sleep(); } } @@ -185,27 +188,28 @@ TEST_F(BumperPluginTest, collision_test) { world_yaml = this_file_dir / fs::path("bumper_tests/collision_test/world.yaml"); - Timekeeper timekeeper; + Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.01); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node,world_yaml.string()); - ros::NodeHandle nh; - ros::Subscriber sub_1, sub_2, sub_3; BumperPluginTest* obj = dynamic_cast(this); - sub_1 = nh.subscribe("collisions", 1, &BumperPluginTest::CollisionCb_A, obj); - sub_2 = - nh.subscribe("collisions_B", 1, &BumperPluginTest::CollisionCb_B, obj); + auto sub_1 = node->create_subscription( + "collisions", 1, + std::bind(&BumperPluginTest::CollisionCb_A, obj, _1)); + auto sub_2 = node->create_subscription( + "collisions_B", 1, + std::bind(&BumperPluginTest::CollisionCb_B, obj, _1)); Bumper* p = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); Body* b0 = p->GetModel()->bodies_[0]; Body* b1 = p->GetModel()->bodies_[1]; - // check that there are no collision at the begining + // check that there are no collision at the beginning for (unsigned int i = 0; i < 100; i++) { w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); } SpinRos(500, 10); // make sure the messages gets through @@ -220,7 +224,7 @@ TEST_F(BumperPluginTest, collision_test) { // moving at the desired velocity b0->physics_body_->SetLinearVelocity(b2Vec2(1, 0.0)); w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); } SpinRos(500, 10); // makes sure the ros message gets through @@ -233,7 +237,7 @@ TEST_F(BumperPluginTest, collision_test) { for (unsigned int i = 0; i < 50; i++) { b0->physics_body_->SetLinearVelocity(b2Vec2(1, 0.0)); w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); } SpinRos(500, 10); ASSERT_TRUE(CollisionsEq(msg1, "map", 2)); @@ -249,7 +253,7 @@ TEST_F(BumperPluginTest, collision_test) { for (unsigned int i = 0; i < 300; i++) { b0->physics_body_->SetLinearVelocity(b2Vec2(-1, 0.0)); w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); } SpinRos(500, 10); @@ -264,7 +268,7 @@ TEST_F(BumperPluginTest, collision_test) { for (unsigned int i = 0; i < 300; i++) { b0->physics_body_->SetLinearVelocity(b2Vec2(-1, 0.0)); w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); } SpinRos(500, 10); From 37b72ad0f42b0509892b2f0ae7f4fb564d98bf11 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 23:24:45 +0000 Subject: [PATCH 33/66] Fix laser plugin tests. --- flatland_plugins/CMakeLists.txt | 6 +-- flatland_plugins/test/laser_test.cpp | 77 +++++++++++++++------------ flatland_plugins/test/laser_test.test | 9 ---- 3 files changed, 45 insertions(+), 47 deletions(-) delete mode 100644 flatland_plugins/test/laser_test.test diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index c1c61b7f..ee4845f6 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -149,11 +149,9 @@ if(BUILD_TESTING) ament_add_gtest(bumper_test test/bumper_test.cpp) target_link_libraries(bumper_test flatland_plugins_lib) -if(FALSE) # fake a block comment - add_rostest_gtest(laser_test test/laser_test.test - test/laser_test.cpp) + ament_add_gtest(laser_test test/laser_test.cpp) target_link_libraries(laser_test flatland_plugins_lib) - +if(FALSE) # fake a block comment add_rostest_gtest(update_timer_test test/update_timer_test.test test/update_timer_test.cpp) target_link_libraries(update_timer_test flatland_plugins_lib) diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index 7f39517f..04e572d1 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/flatland_plugins/test/laser_test.cpp @@ -46,16 +46,16 @@ #include #include -#include #include #include #include -#include +#include #include namespace fs = boost::filesystem; using namespace flatland_server; using namespace flatland_plugins; +using std::placeholders::_1; class LaserPluginTest : public ::testing::Test { public: @@ -63,6 +63,9 @@ class LaserPluginTest : public ::testing::Test { boost::filesystem::path world_yaml; sensor_msgs::msg::LaserScan scan_front, scan_center, scan_back; World* w; + std::shared_ptr node; + + LaserPluginTest() : node(rclcpp::Node::make_shared("test_laser_plugin")) {} void SetUp() override { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); @@ -70,9 +73,7 @@ class LaserPluginTest : public ::testing::Test { } void TearDown() override { - if (w != nullptr) { - delete w; - } + delete w; } static bool fltcmp(const double& n1, const double& n2) { @@ -107,7 +108,7 @@ class LaserPluginTest : public ::testing::Test { } // check the received scan data is as expected - bool ScanEq(const sensor_msgs::msg::LaserScan& scan, std::string frame_id, + bool ScanEq(const sensor_msgs::msg::LaserScan& scan, const std::string& frame_id, float angle_min, float angle_max, float angle_increment, float time_increment, float scan_time, float range_min, float range_max, std::vector ranges, @@ -168,27 +169,31 @@ class LaserPluginTest : public ::testing::Test { TEST_F(LaserPluginTest, range_test) { world_yaml = this_file_dir / fs::path("laser_tests/range_test/world.yaml"); - Timekeeper timekeeper; + Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node,world_yaml.string()); - ros::NodeHandle nh; - ros::Subscriber sub_1, sub_2, sub_3; - LaserPluginTest* obj = dynamic_cast(this); - sub_1 = nh.subscribe("r/scan", 1, &LaserPluginTest::ScanFrontCb, obj); - sub_2 = nh.subscribe("scan_center", 1, &LaserPluginTest::ScanCenterCb, obj); - sub_3 = nh.subscribe("r/scan_back", 1, &LaserPluginTest::ScanBackCb, obj); - - Laser* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - Laser* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); - Laser* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); + auto* obj = dynamic_cast(this); + auto sub_1 = node->create_subscription( + "scan", 1, + std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); + auto sub_2 = node->create_subscription( + "scan_center", 1, + std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); + auto sub_3 = node->create_subscription( + "scan_back", 1, + std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); + + auto* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + auto* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); + auto* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); // let it spin for 10 times to make sure the message gets through - ros::WallRate rate(500); + rclcpp::WallRate rate(500); for (unsigned int i = 0; i < 10; i++) { w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); rate.sleep(); } @@ -206,7 +211,7 @@ TEST_F(LaserPluginTest, range_test) { EXPECT_TRUE(ScanEq(scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {})); - EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_; + EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p3->update_rate_; EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); } /** @@ -216,27 +221,31 @@ TEST_F(LaserPluginTest, intensity_test) { world_yaml = this_file_dir / fs::path("laser_tests/intensity_test/world.yaml"); - Timekeeper timekeeper; + Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node,world_yaml.string()); - ros::NodeHandle nh; - ros::Subscriber sub_1, sub_2, sub_3; - LaserPluginTest* obj = dynamic_cast(this); - sub_1 = nh.subscribe("r/scan", 1, &LaserPluginTest::ScanFrontCb, obj); - sub_2 = nh.subscribe("scan_center", 1, &LaserPluginTest::ScanCenterCb, obj); - sub_3 = nh.subscribe("r/scan_back", 1, &LaserPluginTest::ScanBackCb, obj); - - Laser* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - Laser* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); - Laser* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); + auto* obj = dynamic_cast(this); + auto sub_1 = node->create_subscription( + "scan", 1, + std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); + auto sub_2 = node->create_subscription( + "scan_center", 1, + std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); + auto sub_3 = node->create_subscription( + "scan_back", 1, + std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); + + auto* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + auto* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); + auto* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); // let it spin for 10 times to make sure the message gets through - ros::WallRate rate(500); + rclcpp::WallRate rate(500); for (unsigned int i = 0; i < 10; i++) { w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); rate.sleep(); } @@ -253,7 +262,7 @@ TEST_F(LaserPluginTest, intensity_test) { EXPECT_EQ(p2->body_, w->models_[0]->bodies_[0]); EXPECT_TRUE(ScanEq(scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {0, 0, 0, 0, 0})); - EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p2->update_rate_; + EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p3->update_rate_; EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); } diff --git a/flatland_plugins/test/laser_test.test b/flatland_plugins/test/laser_test.test deleted file mode 100644 index b49afb43..00000000 --- a/flatland_plugins/test/laser_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file From 707b8c1a97770bfcef550fba9d0741296fe82842 Mon Sep 17 00:00:00 2001 From: Ana Barros Date: Sun, 27 Nov 2022 23:44:21 +0000 Subject: [PATCH 34/66] WIP: Fix update timer tests. --- flatland_plugins/CMakeLists.txt | 6 ++--- flatland_plugins/test/update_timer_test.cpp | 28 +++++++++++--------- flatland_plugins/test/update_timer_test.test | 9 ------- 3 files changed, 17 insertions(+), 26 deletions(-) delete mode 100644 flatland_plugins/test/update_timer_test.test diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index ee4845f6..e3d79242 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -151,11 +151,9 @@ if(BUILD_TESTING) ament_add_gtest(laser_test test/laser_test.cpp) target_link_libraries(laser_test flatland_plugins_lib) -if(FALSE) # fake a block comment - add_rostest_gtest(update_timer_test test/update_timer_test.test - test/update_timer_test.cpp) + + ament_add_gtest(update_timer_test test/update_timer_test.cpp) target_link_libraries(update_timer_test flatland_plugins_lib) endif() -endif() ament_package() diff --git a/flatland_plugins/test/update_timer_test.cpp b/flatland_plugins/test/update_timer_test.cpp index 040eb119..a73a5d5e 100644 --- a/flatland_plugins/test/update_timer_test.cpp +++ b/flatland_plugins/test/update_timer_test.cpp @@ -54,6 +54,7 @@ namespace fs = boost::filesystem; using namespace flatland_server; using namespace flatland_plugins; +using namespace std::chrono_literals; class TestPlugin : public ModelPlugin { public: @@ -83,8 +84,11 @@ class UpdateTimerTest : public ::testing::Test { double actual_rate; double wall_rate; double step_size; - double sim_test_time; + int64_t sim_test_time; World* w; + std::shared_ptr node; + + UpdateTimerTest() : node(rclcpp::Node::make_shared("test_update_timer")) {} void SetUp() override { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); @@ -92,34 +96,32 @@ class UpdateTimerTest : public ::testing::Test { } void TearDown() override { - if (w != nullptr) { - delete w; - } + delete w; } void ExecuteRateTest() { - Timekeeper timekeeper; - std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + Timekeeper timekeeper(node); + w = World::MakeWorld(node,world_yaml.string()); // artificially load a plugin - boost::shared_ptr p(new TestPlugin()); - p->Initialize("TestPlugin", "test_plugin", w->models_[0], YAML::Node()); + std::shared_ptr p(new TestPlugin()); + p->Initialize(node, "TestPlugin", "test_plugin", w->models_[0], YAML::Node()); w->plugin_manager_.model_plugins_.push_back(p); p->update_timer_.SetRate(set_rate); timekeeper.SetMaxStepSize(step_size); - ros::WallRate rate(wall_rate); + rclcpp::WallRate rate(wall_rate); // run for two seconds - while (timekeeper.GetSimTime() < rclcpp::Time(sim_test_time)) { + auto timeLimit = rclcpp::Time(sim_test_time); + while (timekeeper.GetSimTime() < timeLimit) { w->Update(timekeeper); - ros::spinOnce(); + rclcpp::spin_some(node); rate.sleep(); } - actual_rate = p->update_counter_ / timekeeper.GetSimTime().toSec(); + actual_rate = p->update_counter_ / timekeeper.GetSimTime().seconds(); printf("Actual Rate: %f, Expected Rate: %f\n", actual_rate, expected_rate); } diff --git a/flatland_plugins/test/update_timer_test.test b/flatland_plugins/test/update_timer_test.test deleted file mode 100644 index 73710855..00000000 --- a/flatland_plugins/test/update_timer_test.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - \ No newline at end of file From d427a13f84a5218e41e9411b121ad02ade9846f9 Mon Sep 17 00:00:00 2001 From: The Zambi Date: Mon, 28 Nov 2022 09:36:54 +0000 Subject: [PATCH 35/66] Fixed update timer tests --- flatland_plugins/test/update_timer_test.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/flatland_plugins/test/update_timer_test.cpp b/flatland_plugins/test/update_timer_test.cpp index a73a5d5e..b7e98be9 100644 --- a/flatland_plugins/test/update_timer_test.cpp +++ b/flatland_plugins/test/update_timer_test.cpp @@ -49,6 +49,7 @@ #include #include #include + #include namespace fs = boost::filesystem; @@ -61,12 +62,12 @@ class TestPlugin : public ModelPlugin { UpdateTimer update_timer_; int update_counter_; - void OnInitialize(const YAML::Node& config) override { + void OnInitialize(const YAML::Node &config) override { update_timer_.SetRate(0); update_counter_ = 0; } - void BeforePhysicsStep(const Timekeeper& timekeeper) override { + void BeforePhysicsStep(const Timekeeper &timekeeper) override { // keeps this function updating at a specific rate if (!update_timer_.CheckUpdate(timekeeper)) { return; @@ -85,7 +86,7 @@ class UpdateTimerTest : public ::testing::Test { double wall_rate; double step_size; int64_t sim_test_time; - World* w; + World *w; std::shared_ptr node; UpdateTimerTest() : node(rclcpp::Node::make_shared("test_update_timer")) {} @@ -95,17 +96,16 @@ class UpdateTimerTest : public ::testing::Test { w = nullptr; } - void TearDown() override { - delete w; - } + void TearDown() override { delete w; } void ExecuteRateTest() { Timekeeper timekeeper(node); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); // artificially load a plugin std::shared_ptr p(new TestPlugin()); - p->Initialize(node, "TestPlugin", "test_plugin", w->models_[0], YAML::Node()); + p->Initialize(node, "TestPlugin", "test_plugin", w->models_[0], + YAML::Node()); w->plugin_manager_.model_plugins_.push_back(p); p->update_timer_.SetRate(set_rate); @@ -114,7 +114,7 @@ class UpdateTimerTest : public ::testing::Test { rclcpp::WallRate rate(wall_rate); // run for two seconds - auto timeLimit = rclcpp::Time(sim_test_time); + auto timeLimit = rclcpp::Time(sim_test_time, 0); while (timekeeper.GetSimTime() < timeLimit) { w->Update(timekeeper); rclcpp::spin_some(node); @@ -218,7 +218,7 @@ TEST_F(UpdateTimerTest, rate_test_E) { } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char **argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); From e065cadf1c52b2d7a8ea978b7c97f8f36347abb9 Mon Sep 17 00:00:00 2001 From: The Zambi Date: Mon, 28 Nov 2022 09:37:44 +0000 Subject: [PATCH 36/66] Format test files --- flatland_plugins/test/bumper_test.cpp | 13 +++-- flatland_plugins/test/diff_drive_test.cpp | 1 + flatland_plugins/test/gps_test.cpp | 1 + flatland_plugins/test/laser_test.cpp | 49 +++++++++---------- .../test/model_tf_publisher_test.cpp | 19 ++++--- flatland_plugins/test/tricycle_drive_test.cpp | 1 + flatland_plugins/test/tween_test.cpp | 7 +-- 7 files changed, 45 insertions(+), 46 deletions(-) diff --git a/flatland_plugins/test/bumper_test.cpp b/flatland_plugins/test/bumper_test.cpp index 168264ae..5da53a54 100644 --- a/flatland_plugins/test/bumper_test.cpp +++ b/flatland_plugins/test/bumper_test.cpp @@ -44,13 +44,14 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include #include #include #include #include #include + +#include #include namespace fs = boost::filesystem; @@ -191,15 +192,13 @@ TEST_F(BumperPluginTest, collision_test) { Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.01); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); BumperPluginTest* obj = dynamic_cast(this); auto sub_1 = node->create_subscription( - "collisions", 1, - std::bind(&BumperPluginTest::CollisionCb_A, obj, _1)); + "collisions", 1, std::bind(&BumperPluginTest::CollisionCb_A, obj, _1)); auto sub_2 = node->create_subscription( - "collisions_B", 1, - std::bind(&BumperPluginTest::CollisionCb_B, obj, _1)); + "collisions_B", 1, std::bind(&BumperPluginTest::CollisionCb_B, obj, _1)); Bumper* p = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); @@ -293,7 +292,7 @@ TEST_F(BumperPluginTest, invalid_A) { try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { std::cmatch match; diff --git a/flatland_plugins/test/diff_drive_test.cpp b/flatland_plugins/test/diff_drive_test.cpp index 4c371dfb..1c156446 100644 --- a/flatland_plugins/test/diff_drive_test.cpp +++ b/flatland_plugins/test/diff_drive_test.cpp @@ -46,6 +46,7 @@ #include #include + #include #include diff --git a/flatland_plugins/test/gps_test.cpp b/flatland_plugins/test/gps_test.cpp index d128b38d..f4e78056 100644 --- a/flatland_plugins/test/gps_test.cpp +++ b/flatland_plugins/test/gps_test.cpp @@ -1,5 +1,6 @@ #include #include + #include #include diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index 04e572d1..d9a34f2d 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/flatland_plugins/test/laser_test.cpp @@ -49,8 +49,9 @@ #include #include #include -#include + #include +#include namespace fs = boost::filesystem; using namespace flatland_server; @@ -72,9 +73,7 @@ class LaserPluginTest : public ::testing::Test { w = nullptr; } - void TearDown() override { - delete w; - } + void TearDown() override { delete w; } static bool fltcmp(const double& n1, const double& n2) { if (std::isinf(n1) && std::isinf(n2)) { @@ -108,10 +107,10 @@ class LaserPluginTest : public ::testing::Test { } // check the received scan data is as expected - bool ScanEq(const sensor_msgs::msg::LaserScan& scan, const std::string& frame_id, - float angle_min, float angle_max, float angle_increment, - float time_increment, float scan_time, float range_min, - float range_max, std::vector ranges, + bool ScanEq(const sensor_msgs::msg::LaserScan& scan, + const std::string& frame_id, float angle_min, float angle_max, + float angle_increment, float time_increment, float scan_time, + float range_min, float range_max, std::vector ranges, std::vector intensities) { if (scan.header.frame_id != frame_id) { printf("frame_id Actual:%s != Expected:%s\n", @@ -158,8 +157,12 @@ class LaserPluginTest : public ::testing::Test { return true; } - void ScanFrontCb(const sensor_msgs::msg::LaserScan& msg) { scan_front = msg; }; - void ScanCenterCb(const sensor_msgs::msg::LaserScan& msg) { scan_center = msg; }; + void ScanFrontCb(const sensor_msgs::msg::LaserScan& msg) { + scan_front = msg; + }; + void ScanCenterCb(const sensor_msgs::msg::LaserScan& msg) { + scan_center = msg; + }; void ScanBackCb(const sensor_msgs::msg::LaserScan& msg) { scan_back = msg; }; }; @@ -172,18 +175,15 @@ TEST_F(LaserPluginTest, range_test) { Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); auto* obj = dynamic_cast(this); auto sub_1 = node->create_subscription( - "scan", 1, - std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); + "scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); auto sub_2 = node->create_subscription( - "scan_center", 1, - std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); + "scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); auto sub_3 = node->create_subscription( - "scan_back", 1, - std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); + "scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); auto* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); auto* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); @@ -224,18 +224,15 @@ TEST_F(LaserPluginTest, intensity_test) { Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); auto* obj = dynamic_cast(this); auto sub_1 = node->create_subscription( - "scan", 1, - std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); + "scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); auto sub_2 = node->create_subscription( - "scan_center", 1, - std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); + "scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); auto sub_3 = node->create_subscription( - "scan_back", 1, - std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); + "scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); auto* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); auto* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); @@ -275,7 +272,7 @@ TEST_F(LaserPluginTest, invalid_A) { try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { @@ -301,7 +298,7 @@ TEST_F(LaserPluginTest, invalid_B) { try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { diff --git a/flatland_plugins/test/model_tf_publisher_test.cpp b/flatland_plugins/test/model_tf_publisher_test.cpp index 950cc3c8..74dbda7a 100644 --- a/flatland_plugins/test/model_tf_publisher_test.cpp +++ b/flatland_plugins/test/model_tf_publisher_test.cpp @@ -48,13 +48,14 @@ #include #include #include -#include #include #include -#include #include -#include +#include + +#include #include +#include namespace fs = boost::filesystem; using namespace flatland_server; @@ -71,9 +72,7 @@ class ModelTfPublisherTest : public ::testing::Test { w = nullptr; } - void TearDown() override { - delete w; - } + void TearDown() override { delete w; } static bool fltcmp(const double& n1, const double& n2) { if (std::isinf(n1) && std::isinf(n2)) { @@ -126,7 +125,7 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_A) { rclcpp::Node::make_shared("test_tf_publisher_tf_publish_test_A"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); auto* p = dynamic_cast( w->plugin_manager_.model_plugins_[0].get()); @@ -202,7 +201,7 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_B) { rclcpp::Node::make_shared("test_tf_publisher_tf_publish_test_B"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); ModelTfPublisher* p = dynamic_cast( w->plugin_manager_.model_plugins_[0].get()); @@ -264,7 +263,7 @@ TEST_F(ModelTfPublisherTest, invalid_A) { try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { @@ -291,7 +290,7 @@ TEST_F(ModelTfPublisherTest, invalid_B) { try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); - w = World::MakeWorld(node,world_yaml.string()); + w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; } catch (const PluginException& e) { diff --git a/flatland_plugins/test/tricycle_drive_test.cpp b/flatland_plugins/test/tricycle_drive_test.cpp index c0189e4f..43f47d23 100644 --- a/flatland_plugins/test/tricycle_drive_test.cpp +++ b/flatland_plugins/test/tricycle_drive_test.cpp @@ -46,6 +46,7 @@ #include #include + #include #include diff --git a/flatland_plugins/test/tween_test.cpp b/flatland_plugins/test/tween_test.cpp index 0235e160..111d8bdd 100644 --- a/flatland_plugins/test/tween_test.cpp +++ b/flatland_plugins/test/tween_test.cpp @@ -49,6 +49,7 @@ #include #include #include + #include namespace fs = boost::filesystem; @@ -88,7 +89,7 @@ TEST_F(TweenPluginTest, once_test) { rclcpp::Node::make_shared("test_tween_once_test"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.5); - World *w = World::MakeWorld(node, world_yaml.string()); + World* w = World::MakeWorld(node, world_yaml.string()); Tween* tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); @@ -124,7 +125,7 @@ TEST_F(TweenPluginTest, yoyo_test) { rclcpp::Node::make_shared("test_tween_yoyo_test"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.5); - World *w = World::MakeWorld(node, world_yaml.string()); + World* w = World::MakeWorld(node, world_yaml.string()); Tween* tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); @@ -169,7 +170,7 @@ TEST_F(TweenPluginTest, loop_test) { rclcpp::Node::make_shared("test_tween_loop_test"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(node,world_yaml.string()); + World* w = World::MakeWorld(node, world_yaml.string()); Tween* tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); From aa654389ed5600617edffabdb91b69eaf886bc38 Mon Sep 17 00:00:00 2001 From: The Zambi Date: Mon, 28 Nov 2022 10:24:31 +0000 Subject: [PATCH 37/66] Fix missing imports --- flatland_plugins/include/flatland_plugins/bumper.h | 1 + 1 file changed, 1 insertion(+) diff --git a/flatland_plugins/include/flatland_plugins/bumper.h b/flatland_plugins/include/flatland_plugins/bumper.h index 4a51822a..8129adae 100644 --- a/flatland_plugins/include/flatland_plugins/bumper.h +++ b/flatland_plugins/include/flatland_plugins/bumper.h @@ -46,6 +46,7 @@ #include #include +#include #include #ifndef FLATLAND_PLUGINS_BUMPER_H From 41ae4c3f5f8227e6e04fc06a2ec1b4f637d1614e Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 28 Nov 2022 18:47:26 +0000 Subject: [PATCH 38/66] Fix yaml test file number format (liblua version 5.3 -> 5.1). --- .../test/yaml_preprocessor/yaml/eval.strings.out.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flatland_server/test/yaml_preprocessor/yaml/eval.strings.out.yaml b/flatland_server/test/yaml_preprocessor/yaml/eval.strings.out.yaml index f35e81d9..8245d093 100644 --- a/flatland_server/test/yaml_preprocessor/yaml/eval.strings.out.yaml +++ b/flatland_server/test/yaml_preprocessor/yaml/eval.strings.out.yaml @@ -27,7 +27,7 @@ testParam: param3: Foo param4: 10.5 param5: "" - param6: 223.0 + param6: 223 param7: BBBC param8: true - param9: true \ No newline at end of file + param9: true From e6778ff458ba72148915577db1c571269699dab6 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Tue, 29 Nov 2022 23:05:53 +0000 Subject: [PATCH 39/66] Change layer publisher QoS policy. This fixes a problem where rviz doesn't start on time to catch the publish of the layer markers. --- flatland_server/src/debug_visualization.cpp | 61 ++++++++++++++------- 1 file changed, 41 insertions(+), 20 deletions(-) diff --git a/flatland_server/src/debug_visualization.cpp b/flatland_server/src/debug_visualization.cpp index c1cba5e3..90b9cb64 100644 --- a/flatland_server/src/debug_visualization.cpp +++ b/flatland_server/src/debug_visualization.cpp @@ -45,22 +45,27 @@ */ #include "flatland_server/debug_visualization.h" + #include -#include +#include #include #include #include + #include +#include #include namespace flatland_server { -DebugVisualization::DebugVisualization(rclcpp::Node::SharedPtr node) : node_(node) { +DebugVisualization::DebugVisualization(rclcpp::Node::SharedPtr node) + : node_(node) { topic_list_publisher_ = node_->create_publisher("topics", 1); } -std::shared_ptr DebugVisualization::Get(rclcpp::Node::SharedPtr node) { +std::shared_ptr DebugVisualization::Get( + rclcpp::Node::SharedPtr node) { static std::shared_ptr instance; // todo: fix this? How should singletons work with shared pointers? if (!instance) { @@ -70,13 +75,13 @@ std::shared_ptr DebugVisualization::Get(rclcpp::Node::Shared } void DebugVisualization::JointToMarkers( - visualization_msgs::msg::MarkerArray& markers, b2Joint* joint, float r, float g, - float b, float a) { + visualization_msgs::msg::MarkerArray& markers, b2Joint* joint, float r, + float g, float b, float a) { if (joint->GetType() == e_distanceJoint || joint->GetType() == e_pulleyJoint || joint->GetType() == e_mouseJoint) { RCLCPP_ERROR(rclcpp::get_logger("DebugVis"), - "Unimplemented visualization joints. See b2World.cpp for " - "implementation"); + "Unimplemented visualization joints. See b2World.cpp for " + "implementation"); return; } @@ -122,9 +127,9 @@ void DebugVisualization::JointToMarkers( markers.markers.push_back(marker); } -void DebugVisualization::BodyToMarkers(visualization_msgs::msg::MarkerArray& markers, - b2Body* body, float r, float g, float b, - float a) { +void DebugVisualization::BodyToMarkers( + visualization_msgs::msg::MarkerArray& markers, b2Body* body, float r, + float g, float b, float a) { b2Fixture* fixture = body->GetFixtureList(); while (fixture != NULL) { // traverse fixture linked list @@ -175,7 +180,7 @@ void DebugVisualization::BodyToMarkers(visualization_msgs::msg::MarkerArray& mar } break; - case b2Shape::e_edge: { // Convert b2Edge -> LINE_LIST + case b2Shape::e_edge: { // Convert b2Edge -> LINE_LIST geometry_msgs::msg::Point p; // b2Edge uses vertex1 and 2 for its edges b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); @@ -207,8 +212,9 @@ void DebugVisualization::BodyToMarkers(visualization_msgs::msg::MarkerArray& mar default: // Unsupported shape rclcpp::Clock steady_clock = rclcpp::Clock(RCL_STEADY_TIME); - RCLCPP_WARN_THROTTLE(rclcpp::get_logger("Debug Visualization"), steady_clock, 1000, "Unsupported Box2D shape %d", - static_cast(fixture->GetType())); + RCLCPP_WARN_THROTTLE(rclcpp::get_logger("Debug Visualization"), + steady_clock, 1000, "Unsupported Box2D shape %d", + static_cast(fixture->GetType())); fixture = fixture->GetNext(); continue; // Do not add broken marker break; @@ -247,7 +253,8 @@ void DebugVisualization::Publish(const Timekeeper& timekeeper) { if (to_delete.size() > 0) { for (const auto& topic : to_delete) { - RCLCPP_WARN(rclcpp::get_logger("DebugVis"), "Deleting topic %s", topic.c_str()); + RCLCPP_WARN(rclcpp::get_logger("DebugVis"), "Deleting topic %s", + topic.c_str()); topics_.erase(topic); } PublishTopicList(); @@ -276,8 +283,9 @@ void DebugVisualization::VisualizeLayer(std::string name, Body* body) { marker.pose.position.y = body->physics_body_->GetPosition().y; tf2::Quaternion q; // use tf2 to convert 2d yaw -> 3d quaternion - q.setRPY(0, 0, body->physics_body_ - ->GetAngle()); // from euler angles: roll, pitch, yaw + q.setRPY(0, 0, + body->physics_body_ + ->GetAngle()); // from euler angles: roll, pitch, yaw marker.pose.orientation = tf2::toMsg(q); marker.type = marker.TRIANGLE_LIST; @@ -348,13 +356,26 @@ void DebugVisualization::Reset(std::string name) { } void DebugVisualization::AddTopicIfNotExist(const std::string& name) { + static const rmw_qos_profile_t qos_profile = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + // If the topic doesn't exist yet, create it if (topics_.count(name) == 0) { topics_[name] = { - node_->create_publisher(name, 1), true, - visualization_msgs::msg::MarkerArray()}; + node_->create_publisher( + name, rclcpp::QoS(rclcpp::KeepLast(1), qos_profile)), + true, visualization_msgs::msg::MarkerArray()}; - RCLCPP_INFO_ONCE(rclcpp::get_logger("Debug Visualization"), "Visualizing %s", name.c_str()); + RCLCPP_INFO_ONCE(rclcpp::get_logger("Debug Visualization"), + "Visualizing %s", name.c_str()); PublishTopicList(); } } @@ -365,4 +386,4 @@ void DebugVisualization::PublishTopicList() { topic_list.topics.push_back(topic_pair.first); topic_list_publisher_->publish(topic_list); } -} //namespace flatland_server +} // namespace flatland_server From 7695f25a838123a16801b53ba9dbfa6fac3b88a5 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Wed, 30 Nov 2022 16:45:58 +0000 Subject: [PATCH 40/66] Create pause tool plugin for rviz. When pressed, it toggles the paused state of the simulation. --- flatland_rviz_plugins/CMakeLists.txt | 117 ++++++++++++++++++ flatland_rviz_plugins/icons/pause.svg | 43 +++++++ .../flatland_rviz_plugins/pause_tool.hpp | 36 ++++++ flatland_rviz_plugins/package.xml | 37 ++++++ flatland_rviz_plugins/plugins_description.xml | 15 +++ flatland_rviz_plugins/src/pause_tool.cpp | 44 +++++++ 6 files changed, 292 insertions(+) create mode 100644 flatland_rviz_plugins/CMakeLists.txt create mode 100644 flatland_rviz_plugins/icons/pause.svg create mode 100644 flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp create mode 100644 flatland_rviz_plugins/package.xml create mode 100644 flatland_rviz_plugins/plugins_description.xml create mode 100644 flatland_rviz_plugins/src/pause_tool.cpp diff --git a/flatland_rviz_plugins/CMakeLists.txt b/flatland_rviz_plugins/CMakeLists.txt new file mode 100644 index 00000000..2934fd27 --- /dev/null +++ b/flatland_rviz_plugins/CMakeLists.txt @@ -0,0 +1,117 @@ +cmake_minimum_required(VERSION 3.5) +project(flatland_rviz_plugins) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + +set(flatland_rviz_plugins_headers_to_moc + include/flatland_rviz_plugins/pause_tool.hpp +) + +include_directories( + include +) + +set(library_name ${PROJECT_NAME}) + +add_library(${library_name} SHARED + src/pause_tool.cpp + ${flatland_rviz_plugins_headers_to_moc} +) + +set(dependencies + geometry_msgs + pluginlib + Qt5 + rclcpp + rclcpp_lifecycle + rviz_common + rviz_default_plugins + rviz_ogre_vendor + rviz_rendering + std_msgs + std_srvs + tf2_geometry_msgs +) + +ament_target_dependencies(${library_name} + ${dependencies} +) + +target_include_directories(${library_name} PUBLIC + ${Qt5Widgets_INCLUDE_DIRS} + ${OGRE_INCLUDE_DIRS} +) + +target_link_libraries(${library_name} + rviz_common::rviz_common +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +# TODO: Make this specific to this project (not rviz default plugins) +target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +install( + TARGETS ${library_name} + EXPORT ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include/ +) + +install( + DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + DESTINATION "share/${PROJECT_NAME}" +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_targets(${library_name} HAS_LIBRARY_TARGET) +ament_export_dependencies( + Qt5 + rviz_common + geometry_msgs + map_msgs + nav_msgs + rclcpp +) + +ament_package() diff --git a/flatland_rviz_plugins/icons/pause.svg b/flatland_rviz_plugins/icons/pause.svg new file mode 100644 index 00000000..5e90dda1 --- /dev/null +++ b/flatland_rviz_plugins/icons/pause.svg @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp new file mode 100644 index 00000000..cfdd89da --- /dev/null +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp @@ -0,0 +1,36 @@ +#ifndef FLATLAND_RVIZ_PLUGINS__PAUSE_TOOL_HPP_ +#define FLATLAND_RVIZ_PLUGINS__PAUSE_TOOL_HPP_ + +#include +#include +#include +#include + +#include + +#include "rviz_default_plugins/visibility_control.hpp" + +namespace flatland_rviz_plugins +{ + +class RVIZ_DEFAULT_PLUGINS_PUBLIC PauseTool : public rviz_common::Tool +{ + Q_OBJECT + +public: + PauseTool(); + ~PauseTool() override; + + void onInitialize() override; + + void activate() override; + void deactivate() override; + +private: + std::shared_ptr node_; + rclcpp::Client::SharedPtr pause_service_; +}; + +} // namespace flatland_rviz_plugins + +#endif // FLATLAND_RVIZ_PLUGINS__PAUSE_TOOL_HPP_ diff --git a/flatland_rviz_plugins/package.xml b/flatland_rviz_plugins/package.xml new file mode 100644 index 00000000..7904c8dd --- /dev/null +++ b/flatland_rviz_plugins/package.xml @@ -0,0 +1,37 @@ + + + + flatland_rviz_plugins + 1.0.0 + Flatland plugins for rviz + João Costa + Apache-2.0 + + ament_cmake + qtbase5-dev + + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + resource_retriever + rviz_common + rviz_default_plugins + rviz_ogre_vendor + rviz_rendering + std_msgs + tf2_geometry_msgs + visualization_msgs + + libqt5-core + libqt5-gui + libqt5-opengl + libqt5-widgets + + ament_lint_common + ament_lint_auto + + + ament_cmake + + diff --git a/flatland_rviz_plugins/plugins_description.xml b/flatland_rviz_plugins/plugins_description.xml new file mode 100644 index 00000000..2719ca81 --- /dev/null +++ b/flatland_rviz_plugins/plugins_description.xml @@ -0,0 +1,15 @@ + + + + + + + Toggle the pause state of the simulation + + + + diff --git a/flatland_rviz_plugins/src/pause_tool.cpp b/flatland_rviz_plugins/src/pause_tool.cpp new file mode 100644 index 00000000..4a5157e6 --- /dev/null +++ b/flatland_rviz_plugins/src/pause_tool.cpp @@ -0,0 +1,44 @@ +#include "flatland_rviz_plugins/pause_tool.hpp" + +#include +#include +#include +#include + +namespace flatland_rviz_plugins +{ + +PauseTool::PauseTool() +{ + shortcut_key_ = 'p'; +} + +PauseTool::~PauseTool() +{ +} + +void PauseTool::onInitialize() +{ + node_ = rclcpp::Node::make_shared("pause_sim_tool"); + pause_service_ = node_->create_client("toggle_pause"); + + setName("Toggle Pause"); + setIcon(rviz_common::loadPixmap("package://flatland_rviz_plugins/icons/pause.svg")); +} + +void PauseTool::activate() { + auto request = std::make_shared(); + auto result = pause_service_->async_send_request(request); + rclcpp::spin_until_future_complete(node_, result); + + auto tool_man = context_->getToolManager(); + auto dflt_tool = tool_man->getDefaultTool(); + tool_man->setCurrentTool(dflt_tool); +} + +void PauseTool::deactivate() {} + +} // namespace flatland_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(flatland_rviz_plugins::PauseTool, rviz_common::Tool) From 2158e7939447cfaead33da1803f6076ffa958898 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Wed, 30 Nov 2022 19:13:03 +0000 Subject: [PATCH 41/66] Add custom interact plugin that toggles the interactive markers. --- flatland_rviz_plugins/CMakeLists.txt | 2 +- .../interactive_tool.hpp | 23 ++++++++ flatland_rviz_plugins/plugins_description.xml | 12 +++- .../src/interactive_tool.cpp | 56 +++++++++++++++++++ 4 files changed, 90 insertions(+), 3 deletions(-) create mode 100644 flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp create mode 100644 flatland_rviz_plugins/src/interactive_tool.cpp diff --git a/flatland_rviz_plugins/CMakeLists.txt b/flatland_rviz_plugins/CMakeLists.txt index 2934fd27..d75792ad 100644 --- a/flatland_rviz_plugins/CMakeLists.txt +++ b/flatland_rviz_plugins/CMakeLists.txt @@ -31,7 +31,7 @@ find_package(visualization_msgs REQUIRED) set(flatland_rviz_plugins_headers_to_moc include/flatland_rviz_plugins/pause_tool.hpp -) + src/interactive_tool.cpp include/flatland_rviz_plugins/interactive_tool.hpp) include_directories( include diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp new file mode 100644 index 00000000..7ae5c646 --- /dev/null +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp @@ -0,0 +1,23 @@ +#ifndef FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ +#define FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ + +#include +#include +#include + +namespace flatland_rviz_plugins { +class RVIZ_DEFAULT_PLUGINS_PUBLIC InteractiveTool : public rviz_default_plugins::tools::InteractionTool { + public: + InteractiveTool(); + + void onInitialize() override; + + void activate() override; + void deactivate() override; + + private: + void enableMarkers(bool is_enabled); +}; +} + +#endif //FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ diff --git a/flatland_rviz_plugins/plugins_description.xml b/flatland_rviz_plugins/plugins_description.xml index 2719ca81..6411f77d 100644 --- a/flatland_rviz_plugins/plugins_description.xml +++ b/flatland_rviz_plugins/plugins_description.xml @@ -1,7 +1,5 @@ - - + + + Toggle the pause state of the simulation + + + diff --git a/flatland_rviz_plugins/src/interactive_tool.cpp b/flatland_rviz_plugins/src/interactive_tool.cpp new file mode 100644 index 00000000..6b5651b9 --- /dev/null +++ b/flatland_rviz_plugins/src/interactive_tool.cpp @@ -0,0 +1,56 @@ +#include "include/flatland_rviz_plugins/interactive_tool.hpp" + +#include +#include +#include +#include + +namespace flatland_rviz_plugins { + +InteractiveTool::InteractiveTool() : rviz_default_plugins::tools::InteractionTool() { + /* + hide_inactive_property_ = std::make_unique( + "Hide Inactive Objects", + true, + "While holding down a mouse button, hide all other Interactive Objects.", + getPropertyContainer(), + SLOT(hideInactivePropertyChanged()), + this); + */ +} + +void InteractiveTool::enableMarkers(bool is_enabled) { + auto root_display = context_->getRootDisplayGroup(); + for (int i = 0; i < root_display->numDisplays(); ++i) { + auto display = root_display->getDisplayAt(i); + if (display->getClassId() == "rviz_default_plugins/InteractiveMarkers") { + display->setEnabled(is_enabled); + } + } +} + +void InteractiveTool::onInitialize() { + rviz_default_plugins::tools::InteractionTool::onInitialize(); + + this->enableMarkers(false); + + setName("Interact"); + setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/Interact.png")); +} + +void InteractiveTool::activate() { + rviz_default_plugins::tools::InteractionTool::activate(); + + this->enableMarkers(true); +} + +void InteractiveTool::deactivate() { + rviz_default_plugins::tools::InteractionTool::deactivate(); + + this->enableMarkers(false); +} + +} + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(flatland_rviz_plugins::InteractiveTool, rviz_common::Tool) From 5b1e8a277c9e8d025ae290f85fc05c8d5b21f290 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Wed, 30 Nov 2022 19:26:20 +0000 Subject: [PATCH 42/66] Change pause tool keybind to space. This avoids the conflict with the pose estimate 2d tool. --- flatland_rviz_plugins/src/pause_tool.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flatland_rviz_plugins/src/pause_tool.cpp b/flatland_rviz_plugins/src/pause_tool.cpp index 4a5157e6..ab652abc 100644 --- a/flatland_rviz_plugins/src/pause_tool.cpp +++ b/flatland_rviz_plugins/src/pause_tool.cpp @@ -10,7 +10,7 @@ namespace flatland_rviz_plugins PauseTool::PauseTool() { - shortcut_key_ = 'p'; + shortcut_key_ = ' '; } PauseTool::~PauseTool() From da46a4a8e8223e6b69ed2a625667570ed96e66cc Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Wed, 30 Nov 2022 20:41:29 +0000 Subject: [PATCH 43/66] Delete useless comment. --- flatland_rviz_plugins/src/interactive_tool.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/flatland_rviz_plugins/src/interactive_tool.cpp b/flatland_rviz_plugins/src/interactive_tool.cpp index 6b5651b9..646c73b8 100644 --- a/flatland_rviz_plugins/src/interactive_tool.cpp +++ b/flatland_rviz_plugins/src/interactive_tool.cpp @@ -8,15 +8,6 @@ namespace flatland_rviz_plugins { InteractiveTool::InteractiveTool() : rviz_default_plugins::tools::InteractionTool() { - /* - hide_inactive_property_ = std::make_unique( - "Hide Inactive Objects", - true, - "While holding down a mouse button, hide all other Interactive Objects.", - getPropertyContainer(), - SLOT(hideInactivePropertyChanged()), - this); - */ } void InteractiveTool::enableMarkers(bool is_enabled) { From 4b9c21fd762587d940338a44fe37845578f3a926 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Wed, 30 Nov 2022 20:42:12 +0000 Subject: [PATCH 44/66] Add spawn model tool as rviz plugin. --- flatland_rviz_plugins/CMakeLists.txt | 18 +- .../spawn_model_tool.hpp | 171 ++++++++ flatland_rviz_plugins/package.xml | 4 +- flatland_rviz_plugins/plugins_description.xml | 12 +- .../src/load_model_dialog.cpp | 216 ++++++++++ .../src/load_model_dialog.hpp | 135 ++++++ .../src/spawn_model_tool.cpp | 391 ++++++++++++++++++ 7 files changed, 944 insertions(+), 3 deletions(-) create mode 100644 flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp create mode 100644 flatland_rviz_plugins/src/load_model_dialog.cpp create mode 100644 flatland_rviz_plugins/src/load_model_dialog.hpp create mode 100644 flatland_rviz_plugins/src/spawn_model_tool.cpp diff --git a/flatland_rviz_plugins/CMakeLists.txt b/flatland_rviz_plugins/CMakeLists.txt index d75792ad..6819b686 100644 --- a/flatland_rviz_plugins/CMakeLists.txt +++ b/flatland_rviz_plugins/CMakeLists.txt @@ -28,10 +28,19 @@ find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(flatland_server REQUIRED) +find_package(flatland_msgs REQUIRED) + +# lua5.1 +find_package(Lua 5.1 REQUIRED) + +# Boost +find_package(Boost REQUIRED COMPONENTS date_time system filesystem) set(flatland_rviz_plugins_headers_to_moc include/flatland_rviz_plugins/pause_tool.hpp - src/interactive_tool.cpp include/flatland_rviz_plugins/interactive_tool.hpp) + include/flatland_rviz_plugins/interactive_tool.hpp + include/flatland_rviz_plugins/spawn_model_tool.hpp) include_directories( include @@ -41,6 +50,9 @@ set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED src/pause_tool.cpp + src/interactive_tool.cpp + src/spawn_model_tool.cpp + src/load_model_dialog.cpp ${flatland_rviz_plugins_headers_to_moc} ) @@ -57,6 +69,8 @@ set(dependencies std_msgs std_srvs tf2_geometry_msgs + flatland_server + flatland_msgs ) ament_target_dependencies(${library_name} @@ -69,6 +83,8 @@ target_include_directories(${library_name} PUBLIC ) target_link_libraries(${library_name} + ${Boost_LIBRARIES} + ${Boost_FILESYSTEM_LIBRARY} rviz_common::rviz_common ) diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp new file mode 100644 index 00000000..97219def --- /dev/null +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp @@ -0,0 +1,171 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name spawn_model_tool.h + * @brief Rviz compatible tool for spawning flatland model + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_RVIZ_PLUGINS__SPAWN_MODEL_TOOL_HPP_ +#define FLATLAND_RVIZ_PLUGINS__SPAWN_MODEL_TOOL_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + + +namespace flatland_rviz_plugins { +/** + * @name SpawnModelTool + * @brief Every tool which can be added to the tool bar is a + * subclass of rviz_common::Tool. + */ +class SpawnModelTool : public rviz_common::Tool { + Q_OBJECT + + public: + SpawnModelTool(); + ~SpawnModelTool() override; + + /** + * @name BeginPlacement + * @brief Begin the placement phase, model follows cursor, left + * click deposits model and starts rotation phase + */ + void BeginPlacement(); + /** + * @name SavePath + * @brief Called from dialog to save path + */ + void SavePath(QString p); + /** + * @name SaveName + * @brief Called from dialog to save name + */ + void SaveName(QString n); + /** + * @name SpawnModelInFlatland + * @brief Spawns a model using ros service + */ + void SpawnModelInFlatland(); + + private: + /** + * @name onInitialize + * @brief Initializes tools currently loaded when rviz starts + */ + void onInitialize() override; + /** + virtual void activate(); + * @name activate + * @brief Launch the model dialog + */ + void activate() override; + /** + * @name deactivate + * @brief Cleanup when tool is removed + */ + void deactivate() override; + /** + * @name processMouseEvent + * @brief Main loop of tool + * @param event Mouse event + */ + int processMouseEvent(rviz_common::ViewportMouseEvent &event) override; + /** + * @name SetMovingModelColor + * @brief Set the color of the moving model + * @param c QColor to set the 3d model + */ + void SetMovingModelColor(QColor c); + /** + * @name LoadPreview + * @brief Load a vector preview of the model + */ + void LoadPreview(); + /** + * @name LoadPolygonFootprint + * @brief Load a vector preview of the model's polygon footprint + * @param footprint The footprint yaml node + */ + void LoadPolygonFootprint(flatland_server::YamlReader &footprint); + /** + * @name LoadCircleFootprint + * @brief Load a vector preview of the model's circle footprint + * @param footprint The footprint yaml node + */ + void LoadCircleFootprint(flatland_server::YamlReader &footprint); + + Ogre::Vector3 + intersection; // location cursor intersects ground plane, ie the + // location to create the model + float initial_angle; // the angle to create the model at + Ogre::SceneNode *moving_model_node_; // the node for the 3D object + enum ModelState { m_hidden, m_dragging, m_rotating }; + ModelState model_state; // model state, first hidden, then dragging to + // intersection point, then rotating to desired angle + static QString path_to_model_file_; // full path to model file (yaml) + static QString model_name; // base file name with path and extension removed + + protected: + rviz_rendering::Arrow *arrow_; // Rviz 3d arrow to show axis of rotation + rclcpp::Client::SharedPtr client; + std::vector> lines_list_; +}; + +} // end namespace flatland_rviz_plugins + +#endif // FLATLAND_RVIZ_PLUGINS__SPAWN_MODEL_TOOL_HPP_ \ No newline at end of file diff --git a/flatland_rviz_plugins/package.xml b/flatland_rviz_plugins/package.xml index 7904c8dd..fb8d4807 100644 --- a/flatland_rviz_plugins/package.xml +++ b/flatland_rviz_plugins/package.xml @@ -22,7 +22,9 @@ std_msgs tf2_geometry_msgs visualization_msgs - + flatland_server + flatland_msgs + libqt5-core libqt5-gui libqt5-opengl diff --git a/flatland_rviz_plugins/plugins_description.xml b/flatland_rviz_plugins/plugins_description.xml index 6411f77d..db5bbb18 100644 --- a/flatland_rviz_plugins/plugins_description.xml +++ b/flatland_rviz_plugins/plugins_description.xml @@ -16,7 +16,17 @@ base_class_type="rviz_common::Tool" > - Toggle the pause state of the simulation + Allows interacting with interactive markers + + + + + + Spawns a model diff --git a/flatland_rviz_plugins/src/load_model_dialog.cpp b/flatland_rviz_plugins/src/load_model_dialog.cpp new file mode 100644 index 00000000..f64a36c4 --- /dev/null +++ b/flatland_rviz_plugins/src/load_model_dialog.cpp @@ -0,0 +1,216 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name load_model_dialog.cpp + * @brief Spawn dialog + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "load_model_dialog.hpp" + +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace flatland_rviz_plugins { + +QString LoadModelDialog::path_to_model_file; +int LoadModelDialog::count; +bool LoadModelDialog::numbering; + +LoadModelDialog::LoadModelDialog(QWidget *parent, + SpawnModelTool *tool) + : QDialog(parent), tool_(tool) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "ModelDialog::ModelDialog"); + auto *v_layout = new QVBoxLayout; + setLayout(v_layout); + + // we are injecting horizontal layouts into the master vertical layout + auto *h0_layout = new QHBoxLayout; + auto *h1_layout = new QHBoxLayout; + auto *h2_layout = new QHBoxLayout; + auto *h3_layout = new QHBoxLayout; + + // create widgets + auto *pathButton = new QPushButton("choose file"); + p_label = new QLabel; + n_checkbox = new QCheckBox; + n_edit = new QLineEdit; + auto *okButton = new QPushButton("ok"); + auto *cancelButton = new QPushButton("cancel"); + + // set focus policy, otherwise cr in textfield triggers all the slots + pathButton->setFocusPolicy(Qt::NoFocus); + p_label->setFocusPolicy(Qt::NoFocus); + n_checkbox->setFocusPolicy(Qt::NoFocus); + n_edit->setFocusPolicy(Qt::ClickFocus); // only name gets focus + okButton->setFocusPolicy(Qt::NoFocus); + cancelButton->setFocusPolicy(Qt::NoFocus); + + connect(pathButton, &QAbstractButton::clicked, this, + &LoadModelDialog::on_PathButtonClicked); + connect(okButton, &QAbstractButton::clicked, this, + &LoadModelDialog::OkButtonClicked); + connect(cancelButton, &QAbstractButton::clicked, this, + &LoadModelDialog::CancelButtonClicked); + connect(n_checkbox, &QAbstractButton::clicked, this, + &LoadModelDialog::NumberCheckBoxChanged); + + // path button + h0_layout->addWidget(pathButton); + + // path label + p_label->setText(path_to_model_file); + h1_layout->addWidget(new QLabel("path:")); + h1_layout->addWidget(p_label); + + // + h2_layout->addWidget(new QLabel("number:")); + h2_layout->addWidget(n_checkbox); + n_checkbox->setChecked(numbering); + h2_layout->addWidget(new QLabel("name:")); + h2_layout->addWidget(n_edit); + + // set the default name to the filename parsed using boost + AddNumberAndUpdateName(); + + // ok button + h3_layout->addWidget(okButton); + + // cancel button + h3_layout->addWidget(cancelButton); + + // add the horizontal layouts to the vertical layout + v_layout->addLayout(h0_layout); + v_layout->addLayout(h1_layout); + v_layout->addLayout(h2_layout); + v_layout->addLayout(h3_layout); + + // set the top level layout + setLayout(v_layout); + + // delete the Dialog if the user clicks on the x to close window + this->setAttribute(Qt::WA_DeleteOnClose, true); +} + +void LoadModelDialog::CancelButtonClicked() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::CancelButtonClicked"); + this->close(); +} + +void LoadModelDialog::OkButtonClicked() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::OkButtonClicked"); + + QString name = n_edit->displayText(); + + tool_->SaveName(name); + tool_->SavePath(path_to_model_file); + tool_->BeginPlacement(); + + this->close(); +} + +void LoadModelDialog::AddNumberAndUpdateName() { + std::string bsfn = + boost::filesystem::basename(path_to_model_file.toStdString()); + QString name = QString::fromStdString(bsfn); + + + if (numbering) { + name = name.append(QString::number(count++)); + } + + // update the name text field + n_edit->setText(name); +} + +void LoadModelDialog::on_PathButtonClicked() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::on_PathButtonClicked"); + path_to_model_file = ChooseFile(); + + AddNumberAndUpdateName(); + p_label->setText(path_to_model_file); + n_edit->setFocus(); +} + +void LoadModelDialog::NumberCheckBoxChanged(bool i) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "NumberCheckBoxChanged " << i); + numbering = !numbering; + AddNumberAndUpdateName(); +} + +QString LoadModelDialog::ChooseFile() { + QString fileName = + QFileDialog::getOpenFileName(NULL, tr("Open model file"), "", ""); + if (fileName.isEmpty()) + return fileName; + else { + QFile file(fileName); + + if (!file.open(QIODevice::ReadOnly)) { + QMessageBox::information(NULL, tr("Unable to open file"), + file.errorString()); + return fileName; + } + file.close(); + + return fileName; + } +} + +} diff --git a/flatland_rviz_plugins/src/load_model_dialog.hpp b/flatland_rviz_plugins/src/load_model_dialog.hpp new file mode 100644 index 00000000..55303abd --- /dev/null +++ b/flatland_rviz_plugins/src/load_model_dialog.hpp @@ -0,0 +1,135 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name load_model_dialog.h + * @brief Spawn dialog + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ +#define FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include "flatland_rviz_plugins/spawn_model_tool.hpp" + +namespace flatland_rviz_plugins { + +class LoadModelDialog : public QDialog { + public: + /** + * @name LoadModelDialog + * @brief Launch load model dialog + * @param parent, parent widget pointer + * @param tool, pointer to this so dialog can call methods + */ + LoadModelDialog(QWidget *parent, SpawnModelTool *tool); + + private: + /** + * @name ChooseFile + * @brief Launch file selection dialog + */ + QString ChooseFile(); + /** + * @name AddNumberAndUpdateName + * @brief Add numbering to name and show in the name widget + */ + void AddNumberAndUpdateName(); + + static QString path_to_model_file; // full path to model file + static int count; // counter for adding unique number to filename + static bool numbering; // flag to use unique numbering + + SpawnModelTool *tool_; + QLineEdit *n_edit; // name lineEdit widget + QLabel *p_label; // path label widget + QCheckBox *n_checkbox; // checkbox widget + + public Q_SLOTS: + /** + * @name NumberCheckBoxChanged + * @brief Checkbox was clicked, toggle numbering of names + */ + void NumberCheckBoxChanged(bool value); + /** + * @name CancelButtonClicked + * @brief Cancel button was clicked, dismiss dialog + */ + void CancelButtonClicked(); + /** + * @name OkButtonClicked + * @brief Ok button was clicked, begin placement + */ + void OkButtonClicked(); + /** + * @name PathButtonClicked + * @brief Path button was clicked, launch file selection dialog + */ + void on_PathButtonClicked(); +}; + +} + +#endif // FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ diff --git a/flatland_rviz_plugins/src/spawn_model_tool.cpp b/flatland_rviz_plugins/src/spawn_model_tool.cpp new file mode 100644 index 00000000..984c3dee --- /dev/null +++ b/flatland_rviz_plugins/src/spawn_model_tool.cpp @@ -0,0 +1,391 @@ +/* + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2018 Avidbots Corp. + * @name spawn_model_tool.cpp + * @brief Rviz compatible tool for spawning flatland model + * @author Joseph Duchesne + * @author Mike Brousseau + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Avidbots Corp. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "flatland_rviz_plugins/spawn_model_tool.hpp" +#include "load_model_dialog.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include + +namespace flatland_rviz_plugins { +QString SpawnModelTool::path_to_model_file_; +QString SpawnModelTool::model_name; + +// Set the "shortcut_key_" member variable defined in the +// superclass to declare which key will activate the tool. +SpawnModelTool::SpawnModelTool() : moving_model_node_(nullptr) { + shortcut_key_ = 'm'; +} + +// The destructor destroys the Ogre scene nodes for the models so they +// disappear from the 3D scene. The destructor for a Tool subclass is +// only called when the tool is removed from the toolbar with the "-" +// button. +SpawnModelTool::~SpawnModelTool() { + scene_manager_->destroySceneNode(arrow_->getSceneNode()); + scene_manager_->destroySceneNode(moving_model_node_); +} + +// onInitialize() is called by the superclass after scene_manager_ and +// context_ are set. It should be called only once per instantiation. +// This is where most one-time initialization work should be done. +// onInitialize() is called during initial instantiation of the tool +// object. At this point the tool has not been activated yet, so any +// scene objects created should be invisible or disconnected from the +// scene at this point. +// +// In this case we load a mesh object with the shape and appearance of +// the model, create an Ogre::SceneNode for the moving model, and then +// set it invisible. +void SpawnModelTool::onInitialize() { + // hide 3d model until the user clicks the span model tool button + model_state = m_hidden; + + // make an arrow to show axis of rotation + arrow_ = new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); + arrow_->setColor(0.0f, 0.0f, 1.0f, 1.0f); // blue + arrow_->getSceneNode()->setVisible( + false); // will only be visible during rotation phase + + // set arrow to point up (along z) + Ogre::Quaternion orientation(Ogre::Radian(M_PI), Ogre::Vector3(1, 0, 0)); + arrow_->setOrientation(orientation); + + // create an Ogre child scene node for rendering the preview outline + moving_model_node_ = + scene_manager_->getRootSceneNode()->createChildSceneNode(); + moving_model_node_->setVisible(false); + + SetMovingModelColor(Qt::green); + + setName("Load Model"); + setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/RobotModel.png")); +} + +// Activate() is called when the tool is started by the user, either +// by clicking on its button in the toolbar or by pressing its hotkey. +// +// First we set the moving model node to be visible, then we create an +// rviz::VectorProperty to show the user the position of the model. +// Unlike rviz::Display, rviz::Tool is not a subclass of +// rviz::Property, so when we want to add a tool property we need to +// get the parent container with getPropertyContainer() and add it to +// that. +// +// We wouldn't have to set current_model_property_ to be read-only, but +// if it were writable the model should really change position when the +// user edits the property. This is a fine idea, and is possible, but +// is left as an exercise for the reader. +void SpawnModelTool::activate() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::activate "); + + LoadModelDialog *model_dialog = new LoadModelDialog(nullptr, this); + model_dialog->setModal(true); + model_dialog->show(); +} + +void SpawnModelTool::BeginPlacement() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::BeginPlacement"); + model_state = m_dragging; + + if (moving_model_node_) { + moving_model_node_->setVisible(true); + } +} + +// deactivate() is called when the tool is being turned off because +// another tool has been chosen. +// +// We make the moving model invisible, then delete the current model +// property. Deleting a property also removes it from its parent +// property, so that doesn't need to be done in a separate step. If +// we didn't delete it here, it would stay in the list of models when +// we switch to another tool. +void SpawnModelTool::deactivate() { + if (moving_model_node_) { + moving_model_node_->setVisible(false); + } +} + +void SpawnModelTool::SpawnModelInFlatland() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), + "SpawnModelTool::SpawnModelInFlatland name:" << model_name.toStdString()); + auto srv = std::make_shared(); + + // model names can not have embeded period char + model_name = model_name.replace(".", "_", Qt::CaseSensitive); + + // fill in the service request + srv->name = model_name.toStdString(); + srv->ns = model_name.toStdString(); + srv->yaml_path = path_to_model_file_.toStdString(); + srv->pose.x = intersection[0]; + srv->pose.y = intersection[1]; + srv->pose.theta = initial_angle; + + std::shared_ptr node = rclcpp::Node::make_shared("spawn_model_tool"); // TODO + client = node->create_client("spawn_model"); + + // make ros service call + auto result = client->async_send_request(srv); + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + auto response = result.get(); + if (!response->success) { + QMessageBox msgBox; + msgBox.setText(response->message.c_str()); + msgBox.exec(); + } + } else { + QMessageBox msgBox; + msgBox.setText("Error: You must have a client running."); + msgBox.exec(); + } +} + +void SpawnModelTool::SetMovingModelColor(QColor c) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::SetMovingModelColor"); + + try { + auto *m_pEntity = + dynamic_cast(moving_model_node_->getAttachedObject(0)); + const Ogre::MaterialPtr m_pMat = m_pEntity->getSubEntity(0)->getMaterial(); + m_pMat->getTechnique(0)->getPass(0)->setAmbient(1, 0, 0); + m_pMat->getTechnique(0)->getPass(0)->setDiffuse(c.redF(), c.greenF(), + c.blueF(), 0); + } catch (Ogre::InvalidParametersException &e) { + //RCLCPP_WARN("Invalid preview model"); + } +} + +// processMouseEvent() is sort of the main function of a Tool, because +// mouse interactions are the point of Tools. +// +// We use the utility function rviz_rendering::getPointOnPlaneFromWindowXY() to +// see where on the ground plane the user's mouse is pointing, then +// move the moving model to that point and update the VectorProperty. +// +// If this mouse event was a left button press, we want to save the +// current model location. Therefore we make a new model at the same +// place and drop the pointer to the VectorProperty. Dropping the +// pointer means when the tool is deactivated the VectorProperty won't +// be deleted, which is what we want. +int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { + if (!moving_model_node_) { + return Render; + } + + Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); + + if (model_state == m_dragging) { + auto intersectionPair = rviz_rendering::ViewportProjectionFinder().getViewportPointProjectionOnXYPlane(event.panel->getRenderWindow(), + event.x, + event.y); + intersection = intersectionPair.second; + if (intersectionPair.first) { + moving_model_node_->setVisible(true); + moving_model_node_->setPosition(intersection); + + if (event.leftDown()) { + model_state = m_rotating; + arrow_->getSceneNode()->setVisible(true); + arrow_->setPosition(intersection); + + return Render; + } + } else { + moving_model_node_->setVisible( + false); // If the mouse is not pointing at the + // ground plane, don't show the model. + } + } + if (model_state == m_rotating) { // model_state is m_rotating + auto intersectionPair = rviz_rendering::ViewportProjectionFinder().getViewportPointProjectionOnXYPlane(event.panel->getRenderWindow(), + event.x, + event.y); + Ogre::Vector3 intersection2 = intersectionPair.second; + if (intersectionPair.first) { + if (event.leftDown()) { + model_state = m_hidden; + arrow_->getSceneNode()->setVisible(false); + + // change Vector3 from x,y,z to x,y,angle + // z is zero since we are intersecting with the ground + intersection[2] = initial_angle; + + SpawnModelInFlatland(); + + return Render | Finished; + } + moving_model_node_->setVisible(true); + moving_model_node_->setPosition(intersection); + Ogre::Vector3 dir = intersection2 - intersection; + initial_angle = atan2(dir.y, dir.x); + Ogre::Quaternion orientation(Ogre::Radian(initial_angle), + Ogre::Vector3(0, 0, 1)); + moving_model_node_->setOrientation(orientation); + } + } + return Render; +} + +void SpawnModelTool::LoadPreview() { + // Clear the old preview, if there is one + moving_model_node_->removeAllChildren(); + lines_list_.clear(); + + std::shared_ptr node = rclcpp::Node::make_shared("spawn_model_tool"); // TODO + + // Load the bodies list into a model object + flatland_server::YamlReader reader(node, path_to_model_file_.toStdString()); + + try { + flatland_server::YamlReader bodies_reader = + reader.Subnode("bodies", flatland_server::YamlReader::LIST); + // Iterate each body and add to the preview + for (int i = 0; i < bodies_reader.NodeSize(); i++) { + flatland_server::YamlReader body_reader = + bodies_reader.Subnode(i, flatland_server::YamlReader::MAP); + if (!body_reader.Get("enabled", "true")) { // skip if disabled + continue; + } + + auto pose = body_reader.GetPose("pose", flatland_server::Pose()); + + flatland_server::YamlReader footprints_node = + body_reader.Subnode("footprints", flatland_server::YamlReader::LIST); + for (int j = 0; j < footprints_node.NodeSize(); j++) { + flatland_server::YamlReader footprint = + footprints_node.Subnode(j, flatland_server::YamlReader::MAP); + + lines_list_.push_back(std::make_shared( + context_->getSceneManager(), moving_model_node_)); + auto lines = lines_list_.back(); + lines->setColor(0.0, 1.0, 0.0, 0.75); // Green + lines->setLineWidth(0.05); + lines->setOrientation( + Ogre::Quaternion(Ogre::Radian(pose.theta), Ogre::Vector3(0, 0, 1))); + lines->setPosition(Ogre::Vector3(pose.x, pose.y, 0)); + + auto type = footprint.Get("type"); + if (type == "circle") { + LoadCircleFootprint(footprint); + } else if (type == "polygon") { + LoadPolygonFootprint(footprint); + } else { + throw flatland_server::YAMLException("Invalid footprint \"type\""); + } + } + } + } catch (const flatland_server::YAMLException &e) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("SpawnModelTool"), "Couldn't load model bodies for preview" << e.what()); + } +} + +void SpawnModelTool::LoadPolygonFootprint(flatland_server::YamlReader &footprint) { + auto lines = lines_list_.back(); + auto points = footprint.GetList("points", 3, + b2_maxPolygonVertices); + for (auto p : points) { + lines->addPoint(Ogre::Vector3(p.x, p.y, 0.)); + } + if (points.size() > 0) { + lines->addPoint(Ogre::Vector3(points.at(0).x, points.at(0).y, 0.)); // Close the box + } +} + +void SpawnModelTool::LoadCircleFootprint(flatland_server::YamlReader &footprint) { + auto lines = lines_list_.back(); + auto center = footprint.GetVec2("center", flatland_server::Vec2()); + auto radius = footprint.Get("radius", 1.0); + + for (float a = 0.; a < M_PI * 2.0; a += M_PI / 8.) { // 16 point circle + lines->addPoint(Ogre::Vector3(center.x + radius * cos(a), + center.y + radius * sin(a), 0.)); + } + lines->addPoint( + Ogre::Vector3(center.x + radius, center.y, 0.)); // close the loop +} + +void SpawnModelTool::SavePath(QString p) { + path_to_model_file_ = p; + LoadPreview(); +} + +void SpawnModelTool::SaveName(QString n) { model_name = n; } + +// At the end of every plugin class implementation, we end the +// namespace and then tell pluginlib about the class. It is important +// to do this in global scope, outside our package's namespace. + +} // end namespace flatland_viz + +#include +PLUGINLIB_EXPORT_CLASS(flatland_rviz_plugins::SpawnModelTool, rviz_common::Tool) From 209dce7849263df4f1373f35a838ea2d8b64a4c7 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Wed, 30 Nov 2022 20:49:11 +0000 Subject: [PATCH 45/66] Formated source files. --- .../interactive_tool.hpp | 11 +-- .../flatland_rviz_plugins/pause_tool.hpp | 17 ++-- .../spawn_model_tool.hpp | 38 ++++---- .../src/interactive_tool.cpp | 16 ++-- .../src/load_model_dialog.cpp | 32 +++---- .../src/load_model_dialog.hpp | 27 +++--- flatland_rviz_plugins/src/pause_tool.cpp | 20 ++--- .../src/spawn_model_tool.cpp | 90 +++++++++++-------- 8 files changed, 124 insertions(+), 127 deletions(-) diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp index 7ae5c646..68c409ab 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp @@ -1,12 +1,13 @@ #ifndef FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ -#include -#include #include +#include +#include namespace flatland_rviz_plugins { -class RVIZ_DEFAULT_PLUGINS_PUBLIC InteractiveTool : public rviz_default_plugins::tools::InteractionTool { +class RVIZ_DEFAULT_PLUGINS_PUBLIC InteractiveTool + : public rviz_default_plugins::tools::InteractionTool { public: InteractiveTool(); @@ -18,6 +19,6 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC InteractiveTool : public rviz_default_plugins: private: void enableMarkers(bool is_enabled); }; -} +} // namespace flatland_rviz_plugins -#endif //FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ +#endif // FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp index cfdd89da..14fbff27 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp @@ -1,23 +1,20 @@ #ifndef FLATLAND_RVIZ_PLUGINS__PAUSE_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__PAUSE_TOOL_HPP_ -#include -#include -#include #include - #include +#include +#include +#include #include "rviz_default_plugins/visibility_control.hpp" -namespace flatland_rviz_plugins -{ +namespace flatland_rviz_plugins { -class RVIZ_DEFAULT_PLUGINS_PUBLIC PauseTool : public rviz_common::Tool -{ +class RVIZ_DEFAULT_PLUGINS_PUBLIC PauseTool : public rviz_common::Tool { Q_OBJECT -public: + public: PauseTool(); ~PauseTool() override; @@ -26,7 +23,7 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC PauseTool : public rviz_common::Tool void activate() override; void deactivate() override; -private: + private: std::shared_ptr node_; rclcpp::Client::SharedPtr pause_service_; }; diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp index 97219def..fb1ba834 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp @@ -48,24 +48,19 @@ #ifndef FLATLAND_RVIZ_PLUGINS__SPAWN_MODEL_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__SPAWN_MODEL_TOOL_HPP_ -#include -#include -#include - -#include - #include #include #include #include - #include -#include +#include +#include #include -#include +#include #include - +#include +#include namespace flatland_rviz_plugins { /** @@ -97,9 +92,9 @@ class SpawnModelTool : public rviz_common::Tool { */ void SaveName(QString n); /** - * @name SpawnModelInFlatland - * @brief Spawns a model using ros service - */ + * @name SpawnModelInFlatland + * @brief Spawns a model using ros service + */ void SpawnModelInFlatland(); private: @@ -126,10 +121,10 @@ class SpawnModelTool : public rviz_common::Tool { */ int processMouseEvent(rviz_common::ViewportMouseEvent &event) override; /** - * @name SetMovingModelColor - * @brief Set the color of the moving model - * @param c QColor to set the 3d model - */ + * @name SetMovingModelColor + * @brief Set the color of the moving model + * @param c QColor to set the 3d model + */ void SetMovingModelColor(QColor c); /** * @name LoadPreview @@ -149,10 +144,9 @@ class SpawnModelTool : public rviz_common::Tool { */ void LoadCircleFootprint(flatland_server::YamlReader &footprint); - Ogre::Vector3 - intersection; // location cursor intersects ground plane, ie the - // location to create the model - float initial_angle; // the angle to create the model at + Ogre::Vector3 intersection; // location cursor intersects ground plane, ie + // the location to create the model + float initial_angle; // the angle to create the model at Ogre::SceneNode *moving_model_node_; // the node for the 3D object enum ModelState { m_hidden, m_dragging, m_rotating }; ModelState model_state; // model state, first hidden, then dragging to @@ -161,7 +155,7 @@ class SpawnModelTool : public rviz_common::Tool { static QString model_name; // base file name with path and extension removed protected: - rviz_rendering::Arrow *arrow_; // Rviz 3d arrow to show axis of rotation + rviz_rendering::Arrow *arrow_; // Rviz 3d arrow to show axis of rotation rclcpp::Client::SharedPtr client; std::vector> lines_list_; }; diff --git a/flatland_rviz_plugins/src/interactive_tool.cpp b/flatland_rviz_plugins/src/interactive_tool.cpp index 646c73b8..f09437ce 100644 --- a/flatland_rviz_plugins/src/interactive_tool.cpp +++ b/flatland_rviz_plugins/src/interactive_tool.cpp @@ -1,14 +1,14 @@ #include "include/flatland_rviz_plugins/interactive_tool.hpp" -#include #include -#include #include +#include +#include namespace flatland_rviz_plugins { -InteractiveTool::InteractiveTool() : rviz_default_plugins::tools::InteractionTool() { -} +InteractiveTool::InteractiveTool() + : rviz_default_plugins::tools::InteractionTool() {} void InteractiveTool::enableMarkers(bool is_enabled) { auto root_display = context_->getRootDisplayGroup(); @@ -26,7 +26,8 @@ void InteractiveTool::onInitialize() { this->enableMarkers(false); setName("Interact"); - setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/Interact.png")); + setIcon(rviz_common::loadPixmap( + "package://rviz_default_plugins/icons/classes/Interact.png")); } void InteractiveTool::activate() { @@ -41,7 +42,8 @@ void InteractiveTool::deactivate() { this->enableMarkers(false); } -} +} // namespace flatland_rviz_plugins #include // NOLINT -PLUGINLIB_EXPORT_CLASS(flatland_rviz_plugins::InteractiveTool, rviz_common::Tool) +PLUGINLIB_EXPORT_CLASS(flatland_rviz_plugins::InteractiveTool, + rviz_common::Tool) diff --git a/flatland_rviz_plugins/src/load_model_dialog.cpp b/flatland_rviz_plugins/src/load_model_dialog.cpp index f64a36c4..a6ac6222 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.cpp +++ b/flatland_rviz_plugins/src/load_model_dialog.cpp @@ -49,12 +49,6 @@ #include -#include - -#include -#include -#include - #include #include #include @@ -64,8 +58,11 @@ #include #include #include - #include +#include +#include +#include +#include namespace flatland_rviz_plugins { @@ -73,10 +70,10 @@ QString LoadModelDialog::path_to_model_file; int LoadModelDialog::count; bool LoadModelDialog::numbering; -LoadModelDialog::LoadModelDialog(QWidget *parent, - SpawnModelTool *tool) +LoadModelDialog::LoadModelDialog(QWidget *parent, SpawnModelTool *tool) : QDialog(parent), tool_(tool) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "ModelDialog::ModelDialog"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), + "ModelDialog::ModelDialog"); auto *v_layout = new QVBoxLayout; setLayout(v_layout); @@ -149,12 +146,14 @@ LoadModelDialog::LoadModelDialog(QWidget *parent, } void LoadModelDialog::CancelButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::CancelButtonClicked"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), + "LoadModelDialog::CancelButtonClicked"); this->close(); } void LoadModelDialog::OkButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::OkButtonClicked"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), + "LoadModelDialog::OkButtonClicked"); QString name = n_edit->displayText(); @@ -170,7 +169,6 @@ void LoadModelDialog::AddNumberAndUpdateName() { boost::filesystem::basename(path_to_model_file.toStdString()); QString name = QString::fromStdString(bsfn); - if (numbering) { name = name.append(QString::number(count++)); } @@ -180,7 +178,8 @@ void LoadModelDialog::AddNumberAndUpdateName() { } void LoadModelDialog::on_PathButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::on_PathButtonClicked"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), + "LoadModelDialog::on_PathButtonClicked"); path_to_model_file = ChooseFile(); AddNumberAndUpdateName(); @@ -189,7 +188,8 @@ void LoadModelDialog::on_PathButtonClicked() { } void LoadModelDialog::NumberCheckBoxChanged(bool i) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "NumberCheckBoxChanged " << i); + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), + "NumberCheckBoxChanged " << i); numbering = !numbering; AddNumberAndUpdateName(); } @@ -213,4 +213,4 @@ QString LoadModelDialog::ChooseFile() { } } -} +} // namespace flatland_rviz_plugins diff --git a/flatland_rviz_plugins/src/load_model_dialog.hpp b/flatland_rviz_plugins/src/load_model_dialog.hpp index 55303abd..51715ba3 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.hpp +++ b/flatland_rviz_plugins/src/load_model_dialog.hpp @@ -48,30 +48,27 @@ #ifndef FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ #define FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ +#include +#include +#include +#include + #include +#include #include #include #include #include #include #include +#include #include #include -#include -#include - -#include -#include -#include -#include - +#include #include - #include #include -#include - #include "flatland_rviz_plugins/spawn_model_tool.hpp" namespace flatland_rviz_plugins { @@ -83,19 +80,19 @@ class LoadModelDialog : public QDialog { * @brief Launch load model dialog * @param parent, parent widget pointer * @param tool, pointer to this so dialog can call methods - */ + */ LoadModelDialog(QWidget *parent, SpawnModelTool *tool); private: /** * @name ChooseFile * @brief Launch file selection dialog - */ + */ QString ChooseFile(); /** * @name AddNumberAndUpdateName * @brief Add numbering to name and show in the name widget - */ + */ void AddNumberAndUpdateName(); static QString path_to_model_file; // full path to model file @@ -130,6 +127,6 @@ class LoadModelDialog : public QDialog { void on_PathButtonClicked(); }; -} +} // namespace flatland_rviz_plugins #endif // FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ diff --git a/flatland_rviz_plugins/src/pause_tool.cpp b/flatland_rviz_plugins/src/pause_tool.cpp index ab652abc..dab94d32 100644 --- a/flatland_rviz_plugins/src/pause_tool.cpp +++ b/flatland_rviz_plugins/src/pause_tool.cpp @@ -1,29 +1,23 @@ #include "flatland_rviz_plugins/pause_tool.hpp" #include -#include #include +#include #include -namespace flatland_rviz_plugins -{ +namespace flatland_rviz_plugins { -PauseTool::PauseTool() -{ - shortcut_key_ = ' '; -} +PauseTool::PauseTool() { shortcut_key_ = ' '; } -PauseTool::~PauseTool() -{ -} +PauseTool::~PauseTool() {} -void PauseTool::onInitialize() -{ +void PauseTool::onInitialize() { node_ = rclcpp::Node::make_shared("pause_sim_tool"); pause_service_ = node_->create_client("toggle_pause"); setName("Toggle Pause"); - setIcon(rviz_common::loadPixmap("package://flatland_rviz_plugins/icons/pause.svg")); + setIcon(rviz_common::loadPixmap( + "package://flatland_rviz_plugins/icons/pause.svg")); } void PauseTool::activate() { diff --git a/flatland_rviz_plugins/src/spawn_model_tool.cpp b/flatland_rviz_plugins/src/spawn_model_tool.cpp index 984c3dee..979dc35b 100644 --- a/flatland_rviz_plugins/src/spawn_model_tool.cpp +++ b/flatland_rviz_plugins/src/spawn_model_tool.cpp @@ -45,6 +45,15 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "flatland_rviz_plugins/spawn_model_tool.hpp" + +#include +#include +#include +#include +#include +#include + #include #include #include @@ -54,26 +63,14 @@ #include #include #include - -#include "flatland_rviz_plugins/spawn_model_tool.hpp" -#include "load_model_dialog.hpp" - -#include -#include -#include -#include - -#include -#include +#include #include #include +#include +#include #include -#include - -#include - -#include +#include "load_model_dialog.hpp" namespace flatland_rviz_plugins { QString SpawnModelTool::path_to_model_file_; @@ -110,10 +107,11 @@ void SpawnModelTool::onInitialize() { model_state = m_hidden; // make an arrow to show axis of rotation - arrow_ = new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); + arrow_ = + new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); arrow_->setColor(0.0f, 0.0f, 1.0f, 1.0f); // blue - arrow_->getSceneNode()->setVisible( - false); // will only be visible during rotation phase + // will only be visible during rotation phase + arrow_->getSceneNode()->setVisible(false); // set arrow to point up (along z) Ogre::Quaternion orientation(Ogre::Radian(M_PI), Ogre::Vector3(1, 0, 0)); @@ -127,7 +125,8 @@ void SpawnModelTool::onInitialize() { SetMovingModelColor(Qt::green); setName("Load Model"); - setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/RobotModel.png")); + setIcon(rviz_common::loadPixmap( + "package://rviz_default_plugins/icons/classes/RobotModel.png")); } // Activate() is called when the tool is started by the user, either @@ -145,7 +144,8 @@ void SpawnModelTool::onInitialize() { // user edits the property. This is a fine idea, and is possible, but // is left as an exercise for the reader. void SpawnModelTool::activate() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::activate "); + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), + "SpawnModelTool::activate "); LoadModelDialog *model_dialog = new LoadModelDialog(nullptr, this); model_dialog->setModal(true); @@ -153,7 +153,8 @@ void SpawnModelTool::activate() { } void SpawnModelTool::BeginPlacement() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::BeginPlacement"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), + "SpawnModelTool::BeginPlacement"); model_state = m_dragging; if (moving_model_node_) { @@ -176,7 +177,8 @@ void SpawnModelTool::deactivate() { } void SpawnModelTool::SpawnModelInFlatland() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), + RCLCPP_INFO_STREAM( + rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::SpawnModelInFlatland name:" << model_name.toStdString()); auto srv = std::make_shared(); @@ -191,12 +193,14 @@ void SpawnModelTool::SpawnModelInFlatland() { srv->pose.y = intersection[1]; srv->pose.theta = initial_angle; - std::shared_ptr node = rclcpp::Node::make_shared("spawn_model_tool"); // TODO + std::shared_ptr node = + rclcpp::Node::make_shared("spawn_model_tool"); // TODO client = node->create_client("spawn_model"); // make ros service call auto result = client->async_send_request(srv); - if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) == + rclcpp::FutureReturnCode::SUCCESS) { auto response = result.get(); if (!response->success) { QMessageBox msgBox; @@ -211,7 +215,8 @@ void SpawnModelTool::SpawnModelInFlatland() { } void SpawnModelTool::SetMovingModelColor(QColor c) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::SetMovingModelColor"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), + "SpawnModelTool::SetMovingModelColor"); try { auto *m_pEntity = @@ -221,7 +226,7 @@ void SpawnModelTool::SetMovingModelColor(QColor c) { m_pMat->getTechnique(0)->getPass(0)->setDiffuse(c.redF(), c.greenF(), c.blueF(), 0); } catch (Ogre::InvalidParametersException &e) { - //RCLCPP_WARN("Invalid preview model"); + // RCLCPP_WARN("Invalid preview model"); } } @@ -245,9 +250,10 @@ int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); if (model_state == m_dragging) { - auto intersectionPair = rviz_rendering::ViewportProjectionFinder().getViewportPointProjectionOnXYPlane(event.panel->getRenderWindow(), - event.x, - event.y); + auto intersectionPair = + rviz_rendering::ViewportProjectionFinder() + .getViewportPointProjectionOnXYPlane(event.panel->getRenderWindow(), + event.x, event.y); intersection = intersectionPair.second; if (intersectionPair.first) { moving_model_node_->setVisible(true); @@ -267,9 +273,10 @@ int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { } } if (model_state == m_rotating) { // model_state is m_rotating - auto intersectionPair = rviz_rendering::ViewportProjectionFinder().getViewportPointProjectionOnXYPlane(event.panel->getRenderWindow(), - event.x, - event.y); + auto intersectionPair = + rviz_rendering::ViewportProjectionFinder() + .getViewportPointProjectionOnXYPlane(event.panel->getRenderWindow(), + event.x, event.y); Ogre::Vector3 intersection2 = intersectionPair.second; if (intersectionPair.first) { if (event.leftDown()) { @@ -301,7 +308,8 @@ void SpawnModelTool::LoadPreview() { moving_model_node_->removeAllChildren(); lines_list_.clear(); - std::shared_ptr node = rclcpp::Node::make_shared("spawn_model_tool"); // TODO + std::shared_ptr node = + rclcpp::Node::make_shared("spawn_model_tool"); // TODO // Load the bodies list into a model object flatland_server::YamlReader reader(node, path_to_model_file_.toStdString()); @@ -345,11 +353,13 @@ void SpawnModelTool::LoadPreview() { } } } catch (const flatland_server::YAMLException &e) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("SpawnModelTool"), "Couldn't load model bodies for preview" << e.what()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("SpawnModelTool"), + "Couldn't load model bodies for preview" << e.what()); } } -void SpawnModelTool::LoadPolygonFootprint(flatland_server::YamlReader &footprint) { +void SpawnModelTool::LoadPolygonFootprint( + flatland_server::YamlReader &footprint) { auto lines = lines_list_.back(); auto points = footprint.GetList("points", 3, b2_maxPolygonVertices); @@ -357,11 +367,13 @@ void SpawnModelTool::LoadPolygonFootprint(flatland_server::YamlReader &footprint lines->addPoint(Ogre::Vector3(p.x, p.y, 0.)); } if (points.size() > 0) { - lines->addPoint(Ogre::Vector3(points.at(0).x, points.at(0).y, 0.)); // Close the box + lines->addPoint( + Ogre::Vector3(points.at(0).x, points.at(0).y, 0.)); // Close the box } } -void SpawnModelTool::LoadCircleFootprint(flatland_server::YamlReader &footprint) { +void SpawnModelTool::LoadCircleFootprint( + flatland_server::YamlReader &footprint) { auto lines = lines_list_.back(); auto center = footprint.GetVec2("center", flatland_server::Vec2()); auto radius = footprint.Get("radius", 1.0); @@ -385,7 +397,7 @@ void SpawnModelTool::SaveName(QString n) { model_name = n; } // namespace and then tell pluginlib about the class. It is important // to do this in global scope, outside our package's namespace. -} // end namespace flatland_viz +} // namespace flatland_rviz_plugins #include PLUGINLIB_EXPORT_CLASS(flatland_rviz_plugins::SpawnModelTool, rviz_common::Tool) From f89d16f1059cf2e9340d67aa57fbf7013b04f249 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Thu, 1 Dec 2022 17:30:28 +0000 Subject: [PATCH 46/66] Make canceling the model spawning dialog revert to the default tool. --- flatland_rviz_plugins/src/load_model_dialog.cpp | 10 ++++++++-- flatland_rviz_plugins/src/load_model_dialog.hpp | 8 +++++++- flatland_rviz_plugins/src/spawn_model_tool.cpp | 2 +- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/flatland_rviz_plugins/src/load_model_dialog.cpp b/flatland_rviz_plugins/src/load_model_dialog.cpp index a6ac6222..b57a9a59 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.cpp +++ b/flatland_rviz_plugins/src/load_model_dialog.cpp @@ -63,6 +63,7 @@ #include #include #include +#include namespace flatland_rviz_plugins { @@ -70,8 +71,8 @@ QString LoadModelDialog::path_to_model_file; int LoadModelDialog::count; bool LoadModelDialog::numbering; -LoadModelDialog::LoadModelDialog(QWidget *parent, SpawnModelTool *tool) - : QDialog(parent), tool_(tool) { +LoadModelDialog::LoadModelDialog(QWidget *parent, rviz_common::DisplayContext *context, SpawnModelTool *tool) + : QDialog(parent), context_(context), tool_(tool) { RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "ModelDialog::ModelDialog"); auto *v_layout = new QVBoxLayout; @@ -148,6 +149,11 @@ LoadModelDialog::LoadModelDialog(QWidget *parent, SpawnModelTool *tool) void LoadModelDialog::CancelButtonClicked() { RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::CancelButtonClicked"); + + auto tool_man = context_->getToolManager(); + auto dflt_tool = tool_man->getDefaultTool(); + tool_man->setCurrentTool(dflt_tool); + this->close(); } diff --git a/flatland_rviz_plugins/src/load_model_dialog.hpp b/flatland_rviz_plugins/src/load_model_dialog.hpp index 51715ba3..dc70acda 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.hpp +++ b/flatland_rviz_plugins/src/load_model_dialog.hpp @@ -64,8 +64,12 @@ #include #include #include + #include + #include + +#include #include #include @@ -81,9 +85,11 @@ class LoadModelDialog : public QDialog { * @param parent, parent widget pointer * @param tool, pointer to this so dialog can call methods */ - LoadModelDialog(QWidget *parent, SpawnModelTool *tool); + LoadModelDialog(QWidget *parent, rviz_common::DisplayContext *context, SpawnModelTool *tool); private: + rviz_common::DisplayContext * context_; + /** * @name ChooseFile * @brief Launch file selection dialog diff --git a/flatland_rviz_plugins/src/spawn_model_tool.cpp b/flatland_rviz_plugins/src/spawn_model_tool.cpp index 979dc35b..b526bde8 100644 --- a/flatland_rviz_plugins/src/spawn_model_tool.cpp +++ b/flatland_rviz_plugins/src/spawn_model_tool.cpp @@ -147,7 +147,7 @@ void SpawnModelTool::activate() { RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::activate "); - LoadModelDialog *model_dialog = new LoadModelDialog(nullptr, this); + LoadModelDialog *model_dialog = new LoadModelDialog(nullptr, context_, this); model_dialog->setModal(true); model_dialog->show(); } From 60ad94bbc3d4d6d2f5549f946ceaa72d6e515b68 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 15:11:16 +0000 Subject: [PATCH 47/66] Add new service (ChangeRate) message. The message that will be used by the change rate service to change the simulation speed in run time. --- flatland_msgs/srv/ChangeRate.srv | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 flatland_msgs/srv/ChangeRate.srv diff --git a/flatland_msgs/srv/ChangeRate.srv b/flatland_msgs/srv/ChangeRate.srv new file mode 100644 index 00000000..d40f7850 --- /dev/null +++ b/flatland_msgs/srv/ChangeRate.srv @@ -0,0 +1,4 @@ +float64 rate +--- +bool success +string message From e257131a1e96d6446ff3625249257f675f17b9f6 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 15:11:44 +0000 Subject: [PATCH 48/66] Add change rate service message to the compilations. --- flatland_msgs/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/flatland_msgs/CMakeLists.txt b/flatland_msgs/CMakeLists.txt index c516150e..c80ce9b4 100644 --- a/flatland_msgs/CMakeLists.txt +++ b/flatland_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ set(msg_files ) set(srv_files + "srv/ChangeRate.srv" "srv/SpawnModel.srv" "srv/DeleteModel.srv" "srv/MoveModel.srv" From b438085be6c0348f47f628eb952e6eabd94867c1 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 15:13:00 +0000 Subject: [PATCH 49/66] Add setter for the simulation rate. Simulation manager needs a setter for the simulation rate, so the ChangeRate service can alter it in runtime. --- .../include/flatland_server/simulation_manager.h | 5 +++++ flatland_server/src/simulation_manager.cpp | 12 +++++++++--- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/flatland_server/include/flatland_server/simulation_manager.h b/flatland_server/include/flatland_server/simulation_manager.h index 6e6e75c8..d1517d77 100644 --- a/flatland_server/include/flatland_server/simulation_manager.h +++ b/flatland_server/include/flatland_server/simulation_manager.h @@ -88,6 +88,11 @@ class SimulationManager { * Kill the world */ void Shutdown(); + + void setUpdateRate(double update_rate); + +private: + rclcpp::WallRate* rate_; }; } // namespace flatland_server #endif // FLATLAND_SERVER_SIMULATION_MANAGER_H diff --git a/flatland_server/src/simulation_manager.cpp b/flatland_server/src/simulation_manager.cpp index c0b39346..c5f2e087 100644 --- a/flatland_server/src/simulation_manager.cpp +++ b/flatland_server/src/simulation_manager.cpp @@ -66,7 +66,8 @@ SimulationManager::SimulationManager(std::shared_ptr node, std::st step_size_(step_size), show_viz_(show_viz), viz_pub_rate_(viz_pub_rate), - world_yaml_file_(world_yaml_file) { + world_yaml_file_(world_yaml_file), + rate_(new rclcpp::WallRate(update_rate)) { RCLCPP_INFO(rclcpp::get_logger("SimMan"), "Simulation params: world_yaml_file(%s) update_rate(%f), " "step_size(%f) show_viz(%s), viz_pub_rate(%f)", @@ -74,6 +75,12 @@ SimulationManager::SimulationManager(std::shared_ptr node, std::st show_viz_ ? "true" : "false", viz_pub_rate_); } +void SimulationManager::setUpdateRate(double update_rate) { + update_rate_ = update_rate; + delete rate_; + rate_ = new rclcpp::WallRate(update_rate_); +} + void SimulationManager::Main() { RCLCPP_INFO(rclcpp::get_logger("SimMan"), "Initializing..."); run_simulator_ = true; @@ -96,7 +103,6 @@ void SimulationManager::Main() { ServiceManager service_manager(this, world_); Timekeeper timekeeper(node_); - rclcpp::WallRate rate(update_rate_); rclcpp::Clock wall_clock(RCL_STEADY_TIME); timekeeper.SetMaxStepSize(step_size_); RCLCPP_INFO(rclcpp::get_logger("SimMan"), "Simulation loop started"); @@ -125,7 +131,7 @@ void SimulationManager::Main() { rclcpp::spin_some(node_); double cycle_time = wall_clock.now().seconds()-start_time; - rate.sleep(); + rate_->sleep(); iterations++; From 852773cc4a43ee64b73dec707624fc03f3b161c6 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 15:13:35 +0000 Subject: [PATCH 50/66] Add ChangeRate service to flatland_server --- .../include/flatland_server/service_manager.h | 14 ++++++++++++- flatland_server/src/service_manager.cpp | 21 +++++++++++++++++++ 2 files changed, 34 insertions(+), 1 deletion(-) diff --git a/flatland_server/include/flatland_server/service_manager.h b/flatland_server/include/flatland_server/service_manager.h index 7a3038e4..e42d2bc5 100644 --- a/flatland_server/include/flatland_server/service_manager.h +++ b/flatland_server/include/flatland_server/service_manager.h @@ -44,6 +44,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include #include @@ -65,10 +66,11 @@ class SimulationManager; */ class ServiceManager { public: - World *world_; ///< aaa handle to the simulation world + World *world_; ///< a handle to the simulation world rclcpp::Node::SharedPtr node_; SimulationManager *sim_man_; ///< a handle to the simulation manager + rclcpp::Service::SharedPtr change_rate_service_; ///< service for changing the simulation rate rclcpp::Service::SharedPtr spawn_model_service_; ///< service for spawning models rclcpp::Service::SharedPtr delete_model_service_; ///< service for deleting models rclcpp::Service::SharedPtr move_model_service_; ///< service for moving models @@ -84,6 +86,16 @@ class ServiceManager { */ ServiceManager(SimulationManager *sim_man, World *world); + /** + * @brief Callback for the change rate service + * @param[in] request_header The ros middleware service header + * @param[in] request Contains the request data for the service + * @param[in/out] response Contains the response for the service + */ + bool ChangeRate(const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + /** * @brief Callback for the spawn model service * @param[in] request_header The ros middleware service header diff --git a/flatland_server/src/service_manager.cpp b/flatland_server/src/service_manager.cpp index f9d0463e..0d0372b9 100644 --- a/flatland_server/src/service_manager.cpp +++ b/flatland_server/src/service_manager.cpp @@ -54,6 +54,8 @@ ServiceManager::ServiceManager(SimulationManager *sim_man, World *world) : world_(world), node_(world->node_), sim_man_(sim_man) { using namespace std::placeholders; // for _1, _2, ... etc + change_rate_service_ = node_->create_service( + "change_rate", std::bind(&ServiceManager::ChangeRate, this, _1, _2, _3)); spawn_model_service_ = node_->create_service( "spawn_model", std::bind(&ServiceManager::SpawnModel, this, _1, _2, _3)); delete_model_service_ = node_->create_service( @@ -86,6 +88,25 @@ ServiceManager::ServiceManager(SimulationManager *sim_man, World *world) } } +bool ServiceManager::ChangeRate( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) { + RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), "Change rate requested with rate(\"%f\")", + request->rate); + + try { + sim_man_->setUpdateRate(request->rate); + response->success = true; + response->message = ""; + } catch (const std::exception &e) { + response->success = false; + response->message = std::string(e.what()); + } + + return true; +} + bool ServiceManager::SpawnModel(const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { From ffd60256af6f16593fadeed4ec557949a53ea797 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 20:55:21 +0000 Subject: [PATCH 51/66] Add .idea and cmake-build-debug to gitignore. --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 784ea8aa..aeeb8878 100644 --- a/.gitignore +++ b/.gitignore @@ -7,4 +7,6 @@ docs/_templates/* .vscode/* */.vscode .history -*/.history \ No newline at end of file +*/.history +**/.idea/ +**/cmake-build-debug/ From 1db078438132966eca11becefdcc6875e13bd647 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 20:56:26 +0000 Subject: [PATCH 52/66] Add destructor for simulation manager Cleans the memory reserved for the ros WallRate object --- flatland_server/include/flatland_server/simulation_manager.h | 2 ++ flatland_server/src/simulation_manager.cpp | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/flatland_server/include/flatland_server/simulation_manager.h b/flatland_server/include/flatland_server/simulation_manager.h index d1517d77..70befe28 100644 --- a/flatland_server/include/flatland_server/simulation_manager.h +++ b/flatland_server/include/flatland_server/simulation_manager.h @@ -79,6 +79,8 @@ class SimulationManager { SimulationManager(std::shared_ptr node, std::string world_yaml_file, double update_rate, double step_size, bool show_viz, double viz_pub_rate); + ~SimulationManager(); + /** * This method contains the loop that runs the simulation */ diff --git a/flatland_server/src/simulation_manager.cpp b/flatland_server/src/simulation_manager.cpp index c5f2e087..f080096a 100644 --- a/flatland_server/src/simulation_manager.cpp +++ b/flatland_server/src/simulation_manager.cpp @@ -75,6 +75,10 @@ SimulationManager::SimulationManager(std::shared_ptr node, std::st show_viz_ ? "true" : "false", viz_pub_rate_); } +SimulationManager::~SimulationManager() { + delete rate_; +} + void SimulationManager::setUpdateRate(double update_rate) { update_rate_ = update_rate; delete rate_; From 759792fc746ddbe5513b6b63cb9a122025cee92c Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 20:57:58 +0000 Subject: [PATCH 53/66] Add change rate tool --- .../change_rate_tool.hpp | 36 +++++++++++++++ .../src/change_rate_tool.cpp | 45 +++++++++++++++++++ 2 files changed, 81 insertions(+) create mode 100644 flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp create mode 100644 flatland_rviz_plugins/src/change_rate_tool.cpp diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp new file mode 100644 index 00000000..0c3e4153 --- /dev/null +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp @@ -0,0 +1,36 @@ +#ifndef FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ +#define FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rviz_default_plugins/visibility_control.hpp" + +namespace flatland_rviz_plugins { + +class RVIZ_DEFAULT_PLUGINS_PUBLIC ChangeRateTool : public rviz_common::Tool { + Q_OBJECT + + public: + ChangeRateTool(); + ~ChangeRateTool() override; + + void onInitialize() override; + + void activate() override; + void deactivate() override; + + void setRate(double rate); + + private: + std::shared_ptr node_; + rclcpp::Client::SharedPtr change_rate_service_; +}; + +} // namespace flatland_rviz_plugins + +#endif // FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ diff --git a/flatland_rviz_plugins/src/change_rate_tool.cpp b/flatland_rviz_plugins/src/change_rate_tool.cpp new file mode 100644 index 00000000..5a4e2945 --- /dev/null +++ b/flatland_rviz_plugins/src/change_rate_tool.cpp @@ -0,0 +1,45 @@ +#include "flatland_rviz_plugins/change_rate_tool.hpp" + +#include +#include +#include +#include +#include + +#include "change_rate_dialog.hpp" + +namespace flatland_rviz_plugins { + +ChangeRateTool::ChangeRateTool() { shortcut_key_ = 'R'; } + +ChangeRateTool::~ChangeRateTool() {} + +void ChangeRateTool::onInitialize() { + node_ = rclcpp::Node::make_shared("change_rate_tool"); + change_rate_service_ = + node_->create_client("change_rate"); + + setName("Change Simulation Rate"); + setIcon(rviz_common::loadPixmap("package://flatland_rviz_plugins/icons/time.png")); +} + + +void ChangeRateTool::setRate(double rate) { + auto request = std::make_shared(); + request->rate = rate; + auto result = change_rate_service_->async_send_request(request); + rclcpp::spin_until_future_complete(node_, result); +} + +void ChangeRateTool::activate() { + auto *model_dialog = new ChangeRateDialog(nullptr, context_, this); + model_dialog->setModal(true); + model_dialog->show(); +} + +void ChangeRateTool::deactivate() {} + +} // namespace flatland_rviz_plugins + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(flatland_rviz_plugins::ChangeRateTool, rviz_common::Tool) From 3e1c80689f4f1aa7fe0691c758705770ad4678ad Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 20:58:28 +0000 Subject: [PATCH 54/66] Add change rate tool to plugin description file It is exported to use in rviz --- flatland_rviz_plugins/CMakeLists.txt | 3 +++ flatland_rviz_plugins/plugins_description.xml | 10 ++++++++++ 2 files changed, 13 insertions(+) diff --git a/flatland_rviz_plugins/CMakeLists.txt b/flatland_rviz_plugins/CMakeLists.txt index 6819b686..61fcee8b 100644 --- a/flatland_rviz_plugins/CMakeLists.txt +++ b/flatland_rviz_plugins/CMakeLists.txt @@ -38,6 +38,7 @@ find_package(Lua 5.1 REQUIRED) find_package(Boost REQUIRED COMPONENTS date_time system filesystem) set(flatland_rviz_plugins_headers_to_moc + include/flatland_rviz_plugins/change_rate_tool.hpp include/flatland_rviz_plugins/pause_tool.hpp include/flatland_rviz_plugins/interactive_tool.hpp include/flatland_rviz_plugins/spawn_model_tool.hpp) @@ -49,6 +50,8 @@ include_directories( set(library_name ${PROJECT_NAME}) add_library(${library_name} SHARED + src/change_rate_tool.cpp + src/change_rate_dialog.cpp src/pause_tool.cpp src/interactive_tool.cpp src/spawn_model_tool.cpp diff --git a/flatland_rviz_plugins/plugins_description.xml b/flatland_rviz_plugins/plugins_description.xml index db5bbb18..b985837b 100644 --- a/flatland_rviz_plugins/plugins_description.xml +++ b/flatland_rviz_plugins/plugins_description.xml @@ -1,5 +1,15 @@ + + + Changes the update rate of the simulation + + + Date: Mon, 12 Dec 2022 20:58:41 +0000 Subject: [PATCH 55/66] Add icon for change rate tool --- flatland_rviz_plugins/icons/time.png | Bin 0 -> 5591 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 flatland_rviz_plugins/icons/time.png diff --git a/flatland_rviz_plugins/icons/time.png b/flatland_rviz_plugins/icons/time.png new file mode 100644 index 0000000000000000000000000000000000000000..cfd307aae4586a79c1a151d16946556bd79871b5 GIT binary patch literal 5591 zcmc&&2T)Vn*4_z_ApI#w733PaARSSVfQ`@v6+sa6g7hNNTcQXyup}T#=ORTZk={vA zL4!1v8xRPIftednfw-Em<7836!* zFvjwTBLHxy2?zWLs9K30S_c4@F6fMH+(h8?Di?7q>uY9H$L+P-eWVX(O!V~9PXcEZq7uD}5*V6kctxSnG2E}zmRC^7ly^NE| zixGFl=<^p$G@9lH-V#3tA+Di5?bp-19-S0@@x6}m{Epidk9}WGzud9Y{oc>}vrh*? za&F5DnmE*=T2l*UZcJFHhJSsd~I#uedbvslwU6BEp{i+;ZZK3-dtb zKTy>-lwRK}_#;JEJyfiSw!7cMR0!+&(Bph@ z4500++RzbL`3CQ6nN3t^4PNHuH{)?WHs%ooYV#itkGZb!ON}73s>ZTfkZyC}uzkq| zwLI|>C2;}rhIE3Hh*TTjI(4(7zW>rX@G5hDBFkWiNohF#h$Xki9o+a4Vq1)w{E0bYelmKLMN3%lj&MgVtlp)0N8K;wn=%*MmQpHSX9Ih`9U(U8DO4DydwhEY zI`xU#^ z;6Mz*1VQ^xDl{a@3&4L={gp}Fj3Iq{`laJ|iKEQeGkDWmy%EW_k3Nr9XeArX~o$z_9@L z!e%Xps$k8%`or}-$&g`f*R$%GkcQm=jfHtuhiwBt)Of*{dC&Uxu!sPpGqkRrbyV@J zS-YM;D|hih%OzE*;0G^DXxtEogI#CceXH!9tAb672Hl+1B;f##CfVW=14ypTVj%RH zOl5m|`PmGAY-5-rV0aq1B2}FmMV{He)w^r%Ef;B%JHK~W`GYbgsfU`ONf^&OV1LzZ zyAk}80N|^z3k$j5tYuS^kB{)-SoCp723TI8-rPRumS5^di%BeOt~#m39jP326LqSZ zJNa~t^*=7j?{mQZ~iDe!EY3ni2fIKz~u7f=JI z8<-{x)_s%+ zzUlVe6yqLeYAZCy92M%g3io}kA((g5RR7MYj<7pwZSA>_{rZ@3zaANTwXQiiIu4RiUpHh+H# z4ozaSy@=13IgC&RnD4vJ*PetgoKlysoQVpO9Z8q~nKs!}kY9Gu>jc zTg;`e$mfibKa7eJz7?fk4kn`L0iJQOX^wEyGY_XWgyrf&FiR&t=je?ouZ8i9y%lww z4@%Z2+v0kY>oF5_^7M!b<@jW$=H>I3@a1^9A+nFL8AV2kFShzJV&GtYkY{|4^!O1? z%cbgL4Y>xG$P%HwKiO4;XluIQ6xmGL7nJ@o3Byc~UAnDCs=)e<%z*+ySQe)wwYW!MB~gOW*)8&Im>-r!s>R}j16T|#USGDXjHH?- znPKDWo`hV}nCvF^1+g;?o7^jGX~PmcspB!i@?YGyg)%wOlkPo&3(u6K+eA+B7kp-t z4p2m_gcdA)3}Xi+;rg8G-F{H_j37S#F#q7D>@E*$U5`gFdAg3U5$*#@;ifE4e%YGG zXebSvHO23Z_{pjN5xQHkU|tU^x&<4tJgC%c&6Sfb|5X1L$gMVrJFNs0TCmW*nfzxR!E4t{oHH(|b;%t1&1CzD;DcJC#lcygu z1-)X|`>}=;l9G$pDz6lgMi~ESh127xNz-rAH+XD%2UMD!A@8Q6$Fn}oe)y_Jno)aVkH6*|NR8tShZgV|ia zd=2zrEha<01%vm$D^rp!a7;61wf?zzLG6eJ%bJSXl<*iPGY=E z46Zp&e-=sX*Nw}+yf_%dwg4atTgruwUY@U86NLSe=eplVde7X)3kuevMmLiID0#ON z^B{uyP%FQvduVc&kSW}#14>@o;01HnT1R!6)!|K*Fm0v&nP+SJ9$t-@*Wu0aiQ;)E zf~|s-?vVExV*w`h(Ivgnx@)+Jxoy!!G*QpsHhs$a9hrU!r${D!BLkO@GVToK*LE4O z4aQI=VE0HHb#f6xG{=xNm*Mfm2|2pzCW{J50a_i{0KY>NtmPS{%$2TvVv z^kG-=+RXecpzZK9u^Zk@#;GX~>$BmhR7DKcmu*2#ziWU=?1av?%$Q2E`|>{M32jzk zP*G!}K`=wQQWi-A*40fPgE|8tk?*t*y^CIk9NEmGNirxO_E2ch@;QW$?-)RW5idQ= zp`taTUGUf@6$W8(-4kcKU7E2N^l0khFWFbNUqu&wCfY;$BO?m)QPm;uVcqkpXWCCH zGUiYR)pf`(`iD1XP`sPbWVb0D%31(0; za4J&P89TqwIxy4#cKA*>Rpo=d-<7=jA5T4?JzwbY#0l@$rr7Ia4+z6%^_n<*1qV5U zgPogCn?8m(Dl(Q*8nQ`FQ0!^-4mAoFL@r|OYAi7-7tQh5K(8?uf|oNCvX*?(;@7nW-x<$2u<>wgqPh<|d0^#xhA8`d7}z6Z%_&j{-` z-9uTG8OgIdVO4$RT!r3;af$xES zZ$WGFHM#s2(StDw_>J^usR}B_?V-`ecH6Eza67e<`pv3Xqx`G0-5V$e>Y_f0S;IoB z&&_~zqkWy~S#upi@|nWFf7SrXA8!n`ZDP$4W*S8$*#x&Up%eC)r3;^VTtgH6CsONg zzsqQxefB_JfL9wCtQHN}*4_c#&3ug{qMED&?GT@6hbZcU%S={_z_SuYWK-!o9lJT0 z=lC71{nMf;Ap2q>Q*QxmUxw!O7W6=a*2Z~^Za9NtzDyqM7(ZVif z>u*WZuiuv-@$q#dIigO6P(Hohd|~;!(2pOLs)qDwRcQw(znONNeTrJnsE857S7_C_ zPdtJ0V6?%(QtFHFLwR29H3svYs9}k$`8iCa16PG{*+@LmsU|qSW+Xnp*ozF=Ecy-vF<8L z6hUsRvn9UklzJ>v9&|@ExO5H6R=uENCi17UP_4nw^T#wP^%yNV_O8kn$|2~bS#)ZK z;@xQEnk0!@bHqF4Pvz%hmez%;?$xbZWMx212&7W&Aa~CG9$-e8io(YmQYQBN-1vc6 zYDR^~vuT=7#M!(MpK^mqRX;e_P?LZlwkAX!i~D_Y6E$xcb`;N&aV3TbLc~ye{n*K_ zug9qIB$N@#_*uvyS>#)40c=rYMaros5jZFW>e7s7{)1VIUq>iiJu+{oijDD zUxUW-a-vj>pD@}y@}}B*kZ|y9W+HDY_4zXPaLHupEgS&eZR!*Sg9YP$tFT*PR{;nd zDa0Ha9Gr%-am5}1I9PZY`Gzr_sA>As`viihHgPz{8~}OJiF3JLQJn}eP@phxv(%Pi zMpwkJEgo^_B5nNXi=^s)n>3^h7bvY?y6pt<`gHpMs#%p%)guB2D}l;MDqG)&#G>rN zF10Zb5SJbaQ88|LF^~|5r#mJ>YL6c*na}>jR;M@xAt2fv(r;ViG!6T*%a8|}R3W8z zPA0~!W}&X1yAb0) Date: Mon, 12 Dec 2022 20:59:20 +0000 Subject: [PATCH 56/66] Add change rate dialog Used by the change rate tool, so the user can choose the desired rate --- .../src/change_rate_dialog.cpp | 72 +++++++++++++++++++ .../src/change_rate_dialog.hpp | 49 +++++++++++++ 2 files changed, 121 insertions(+) create mode 100644 flatland_rviz_plugins/src/change_rate_dialog.cpp create mode 100644 flatland_rviz_plugins/src/change_rate_dialog.hpp diff --git a/flatland_rviz_plugins/src/change_rate_dialog.cpp b/flatland_rviz_plugins/src/change_rate_dialog.cpp new file mode 100644 index 00000000..f9568544 --- /dev/null +++ b/flatland_rviz_plugins/src/change_rate_dialog.cpp @@ -0,0 +1,72 @@ +#include "change_rate_dialog.hpp" + +#include + +namespace flatland_rviz_plugins { + + ChangeRateDialog::ChangeRateDialog(QWidget *parent, + rviz_common::DisplayContext *context, ChangeRateTool *tool) + : QDialog(parent), context_(context), tool_(tool) { + r_edit = new QLineEdit; + auto *okButton = new QPushButton("ok"); + auto *cancelButton = new QPushButton("cancel"); + + r_edit->setFocusPolicy(Qt::ClickFocus); // only name gets focus + okButton->setFocusPolicy(Qt::NoFocus); + cancelButton->setFocusPolicy(Qt::NoFocus); + + connect(okButton, &QAbstractButton::clicked, this, + &ChangeRateDialog::OkButtonClicked); + connect(cancelButton, &QAbstractButton::clicked, this, + &ChangeRateDialog::CancelButtonClicked); + + auto *h0_layout = new QHBoxLayout; + h0_layout->addWidget(new QLabel("rate:")); + h0_layout->addWidget(r_edit); + + auto *h1_layout = new QHBoxLayout; + h1_layout->addWidget(okButton); + h1_layout->addWidget(cancelButton); + + auto *v_layout = new QVBoxLayout; + v_layout->addLayout(h0_layout); + v_layout->addLayout(h1_layout); + + setLayout(v_layout); + + // delete the Dialog if the user clicks on the x to close window + this->setAttribute(Qt::WA_DeleteOnClose, true); + } + + void ChangeRateDialog::CancelButtonClicked() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), + "LoadModelDialog::CancelButtonClicked"); + + auto tool_man = context_->getToolManager(); + auto dflt_tool = tool_man->getDefaultTool(); + tool_man->setCurrentTool(dflt_tool); + + this->close(); + } + + void ChangeRateDialog::OkButtonClicked() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), + "LoadModelDialog::OkButtonClicked"); + bool ok(false); + double rate = r_edit->text().toDouble(&ok); + if (ok) { + tool_->setRate(rate); + + auto tool_man = context_->getToolManager(); + auto dflt_tool = tool_man->getDefaultTool(); + tool_man->setCurrentTool(dflt_tool); + + this->close(); + } else { + QMessageBox msgBox; + msgBox.setText("Error: The rate needs to be a valid double (float 64-bit) value."); + msgBox.exec(); + } + } + +} // namespace flatland_rviz_plugins diff --git a/flatland_rviz_plugins/src/change_rate_dialog.hpp b/flatland_rviz_plugins/src/change_rate_dialog.hpp new file mode 100644 index 00000000..8c307695 --- /dev/null +++ b/flatland_rviz_plugins/src/change_rate_dialog.hpp @@ -0,0 +1,49 @@ +#ifndef FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ +#define FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "flatland_rviz_plugins/change_rate_tool.hpp" + +namespace flatland_rviz_plugins { + +class ChangeRateDialog : public QDialog { +public: + ChangeRateDialog(QWidget *parent, rviz_common::DisplayContext *context, ChangeRateTool *tool); + +private: + rviz_common::DisplayContext * context_; + + QLineEdit *r_edit; // name lineEdit widget + + ChangeRateTool *tool_; + +public Q_SLOTS: + /** + * @name CancelButtonClicked + * @brief Cancel button was clicked, dismiss dialog + */ + void CancelButtonClicked(); + /** + * @name OkButtonClicked + * @brief Ok button was clicked, begin placement + */ + void OkButtonClicked(); +}; + +} // namespace flatland_rviz_plugins + + +#endif //FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ From b2bb624c1be6f33edb3e4790b8c68c4ba7dd8dd9 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 21:00:24 +0000 Subject: [PATCH 57/66] Format service and simulation managers --- .../include/flatland_server/service_manager.h | 60 ++++++---- .../flatland_server/simulation_manager.h | 10 +- flatland_server/src/service_manager.cpp | 107 ++++++++++-------- flatland_server/src/simulation_manager.cpp | 38 +++---- 4 files changed, 122 insertions(+), 93 deletions(-) diff --git a/flatland_server/include/flatland_server/service_manager.h b/flatland_server/include/flatland_server/service_manager.h index e42d2bc5..31ab616c 100644 --- a/flatland_server/include/flatland_server/service_manager.h +++ b/flatland_server/include/flatland_server/service_manager.h @@ -44,12 +44,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include + #include #include #include #include -#include -#include #include #include @@ -66,18 +67,25 @@ class SimulationManager; */ class ServiceManager { public: - World *world_; ///< a handle to the simulation world + World *world_; ///< a handle to the simulation world rclcpp::Node::SharedPtr node_; SimulationManager *sim_man_; ///< a handle to the simulation manager - rclcpp::Service::SharedPtr change_rate_service_; ///< service for changing the simulation rate - rclcpp::Service::SharedPtr spawn_model_service_; ///< service for spawning models - rclcpp::Service::SharedPtr delete_model_service_; ///< service for deleting models - rclcpp::Service::SharedPtr move_model_service_; ///< service for moving models - rclcpp::Service::SharedPtr pause_service_; ///< service for pausing the simulation - rclcpp::Service::SharedPtr resume_service_; ///< service for resuming the simulation - rclcpp::Service::SharedPtr toggle_pause_service_; ///< service for toggling the - /// pause state of the simulation + rclcpp::Service::SharedPtr + change_rate_service_; ///< service for changing the simulation rate + rclcpp::Service::SharedPtr + spawn_model_service_; ///< service for spawning models + rclcpp::Service::SharedPtr + delete_model_service_; ///< service for deleting models + rclcpp::Service::SharedPtr + move_model_service_; ///< service for moving models + rclcpp::Service::SharedPtr + pause_service_; ///< service for pausing the simulation + rclcpp::Service::SharedPtr + resume_service_; ///< service for resuming the simulation + rclcpp::Service::SharedPtr + toggle_pause_service_; ///< service for toggling the + /// pause state of the simulation /** * @brief Service manager constructor @@ -92,9 +100,10 @@ class ServiceManager { * @param[in] request Contains the request data for the service * @param[in/out] response Contains the response for the service */ - bool ChangeRate(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + bool ChangeRate( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the spawn model service @@ -102,9 +111,10 @@ class ServiceManager { * @param[in] request Contains the request data for the service * @param[in/out] response Contains the response for the service */ - bool SpawnModel(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + bool SpawnModel( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the delete model service @@ -112,9 +122,10 @@ class ServiceManager { * @param[in] request Contains the request data for the service * @param[in/out] response Contains the response for the service */ - bool DeleteModel(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + bool DeleteModel( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the move model service @@ -122,9 +133,10 @@ class ServiceManager { * @param[in] request Contains the request data for the service * @param[in/out] response Contains the response for the service */ - bool MoveModel(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + bool MoveModel( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the pause service @@ -147,5 +159,5 @@ class ServiceManager { const std::shared_ptr request, std::shared_ptr response); }; -} +} // namespace flatland_server #endif diff --git a/flatland_server/include/flatland_server/simulation_manager.h b/flatland_server/include/flatland_server/simulation_manager.h index 70befe28..229a75d9 100644 --- a/flatland_server/include/flatland_server/simulation_manager.h +++ b/flatland_server/include/flatland_server/simulation_manager.h @@ -51,6 +51,7 @@ #include #include #include + #include #include @@ -60,7 +61,7 @@ class SimulationManager { public: std::shared_ptr node_; bool run_simulator_; ///< While true, keep running the sim loop - World *world_; ///< Simulation world + World* world_; ///< Simulation world double update_rate_; ///< sim loop rate double step_size_; ///< step size bool show_viz_; ///< flag to determine if to show visualization @@ -76,7 +77,8 @@ class SimulationManager { * @param[in] viz_pub_rate rate to publish visualization * behaving ones */ - SimulationManager(std::shared_ptr node, std::string world_yaml_file, double update_rate, + SimulationManager(std::shared_ptr node, + std::string world_yaml_file, double update_rate, double step_size, bool show_viz, double viz_pub_rate); ~SimulationManager(); @@ -93,8 +95,8 @@ class SimulationManager { void setUpdateRate(double update_rate); -private: + private: rclcpp::WallRate* rate_; }; -} // namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_SIMULATION_MANAGER_H diff --git a/flatland_server/src/service_manager.cpp b/flatland_server/src/service_manager.cpp index 0d0372b9..7d5f3bb2 100644 --- a/flatland_server/src/service_manager.cpp +++ b/flatland_server/src/service_manager.cpp @@ -46,45 +46,54 @@ #include #include + #include namespace flatland_server { ServiceManager::ServiceManager(SimulationManager *sim_man, World *world) - : world_(world), node_(world->node_), sim_man_(sim_man) { - + : world_(world), node_(world->node_), sim_man_(sim_man) { using namespace std::placeholders; // for _1, _2, ... etc change_rate_service_ = node_->create_service( - "change_rate", std::bind(&ServiceManager::ChangeRate, this, _1, _2, _3)); - spawn_model_service_ = node_->create_service( - "spawn_model", std::bind(&ServiceManager::SpawnModel, this, _1, _2, _3)); - delete_model_service_ = node_->create_service( - "delete_model", std::bind(&ServiceManager::DeleteModel, this, _1, _2, _3)); + "change_rate", std::bind(&ServiceManager::ChangeRate, this, _1, _2, _3)); + spawn_model_service_ = node_->create_service( + "spawn_model", std::bind(&ServiceManager::SpawnModel, this, _1, _2, _3)); + delete_model_service_ = + node_->create_service( + "delete_model", + std::bind(&ServiceManager::DeleteModel, this, _1, _2, _3)); move_model_service_ = node_->create_service( - "move_model", std::bind(&ServiceManager::MoveModel, this, _1, _2, _3)); + "move_model", std::bind(&ServiceManager::MoveModel, this, _1, _2, _3)); pause_service_ = node_->create_service( - "pause", std::bind(&ServiceManager::Pause, this, _1, _2, _3)); + "pause", std::bind(&ServiceManager::Pause, this, _1, _2, _3)); resume_service_ = node_->create_service( - "resume", std::bind(&ServiceManager::Resume, this, _1, _2, _3)); + "resume", std::bind(&ServiceManager::Resume, this, _1, _2, _3)); toggle_pause_service_ = node_->create_service( - "toggle_pause", std::bind(&ServiceManager::TogglePause, this, _1, _2, _3)); + "toggle_pause", + std::bind(&ServiceManager::TogglePause, this, _1, _2, _3)); if (spawn_model_service_) { - RCLCPP_INFO(rclcpp::get_logger("Service Manager"), "Model spawning service ready to go"); + RCLCPP_INFO(rclcpp::get_logger("Service Manager"), + "Model spawning service ready to go"); } else { - RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), "Error starting model spawning service"); + RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), + "Error starting model spawning service"); } if (delete_model_service_) { - RCLCPP_INFO(rclcpp::get_logger("Service Manager"), "Model deleting service ready to go"); + RCLCPP_INFO(rclcpp::get_logger("Service Manager"), + "Model deleting service ready to go"); } else { - RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), "Error starting model deleting service"); + RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), + "Error starting model deleting service"); } if (move_model_service_) { - RCLCPP_INFO(rclcpp::get_logger("Service Manager"), "Model moving service ready to go"); + RCLCPP_INFO(rclcpp::get_logger("Service Manager"), + "Model moving service ready to go"); } else { - RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), "Error starting model moving service"); + RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), + "Error starting model moving service"); } } @@ -92,8 +101,8 @@ bool ServiceManager::ChangeRate( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), "Change rate requested with rate(\"%f\")", - request->rate); + RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), + "Change rate requested with rate(\"%f\")", request->rate); try { sim_man_->setUpdateRate(request->rate); @@ -107,15 +116,16 @@ bool ServiceManager::ChangeRate( return true; } -bool ServiceManager::SpawnModel(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { +bool ServiceManager::SpawnModel( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) { RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), - "Model spawn requested with path(\"%s\"), namespace(\"%s\"), " - "name(\'%s\"), pose(%f,%f,%f)", - request->yaml_path.c_str(), request->ns.c_str(), - request->name.c_str(), request->pose.x, request->pose.y, - request->pose.theta); + "Model spawn requested with path(\"%s\"), namespace(\"%s\"), " + "name(\'%s\"), pose(%f,%f,%f)", + request->yaml_path.c_str(), request->ns.c_str(), + request->name.c_str(), request->pose.x, request->pose.y, + request->pose.theta); Pose pose(request->pose.x, request->pose.y, request->pose.theta); @@ -126,8 +136,8 @@ bool ServiceManager::SpawnModel(const std::shared_ptr request_ } catch (const std::exception &e) { response->success = false; response->message = std::string(e.what()); - RCLCPP_ERROR(rclcpp::get_logger("ServiceManager"), "Failed to load model! Exception: %s", - e.what()); + RCLCPP_ERROR(rclcpp::get_logger("ServiceManager"), + "Failed to load model! Exception: %s", e.what()); } return true; @@ -137,8 +147,9 @@ bool ServiceManager::DeleteModel( const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { - RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), "Model delete requested with name(\"%s\")", - request->name.c_str()); + RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), + "Model delete requested with name(\"%s\")", + request->name.c_str()); try { world_->DeleteModel(request->name); @@ -152,11 +163,12 @@ bool ServiceManager::DeleteModel( return true; } -bool ServiceManager::MoveModel(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { - RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), "Model move requested with name(\"%s\")", - request->name.c_str()); +bool ServiceManager::MoveModel( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) { + RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), + "Model move requested with name(\"%s\")", request->name.c_str()); Pose pose(request->pose.x, request->pose.y, request->pose.theta); @@ -172,24 +184,27 @@ bool ServiceManager::MoveModel(const std::shared_ptr request_h return true; } -bool ServiceManager::Pause(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { +bool ServiceManager::Pause( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) { world_->Pause(); return true; } -bool ServiceManager::Resume(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { +bool ServiceManager::Resume( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) { world_->Resume(); return true; } -bool ServiceManager::TogglePause(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { +bool ServiceManager::TogglePause( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) { world_->TogglePaused(); return true; } -}; +}; // namespace flatland_server diff --git a/flatland_server/src/simulation_manager.cpp b/flatland_server/src/simulation_manager.cpp index f080096a..72075247 100644 --- a/flatland_server/src/simulation_manager.cpp +++ b/flatland_server/src/simulation_manager.cpp @@ -45,19 +45,22 @@ */ #include "flatland_server/simulation_manager.h" + #include #include #include #include #include + +#include #include #include #include -#include namespace flatland_server { -SimulationManager::SimulationManager(std::shared_ptr node, std::string world_yaml_file, +SimulationManager::SimulationManager(std::shared_ptr node, + std::string world_yaml_file, double update_rate, double step_size, bool show_viz, double viz_pub_rate) : node_(node), @@ -69,15 +72,13 @@ SimulationManager::SimulationManager(std::shared_ptr node, std::st world_yaml_file_(world_yaml_file), rate_(new rclcpp::WallRate(update_rate)) { RCLCPP_INFO(rclcpp::get_logger("SimMan"), - "Simulation params: world_yaml_file(%s) update_rate(%f), " - "step_size(%f) show_viz(%s), viz_pub_rate(%f)", - world_yaml_file_.c_str(), update_rate_, step_size_, - show_viz_ ? "true" : "false", viz_pub_rate_); + "Simulation params: world_yaml_file(%s) update_rate(%f), " + "step_size(%f) show_viz(%s), viz_pub_rate(%f)", + world_yaml_file_.c_str(), update_rate_, step_size_, + show_viz_ ? "true" : "false", viz_pub_rate_); } -SimulationManager::~SimulationManager() { - delete rate_; -} +SimulationManager::~SimulationManager() { delete rate_; } void SimulationManager::setUpdateRate(double update_rate) { update_rate_ = update_rate; @@ -117,13 +118,13 @@ void SimulationManager::Main() { double start_time = wall_clock.now().seconds(); double f = 0.0; try { - f = std::fmod(wall_clock.now().seconds() + - (update_rate_ / 2.0), - viz_update_period); + f = std::fmod(wall_clock.now().seconds() + (update_rate_ / 2.0), + viz_update_period); } catch (std::runtime_error& ex) { - RCLCPP_ERROR(rclcpp::get_logger("SimMan"), "Flatland runtime error: [%s]", ex.what()); + RCLCPP_ERROR(rclcpp::get_logger("SimMan"), "Flatland runtime error: [%s]", + ex.what()); } - bool update_viz = ((f >= 0.0) && (f < 1.0f/update_rate_)); + bool update_viz = ((f >= 0.0) && (f < 1.0f / update_rate_)); world_->Update(timekeeper); // Step physics by ros cycle time @@ -134,21 +135,20 @@ void SimulationManager::Main() { } rclcpp::spin_some(node_); - double cycle_time = wall_clock.now().seconds()-start_time; + double cycle_time = wall_clock.now().seconds() - start_time; rate_->sleep(); iterations++; - - double expected_cycle_time = 1.0f/update_rate_; + double expected_cycle_time = 1.0f / update_rate_; double cycle_util = cycle_time / expected_cycle_time * 100; // in percent double factor = timekeeper.GetStepSize() / expected_cycle_time; min_cycle_util = std::min(cycle_util, min_cycle_util); if (iterations > 10) max_cycle_util = std::max(cycle_util, max_cycle_util); filtered_cycle_util = 0.99 * filtered_cycle_util + 0.01 * cycle_util; - RCLCPP_INFO_THROTTLE(rclcpp::get_logger("SimMan"), wall_clock, - 1000, + RCLCPP_INFO_THROTTLE( + rclcpp::get_logger("SimMan"), wall_clock, 1000, "utilization: min %.1f%% max %.1f%% ave %.1f%% factor: %.1f", min_cycle_util, max_cycle_util, filtered_cycle_util, factor); } From 1edc61c8d1399b4516c0227ffe9a921aa01a12d4 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Mon, 12 Dec 2022 21:00:58 +0000 Subject: [PATCH 58/66] Format flatland rviz plugins --- .../change_rate_tool.hpp | 5 +- .../spawn_model_tool.hpp | 2 +- .../src/change_rate_dialog.cpp | 130 +++++++++--------- .../src/change_rate_dialog.hpp | 47 ++++--- .../src/change_rate_tool.cpp | 6 +- .../src/load_model_dialog.cpp | 6 +- .../src/load_model_dialog.hpp | 8 +- 7 files changed, 103 insertions(+), 101 deletions(-) diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp index 0c3e4153..e41402bc 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp @@ -2,11 +2,11 @@ #define FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ #include +#include #include #include #include #include -#include #include "rviz_default_plugins/visibility_control.hpp" @@ -28,7 +28,8 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ChangeRateTool : public rviz_common::Tool { private: std::shared_ptr node_; - rclcpp::Client::SharedPtr change_rate_service_; + rclcpp::Client::SharedPtr + change_rate_service_; }; } // namespace flatland_rviz_plugins diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp index fb1ba834..371e002d 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp @@ -146,7 +146,7 @@ class SpawnModelTool : public rviz_common::Tool { Ogre::Vector3 intersection; // location cursor intersects ground plane, ie // the location to create the model - float initial_angle; // the angle to create the model at + float initial_angle; // the angle to create the model at Ogre::SceneNode *moving_model_node_; // the node for the 3D object enum ModelState { m_hidden, m_dragging, m_rotating }; ModelState model_state; // model state, first hidden, then dragging to diff --git a/flatland_rviz_plugins/src/change_rate_dialog.cpp b/flatland_rviz_plugins/src/change_rate_dialog.cpp index f9568544..91d96d97 100644 --- a/flatland_rviz_plugins/src/change_rate_dialog.cpp +++ b/flatland_rviz_plugins/src/change_rate_dialog.cpp @@ -4,69 +4,71 @@ namespace flatland_rviz_plugins { - ChangeRateDialog::ChangeRateDialog(QWidget *parent, - rviz_common::DisplayContext *context, ChangeRateTool *tool) - : QDialog(parent), context_(context), tool_(tool) { - r_edit = new QLineEdit; - auto *okButton = new QPushButton("ok"); - auto *cancelButton = new QPushButton("cancel"); - - r_edit->setFocusPolicy(Qt::ClickFocus); // only name gets focus - okButton->setFocusPolicy(Qt::NoFocus); - cancelButton->setFocusPolicy(Qt::NoFocus); - - connect(okButton, &QAbstractButton::clicked, this, - &ChangeRateDialog::OkButtonClicked); - connect(cancelButton, &QAbstractButton::clicked, this, - &ChangeRateDialog::CancelButtonClicked); - - auto *h0_layout = new QHBoxLayout; - h0_layout->addWidget(new QLabel("rate:")); - h0_layout->addWidget(r_edit); - - auto *h1_layout = new QHBoxLayout; - h1_layout->addWidget(okButton); - h1_layout->addWidget(cancelButton); - - auto *v_layout = new QVBoxLayout; - v_layout->addLayout(h0_layout); - v_layout->addLayout(h1_layout); - - setLayout(v_layout); - - // delete the Dialog if the user clicks on the x to close window - this->setAttribute(Qt::WA_DeleteOnClose, true); - } - - void ChangeRateDialog::CancelButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), - "LoadModelDialog::CancelButtonClicked"); - - auto tool_man = context_->getToolManager(); - auto dflt_tool = tool_man->getDefaultTool(); - tool_man->setCurrentTool(dflt_tool); - - this->close(); - } - - void ChangeRateDialog::OkButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), - "LoadModelDialog::OkButtonClicked"); - bool ok(false); - double rate = r_edit->text().toDouble(&ok); - if (ok) { - tool_->setRate(rate); - - auto tool_man = context_->getToolManager(); - auto dflt_tool = tool_man->getDefaultTool(); - tool_man->setCurrentTool(dflt_tool); - - this->close(); - } else { - QMessageBox msgBox; - msgBox.setText("Error: The rate needs to be a valid double (float 64-bit) value."); - msgBox.exec(); - } - } +ChangeRateDialog::ChangeRateDialog(QWidget *parent, + rviz_common::DisplayContext *context, + ChangeRateTool *tool) + : QDialog(parent), context_(context), tool_(tool) { + r_edit = new QLineEdit; + auto *okButton = new QPushButton("ok"); + auto *cancelButton = new QPushButton("cancel"); + + r_edit->setFocusPolicy(Qt::ClickFocus); // only name gets focus + okButton->setFocusPolicy(Qt::NoFocus); + cancelButton->setFocusPolicy(Qt::NoFocus); + + connect(okButton, &QAbstractButton::clicked, this, + &ChangeRateDialog::OkButtonClicked); + connect(cancelButton, &QAbstractButton::clicked, this, + &ChangeRateDialog::CancelButtonClicked); + + auto *h0_layout = new QHBoxLayout; + h0_layout->addWidget(new QLabel("rate:")); + h0_layout->addWidget(r_edit); + + auto *h1_layout = new QHBoxLayout; + h1_layout->addWidget(okButton); + h1_layout->addWidget(cancelButton); + + auto *v_layout = new QVBoxLayout; + v_layout->addLayout(h0_layout); + v_layout->addLayout(h1_layout); + + setLayout(v_layout); + + // delete the Dialog if the user clicks on the x to close window + this->setAttribute(Qt::WA_DeleteOnClose, true); +} + +void ChangeRateDialog::CancelButtonClicked() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), + "LoadModelDialog::CancelButtonClicked"); + + auto tool_man = context_->getToolManager(); + auto dflt_tool = tool_man->getDefaultTool(); + tool_man->setCurrentTool(dflt_tool); + + this->close(); +} + +void ChangeRateDialog::OkButtonClicked() { + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), + "LoadModelDialog::OkButtonClicked"); + bool ok(false); + double rate = r_edit->text().toDouble(&ok); + if (ok) { + tool_->setRate(rate); + + auto tool_man = context_->getToolManager(); + auto dflt_tool = tool_man->getDefaultTool(); + tool_man->setCurrentTool(dflt_tool); + + this->close(); + } else { + QMessageBox msgBox; + msgBox.setText( + "Error: The rate needs to be a valid double (float 64-bit) value."); + msgBox.exec(); + } +} } // namespace flatland_rviz_plugins diff --git a/flatland_rviz_plugins/src/change_rate_dialog.hpp b/flatland_rviz_plugins/src/change_rate_dialog.hpp index 8c307695..553e71cd 100644 --- a/flatland_rviz_plugins/src/change_rate_dialog.hpp +++ b/flatland_rviz_plugins/src/change_rate_dialog.hpp @@ -12,7 +12,6 @@ #include #include #include - #include #include "flatland_rviz_plugins/change_rate_tool.hpp" @@ -20,30 +19,30 @@ namespace flatland_rviz_plugins { class ChangeRateDialog : public QDialog { -public: - ChangeRateDialog(QWidget *parent, rviz_common::DisplayContext *context, ChangeRateTool *tool); - -private: - rviz_common::DisplayContext * context_; - - QLineEdit *r_edit; // name lineEdit widget - - ChangeRateTool *tool_; - -public Q_SLOTS: - /** - * @name CancelButtonClicked - * @brief Cancel button was clicked, dismiss dialog - */ - void CancelButtonClicked(); - /** - * @name OkButtonClicked - * @brief Ok button was clicked, begin placement - */ - void OkButtonClicked(); + public: + ChangeRateDialog(QWidget *parent, rviz_common::DisplayContext *context, + ChangeRateTool *tool); + + private: + rviz_common::DisplayContext *context_; + + QLineEdit *r_edit; // name lineEdit widget + + ChangeRateTool *tool_; + + public Q_SLOTS: + /** + * @name CancelButtonClicked + * @brief Cancel button was clicked, dismiss dialog + */ + void CancelButtonClicked(); + /** + * @name OkButtonClicked + * @brief Ok button was clicked, begin placement + */ + void OkButtonClicked(); }; } // namespace flatland_rviz_plugins - -#endif //FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ +#endif // FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ diff --git a/flatland_rviz_plugins/src/change_rate_tool.cpp b/flatland_rviz_plugins/src/change_rate_tool.cpp index 5a4e2945..4a36b6bb 100644 --- a/flatland_rviz_plugins/src/change_rate_tool.cpp +++ b/flatland_rviz_plugins/src/change_rate_tool.cpp @@ -1,10 +1,10 @@ #include "flatland_rviz_plugins/change_rate_tool.hpp" +#include #include #include #include #include -#include #include "change_rate_dialog.hpp" @@ -20,10 +20,10 @@ void ChangeRateTool::onInitialize() { node_->create_client("change_rate"); setName("Change Simulation Rate"); - setIcon(rviz_common::loadPixmap("package://flatland_rviz_plugins/icons/time.png")); + setIcon(rviz_common::loadPixmap( + "package://flatland_rviz_plugins/icons/time.png")); } - void ChangeRateTool::setRate(double rate) { auto request = std::make_shared(); request->rate = rate; diff --git a/flatland_rviz_plugins/src/load_model_dialog.cpp b/flatland_rviz_plugins/src/load_model_dialog.cpp index b57a9a59..8592b75b 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.cpp +++ b/flatland_rviz_plugins/src/load_model_dialog.cpp @@ -61,9 +61,9 @@ #include #include #include +#include #include #include -#include namespace flatland_rviz_plugins { @@ -71,7 +71,9 @@ QString LoadModelDialog::path_to_model_file; int LoadModelDialog::count; bool LoadModelDialog::numbering; -LoadModelDialog::LoadModelDialog(QWidget *parent, rviz_common::DisplayContext *context, SpawnModelTool *tool) +LoadModelDialog::LoadModelDialog(QWidget *parent, + rviz_common::DisplayContext *context, + SpawnModelTool *tool) : QDialog(parent), context_(context), tool_(tool) { RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "ModelDialog::ModelDialog"); diff --git a/flatland_rviz_plugins/src/load_model_dialog.hpp b/flatland_rviz_plugins/src/load_model_dialog.hpp index dc70acda..fd7a339d 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.hpp +++ b/flatland_rviz_plugins/src/load_model_dialog.hpp @@ -64,11 +64,8 @@ #include #include #include - #include - #include - #include #include #include @@ -85,10 +82,11 @@ class LoadModelDialog : public QDialog { * @param parent, parent widget pointer * @param tool, pointer to this so dialog can call methods */ - LoadModelDialog(QWidget *parent, rviz_common::DisplayContext *context, SpawnModelTool *tool); + LoadModelDialog(QWidget *parent, rviz_common::DisplayContext *context, + SpawnModelTool *tool); private: - rviz_common::DisplayContext * context_; + rviz_common::DisplayContext *context_; /** * @name ChooseFile From ddf2b49b333ef08c3786a7fe933364ed3888c7f9 Mon Sep 17 00:00:00 2001 From: TheZambi Date: Fri, 30 Dec 2022 13:05:07 +0000 Subject: [PATCH 59/66] Updates copyright licences --- .../change_rate_tool.hpp | 42 +++++++++++++++++++ .../interactive_tool.hpp | 41 ++++++++++++++++++ .../flatland_rviz_plugins/pause_tool.hpp | 41 ++++++++++++++++++ .../src/change_rate_dialog.cpp | 41 ++++++++++++++++++ .../src/change_rate_dialog.hpp | 41 ++++++++++++++++++ .../src/change_rate_tool.cpp | 41 ++++++++++++++++++ .../src/interactive_tool.cpp | 42 +++++++++++++++++++ flatland_rviz_plugins/src/pause_tool.cpp | 41 ++++++++++++++++++ .../include/flatland_viz/pause_sim_tool.h | 41 ++++++++++++++++++ flatland_viz/src/pause_sim_tool.cpp | 41 ++++++++++++++++++ 10 files changed, 412 insertions(+) diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp index e41402bc..ba867cb0 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp @@ -1,3 +1,45 @@ +/* + * @copyright Copyright 2022 FEUP + * @name change_rate_tool.hpp + * @brief changes simulation rate + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + #ifndef FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp index 68c409ab..d4a5e58e 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp @@ -1,3 +1,44 @@ +/* + * @copyright Copyright 2022 FEUP + * @name interactive_tool.hpp + * @brief allows interaction with interactive markers + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + #ifndef FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp index 14fbff27..1dbf0bee 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp @@ -1,3 +1,44 @@ +/* + * @copyright Copyright 2022 FEUP + * @name pause_tool.hpp + * @brief pauses simulation + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + #ifndef FLATLAND_RVIZ_PLUGINS__PAUSE_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__PAUSE_TOOL_HPP_ diff --git a/flatland_rviz_plugins/src/change_rate_dialog.cpp b/flatland_rviz_plugins/src/change_rate_dialog.cpp index 91d96d97..10a2ff18 100644 --- a/flatland_rviz_plugins/src/change_rate_dialog.cpp +++ b/flatland_rviz_plugins/src/change_rate_dialog.cpp @@ -1,3 +1,44 @@ +/* + * @copyright Copyright 2022 FEUP + * @name change_rate_dialog.cpp + * @brief dialog to change rate + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + #include "change_rate_dialog.hpp" #include diff --git a/flatland_rviz_plugins/src/change_rate_dialog.hpp b/flatland_rviz_plugins/src/change_rate_dialog.hpp index 553e71cd..11438031 100644 --- a/flatland_rviz_plugins/src/change_rate_dialog.hpp +++ b/flatland_rviz_plugins/src/change_rate_dialog.hpp @@ -1,3 +1,44 @@ +/* + * @copyright Copyright 2022 FEUP + * @name change_rate_dialog.cpp + * @brief dialog to change rate + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + #ifndef FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ #define FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ diff --git a/flatland_rviz_plugins/src/change_rate_tool.cpp b/flatland_rviz_plugins/src/change_rate_tool.cpp index 4a36b6bb..dfdb7e01 100644 --- a/flatland_rviz_plugins/src/change_rate_tool.cpp +++ b/flatland_rviz_plugins/src/change_rate_tool.cpp @@ -1,3 +1,44 @@ +/* + * @copyright Copyright 2022 FEUP + * @name change_rate_tool.hpp + * @brief changes simulation rate + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + #include "flatland_rviz_plugins/change_rate_tool.hpp" #include diff --git a/flatland_rviz_plugins/src/interactive_tool.cpp b/flatland_rviz_plugins/src/interactive_tool.cpp index f09437ce..aa41a208 100644 --- a/flatland_rviz_plugins/src/interactive_tool.cpp +++ b/flatland_rviz_plugins/src/interactive_tool.cpp @@ -1,3 +1,45 @@ +/* + * @copyright Copyright 2022 FEUP + * @name interactive_tool.hpp + * @brief allows interaction with interactive markers + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + #include "include/flatland_rviz_plugins/interactive_tool.hpp" #include diff --git a/flatland_rviz_plugins/src/pause_tool.cpp b/flatland_rviz_plugins/src/pause_tool.cpp index dab94d32..3abc129a 100644 --- a/flatland_rviz_plugins/src/pause_tool.cpp +++ b/flatland_rviz_plugins/src/pause_tool.cpp @@ -1,3 +1,44 @@ +/* + * @copyright Copyright 2022 FEUP + * @name pause_tool.cpp + * @brief pauses simulation + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + #include "flatland_rviz_plugins/pause_tool.hpp" #include diff --git a/flatland_viz/include/flatland_viz/pause_sim_tool.h b/flatland_viz/include/flatland_viz/pause_sim_tool.h index 6aec66ae..119066be 100644 --- a/flatland_viz/include/flatland_viz/pause_sim_tool.h +++ b/flatland_viz/include/flatland_viz/pause_sim_tool.h @@ -1,3 +1,44 @@ +/* + * @copyright Copyright 2022 FEUP + * @name pause_sim_tool.h + * @brief pauses simulation + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + #ifndef PAUSE_SIM_TOOL_H #define PAUSE_SIM_TOOL_H diff --git a/flatland_viz/src/pause_sim_tool.cpp b/flatland_viz/src/pause_sim_tool.cpp index 491f375c..972fb19a 100644 --- a/flatland_viz/src/pause_sim_tool.cpp +++ b/flatland_viz/src/pause_sim_tool.cpp @@ -1,3 +1,44 @@ +/* + * @copyright Copyright 2022 FEUP + * @name pause_sim_tool.cpp + * @brief pauses simulation + * @author Ana Barros + * @author Henrique Ribeiro + * @author João Costa + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, FEUP + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Avidbots Corp. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + #include namespace flatland_viz { From fa06d3b322472e9a31a6d5d60df55681697c85ab Mon Sep 17 00:00:00 2001 From: TheZambi Date: Fri, 30 Dec 2022 14:54:32 +0000 Subject: [PATCH 60/66] Fix cpplint errors --- .../include/flatland_rviz_plugins/change_rate_tool.hpp | 2 +- .../include/flatland_rviz_plugins/interactive_tool.hpp | 2 +- .../include/flatland_rviz_plugins/spawn_model_tool.hpp | 4 ++-- flatland_rviz_plugins/src/change_rate_dialog.hpp | 6 +++--- flatland_rviz_plugins/src/load_model_dialog.hpp | 6 +++--- flatland_rviz_plugins/src/spawn_model_tool.cpp | 4 ++-- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp index ba867cb0..af6a652b 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp @@ -43,9 +43,9 @@ #ifndef FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ +#include #include #include -#include #include #include #include diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp index d4a5e58e..04d0db89 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp @@ -42,9 +42,9 @@ #ifndef FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__INTERACTIVE_TOOL_HPP_ +#include #include #include -#include namespace flatland_rviz_plugins { class RVIZ_DEFAULT_PLUGINS_PUBLIC InteractiveTool diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp index 371e002d..87f10187 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp @@ -48,6 +48,8 @@ #ifndef FLATLAND_RVIZ_PLUGINS__SPAWN_MODEL_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__SPAWN_MODEL_TOOL_HPP_ +#include +#include #include #include #include @@ -55,12 +57,10 @@ #include #include -#include #include #include #include #include -#include namespace flatland_rviz_plugins { /** diff --git a/flatland_rviz_plugins/src/change_rate_dialog.hpp b/flatland_rviz_plugins/src/change_rate_dialog.hpp index 11438031..5f0356ae 100644 --- a/flatland_rviz_plugins/src/change_rate_dialog.hpp +++ b/flatland_rviz_plugins/src/change_rate_dialog.hpp @@ -39,8 +39,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ -#define FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ +#ifndef CHANGE_RATE_DIALOG_HPP_ +#define CHANGE_RATE_DIALOG_HPP_ #include #include @@ -86,4 +86,4 @@ class ChangeRateDialog : public QDialog { } // namespace flatland_rviz_plugins -#endif // FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ +#endif // CHANGE_RATE_DIALOG_HPP_ diff --git a/flatland_rviz_plugins/src/load_model_dialog.hpp b/flatland_rviz_plugins/src/load_model_dialog.hpp index fd7a339d..14049515 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.hpp +++ b/flatland_rviz_plugins/src/load_model_dialog.hpp @@ -45,8 +45,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ -#define FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ +#ifndef LOAD_MODEL_DIALOG_HPP_ +#define LOAD_MODEL_DIALOG_HPP_ #include #include @@ -133,4 +133,4 @@ class LoadModelDialog : public QDialog { } // namespace flatland_rviz_plugins -#endif // FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ +#endif // LOAD_MODEL_DIALOG_HPP_ diff --git a/flatland_rviz_plugins/src/spawn_model_tool.cpp b/flatland_rviz_plugins/src/spawn_model_tool.cpp index b526bde8..b26b3a7a 100644 --- a/flatland_rviz_plugins/src/spawn_model_tool.cpp +++ b/flatland_rviz_plugins/src/spawn_model_tool.cpp @@ -194,7 +194,7 @@ void SpawnModelTool::SpawnModelInFlatland() { srv->pose.theta = initial_angle; std::shared_ptr node = - rclcpp::Node::make_shared("spawn_model_tool"); // TODO + rclcpp::Node::make_shared("spawn_model_tool"); client = node->create_client("spawn_model"); // make ros service call @@ -309,7 +309,7 @@ void SpawnModelTool::LoadPreview() { lines_list_.clear(); std::shared_ptr node = - rclcpp::Node::make_shared("spawn_model_tool"); // TODO + rclcpp::Node::make_shared("spawn_model_tool"); // Load the bodies list into a model object flatland_server::YamlReader reader(node, path_to_model_file_.toStdString()); From 6806f5b5faea85673a47088aa89b06fa97ab12fa Mon Sep 17 00:00:00 2001 From: TheZambi Date: Fri, 30 Dec 2022 14:55:08 +0000 Subject: [PATCH 61/66] Fix uncrustify --- flatland_rviz_plugins/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flatland_rviz_plugins/CMakeLists.txt b/flatland_rviz_plugins/CMakeLists.txt index 61fcee8b..883e9dc5 100644 --- a/flatland_rviz_plugins/CMakeLists.txt +++ b/flatland_rviz_plugins/CMakeLists.txt @@ -113,8 +113,8 @@ install( ) install( - DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" - DESTINATION "share/${PROJECT_NAME}" + DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + DESTINATION "share/${PROJECT_NAME}" ) if(BUILD_TESTING) From 201bb2726730121cbc367f671289c06b7b223029 Mon Sep 17 00:00:00 2001 From: JoaoCostaIFG Date: Fri, 30 Dec 2022 14:56:50 +0000 Subject: [PATCH 62/66] Format files --- flatland_box2d/include/Box2D/Box2D.h | 55 +- .../Box2D/Collision/Shapes/b2ChainShape.h | 139 +- .../Box2D/Collision/Shapes/b2CircleShape.h | 71 +- .../Box2D/Collision/Shapes/b2EdgeShape.h | 93 +- .../Box2D/Collision/Shapes/b2PolygonShape.h | 138 +- .../include/Box2D/Collision/Shapes/b2Shape.h | 157 +- .../include/Box2D/Collision/b2BroadPhase.h | 341 ++- .../include/Box2D/Collision/b2Collision.h | 312 ++- .../include/Box2D/Collision/b2Distance.h | 164 +- .../include/Box2D/Collision/b2DynamicTree.h | 448 ++-- .../include/Box2D/Collision/b2TimeOfImpact.h | 69 +- .../include/Box2D/Common/b2BlockAllocator.h | 63 +- flatland_box2d/include/Box2D/Common/b2Draw.h | 141 +- .../include/Box2D/Common/b2GrowableStack.h | 118 +- flatland_box2d/include/Box2D/Common/b2Math.h | 981 ++++--- .../include/Box2D/Common/b2Settings.h | 125 +- .../include/Box2D/Common/b2StackAllocator.h | 63 +- flatland_box2d/include/Box2D/Common/b2Timer.h | 56 +- .../Contacts/b2ChainAndCircleContact.h | 45 +- .../Contacts/b2ChainAndPolygonContact.h | 45 +- .../Box2D/Dynamics/Contacts/b2CircleContact.h | 45 +- .../Box2D/Dynamics/Contacts/b2Contact.h | 405 ++- .../Box2D/Dynamics/Contacts/b2ContactSolver.h | 119 +- .../Contacts/b2EdgeAndCircleContact.h | 45 +- .../Contacts/b2EdgeAndPolygonContact.h | 45 +- .../Contacts/b2PolygonAndCircleContact.h | 44 +- .../Dynamics/Contacts/b2PolygonContact.h | 45 +- .../Box2D/Dynamics/Joints/b2DistanceJoint.h | 234 +- .../Box2D/Dynamics/Joints/b2FrictionJoint.h | 177 +- .../Box2D/Dynamics/Joints/b2GearJoint.h | 159 +- .../include/Box2D/Dynamics/Joints/b2Joint.h | 266 +- .../Box2D/Dynamics/Joints/b2MotorJoint.h | 200 +- .../Box2D/Dynamics/Joints/b2MouseJoint.h | 183 +- .../Box2D/Dynamics/Joints/b2PrismaticJoint.h | 291 +- .../Box2D/Dynamics/Joints/b2PulleyJoint.h | 221 +- .../Box2D/Dynamics/Joints/b2RevoluteJoint.h | 296 +-- .../Box2D/Dynamics/Joints/b2RopeJoint.h | 153 +- .../Box2D/Dynamics/Joints/b2WeldJoint.h | 189 +- .../Box2D/Dynamics/Joints/b2WheelJoint.h | 316 ++- .../include/Box2D/Dynamics/b2Body.h | 1297 +++++---- .../include/Box2D/Dynamics/b2ContactManager.h | 58 +- .../include/Box2D/Dynamics/b2Fixture.h | 486 ++-- .../include/Box2D/Dynamics/b2Island.h | 141 +- .../include/Box2D/Dynamics/b2TimeStep.h | 74 +- .../include/Box2D/Dynamics/b2World.h | 532 ++-- .../include/Box2D/Dynamics/b2WorldCallbacks.h | 176 +- flatland_box2d/include/Box2D/Rope/b2Rope.h | 145 +- .../src/Collision/Shapes/b2ChainShape.cpp | 284 +- .../src/Collision/Shapes/b2CircleShape.cpp | 144 +- .../src/Collision/Shapes/b2EdgeShape.cpp | 206 +- .../src/Collision/Shapes/b2PolygonShape.cpp | 782 +++--- flatland_box2d/src/Collision/b2BroadPhase.cpp | 155 +- .../src/Collision/b2CollideCircle.cpp | 269 +- .../src/Collision/b2CollideEdge.cpp | 1157 ++++---- .../src/Collision/b2CollidePolygon.cpp | 425 ++- flatland_box2d/src/Collision/b2Collision.cpp | 429 ++- flatland_box2d/src/Collision/b2Distance.cpp | 1013 ++++--- .../src/Collision/b2DynamicTree.cpp | 1243 ++++----- .../src/Collision/b2TimeOfImpact.cpp | 837 +++--- .../src/Common/b2BlockAllocator.cpp | 318 ++- flatland_box2d/src/Common/b2Draw.cpp | 57 +- flatland_box2d/src/Common/b2Math.cpp | 134 +- flatland_box2d/src/Common/b2Settings.cpp | 55 +- .../src/Common/b2StackAllocator.cpp | 112 +- flatland_box2d/src/Common/b2Timer.cpp | 108 +- .../Contacts/b2ChainAndCircleContact.cpp | 74 +- .../Contacts/b2ChainAndPolygonContact.cpp | 74 +- .../src/Dynamics/Contacts/b2CircleContact.cpp | 71 +- .../src/Dynamics/Contacts/b2Contact.cpp | 390 ++- .../src/Dynamics/Contacts/b2ContactSolver.cpp | 1472 +++++------ .../Contacts/b2EdgeAndCircleContact.cpp | 65 +- .../Contacts/b2EdgeAndPolygonContact.cpp | 65 +- .../Contacts/b2PolygonAndCircleContact.cpp | 65 +- .../Dynamics/Contacts/b2PolygonContact.cpp | 69 +- .../src/Dynamics/Joints/b2DistanceJoint.cpp | 392 ++- .../src/Dynamics/Joints/b2FrictionJoint.cpp | 344 ++- .../src/Dynamics/Joints/b2GearJoint.cpp | 695 +++-- .../src/Dynamics/Joints/b2Joint.cpp | 356 ++- .../src/Dynamics/Joints/b2MotorJoint.cpp | 415 ++- .../src/Dynamics/Joints/b2MouseJoint.cpp | 317 +-- .../src/Dynamics/Joints/b2PrismaticJoint.cpp | 981 ++++--- .../src/Dynamics/Joints/b2PulleyJoint.cpp | 528 ++-- .../src/Dynamics/Joints/b2RevoluteJoint.cpp | 801 +++--- .../src/Dynamics/Joints/b2RopeJoint.cpp | 357 ++- .../src/Dynamics/Joints/b2WeldJoint.cpp | 542 ++-- .../src/Dynamics/Joints/b2WheelJoint.cpp | 709 +++-- flatland_box2d/src/Dynamics/b2Body.cpp | 927 +++---- .../src/Dynamics/b2ContactManager.cpp | 508 ++-- flatland_box2d/src/Dynamics/b2Fixture.cpp | 480 ++-- flatland_box2d/src/Dynamics/b2Island.cpp | 766 +++--- flatland_box2d/src/Dynamics/b2World.cpp | 2338 ++++++++--------- .../src/Dynamics/b2WorldCallbacks.cpp | 56 +- flatland_box2d/src/Rope/b2Rope.cpp | 380 ++- .../include/flatland_plugins/bool_sensor.h | 21 +- .../include/flatland_plugins/bumper.h | 43 +- .../include/flatland_plugins/diff_drive.h | 19 +- .../include/flatland_plugins/gps.h | 56 +- .../include/flatland_plugins/laser.h | 73 +- .../flatland_plugins/model_tf_publisher.h | 35 +- .../include/flatland_plugins/tricycle_drive.h | 43 +- .../include/flatland_plugins/tween.h | 21 +- .../include/flatland_plugins/update_timer.h | 15 +- .../include/flatland_plugins/world_modifier.h | 101 +- .../flatland_plugins/world_random_wall.h | 11 +- .../include/thirdparty/ThreadPool.h | 139 +- .../include/thirdparty/tweeny/dispatcher.h | 71 +- .../include/thirdparty/tweeny/easing.h | 1040 ++++---- .../include/thirdparty/tweeny/easingresolve.h | 182 +- .../include/thirdparty/tweeny/int2type.h | 38 +- .../include/thirdparty/tweeny/tween.h | 1209 +++++---- .../include/thirdparty/tweeny/tweenpoint.h | 113 +- .../include/thirdparty/tweeny/tweentraits.h | 125 +- .../include/thirdparty/tweeny/tweeny.h | 95 +- flatland_plugins/src/bool_sensor.cpp | 41 +- flatland_plugins/src/bumper.cpp | 86 +- flatland_plugins/src/diff_drive.cpp | 88 +- flatland_plugins/src/gps.cpp | 29 +- flatland_plugins/src/laser.cpp | 102 +- flatland_plugins/src/model_tf_publisher.cpp | 64 +- flatland_plugins/src/tricycle_drive.cpp | 143 +- flatland_plugins/src/tween.cpp | 124 +- flatland_plugins/src/update_timer.cpp | 20 +- flatland_plugins/src/world_modifier.cpp | 63 +- flatland_plugins/src/world_random_wall.cpp | 40 +- flatland_plugins/test/bumper_test.cpp | 139 +- flatland_plugins/test/diff_drive_test.cpp | 15 +- flatland_plugins/test/gps_test.cpp | 15 +- flatland_plugins/test/laser_test.cpp | 159 +- .../test/model_tf_publisher_test.cpp | 154 +- flatland_plugins/test/tricycle_drive_test.cpp | 15 +- flatland_plugins/test/tween_test.cpp | 54 +- flatland_plugins/test/update_timer_test.cpp | 45 +- .../change_rate_tool.hpp | 33 +- .../interactive_tool.hpp | 10 +- .../flatland_rviz_plugins/pause_tool.hpp | 10 +- .../spawn_model_tool.hpp | 36 +- .../src/change_rate_dialog.cpp | 42 +- .../src/change_rate_dialog.hpp | 21 +- .../src/change_rate_tool.cpp | 22 +- .../src/interactive_tool.cpp | 25 +- .../src/load_model_dialog.cpp | 85 +- .../src/load_model_dialog.hpp | 29 +- flatland_rviz_plugins/src/pause_tool.cpp | 14 +- .../src/spawn_model_tool.cpp | 139 +- .../include/flatland_server/body.h | 34 +- .../collision_filter_registry.h | 16 +- .../flatland_server/debug_visualization.h | 43 +- .../flatland_server/dummy_model_plugin.h | 12 +- .../flatland_server/dummy_world_plugin.h | 12 +- .../include/flatland_server/entity.h | 23 +- .../include/flatland_server/exceptions.h | 58 +- .../include/flatland_server/flatland_plugin.h | 41 +- .../include/flatland_server/geometry.h | 17 +- .../interactive_marker_manager.h | 93 +- .../include/flatland_server/joint.h | 61 +- .../include/flatland_server/layer.h | 80 +- .../include/flatland_server/model.h | 50 +- .../include/flatland_server/model_body.h | 36 +- .../include/flatland_server/model_plugin.h | 36 +- .../include/flatland_server/plugin_manager.h | 38 +- .../include/flatland_server/service_manager.h | 75 +- .../flatland_server/simulation_manager.h | 20 +- .../include/flatland_server/timekeeper.h | 25 +- .../include/flatland_server/types.h | 50 +- .../include/flatland_server/world.h | 70 +- .../include/flatland_server/world_plugin.h | 55 +- .../flatland_server/yaml_preprocessor.h | 90 +- .../include/flatland_server/yaml_reader.h | 145 +- flatland_server/src/body.cpp | 56 +- .../src/collision_filter_registry.cpp | 39 +- flatland_server/src/debug_visualization.cpp | 126 +- flatland_server/src/dummy_model_plugin.cpp | 25 +- flatland_server/src/dummy_world_plugin.cpp | 26 +- flatland_server/src/entity.cpp | 17 +- flatland_server/src/flatland_server_node.cpp | 75 +- flatland_server/src/geometry.cpp | 16 +- .../src/interactive_marker_manager.cpp | 110 +- flatland_server/src/joint.cpp | 115 +- flatland_server/src/layer.cpp | 144 +- flatland_server/src/model.cpp | 203 +- flatland_server/src/model_body.cpp | 87 +- flatland_server/src/model_plugin.cpp | 36 +- flatland_server/src/plugin_manager.cpp | 151 +- flatland_server/src/service_manager.cpp | 133 +- flatland_server/src/simulation_manager.cpp | 70 +- flatland_server/src/timekeeper.cpp | 26 +- flatland_server/src/world.cpp | 146 +- flatland_server/src/world_plugin.cpp | 16 +- flatland_server/src/yaml_preprocessor.cpp | 92 +- flatland_server/src/yaml_reader.cpp | 103 +- .../test/collision_filter_registry_test.cpp | 35 +- .../test/debug_visualization_test.cpp | 124 +- .../test/dummy_model_plugin_test.cpp | 18 +- .../test/dummy_world_plugin_test.cpp | 18 +- flatland_server/test/geometry_test.cpp | 26 +- flatland_server/test/load_world_test.cpp | 604 +++-- flatland_server/test/model_test.cpp | 27 +- flatland_server/test/null.cpp | 3 +- flatland_server/test/plugin_manager_test.cpp | 263 +- flatland_server/test/service_manager_test.cpp | 158 +- .../yaml_preprocessor_test.cpp | 40 +- .../include/flatland_viz/flatland_viz.h | 86 +- .../include/flatland_viz/flatland_window.h | 33 +- .../include/flatland_viz/load_model_dialog.h | 51 +- .../include/flatland_viz/model_dialog.h | 31 +- .../include/flatland_viz/pause_sim_tool.h | 12 +- .../include/flatland_viz/spawn_model_tool.h | 70 +- flatland_viz/src/flatland_viz.cpp | 156 +- flatland_viz/src/flatland_viz_node.cpp | 12 +- flatland_viz/src/flatland_window.cpp | 10 +- flatland_viz/src/load_model_dialog.cpp | 85 +- flatland_viz/src/model_dialog.cpp | 69 +- flatland_viz/src/pause_sim_tool.cpp | 19 +- flatland_viz/src/spawn_model_tool.cpp | 134 +- 214 files changed, 20581 insertions(+), 21738 deletions(-) diff --git a/flatland_box2d/include/Box2D/Box2D.h b/flatland_box2d/include/Box2D/Box2D.h index 0602763b..043504a8 100644 --- a/flatland_box2d/include/Box2D/Box2D.h +++ b/flatland_box2d/include/Box2D/Box2D.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef BOX2D_H #define BOX2D_H @@ -31,28 +31,18 @@ For discussion please visit http://box2d.org/forum // These include files constitute the main Box2D API -#include "Box2D/Common/b2Settings.h" -#include "Box2D/Common/b2Draw.h" -#include "Box2D/Common/b2Timer.h" - +#include "Box2D/Collision/Shapes/b2ChainShape.h" #include "Box2D/Collision/Shapes/b2CircleShape.h" #include "Box2D/Collision/Shapes/b2EdgeShape.h" -#include "Box2D/Collision/Shapes/b2ChainShape.h" #include "Box2D/Collision/Shapes/b2PolygonShape.h" - #include "Box2D/Collision/b2BroadPhase.h" #include "Box2D/Collision/b2Distance.h" #include "Box2D/Collision/b2DynamicTree.h" #include "Box2D/Collision/b2TimeOfImpact.h" - -#include "Box2D/Dynamics/b2Body.h" -#include "Box2D/Dynamics/b2Fixture.h" -#include "Box2D/Dynamics/b2WorldCallbacks.h" -#include "Box2D/Dynamics/b2TimeStep.h" -#include "Box2D/Dynamics/b2World.h" - +#include "Box2D/Common/b2Draw.h" +#include "Box2D/Common/b2Settings.h" +#include "Box2D/Common/b2Timer.h" #include "Box2D/Dynamics/Contacts/b2Contact.h" - #include "Box2D/Dynamics/Joints/b2DistanceJoint.h" #include "Box2D/Dynamics/Joints/b2FrictionJoint.h" #include "Box2D/Dynamics/Joints/b2GearJoint.h" @@ -64,5 +54,10 @@ For discussion please visit http://box2d.org/forum #include "Box2D/Dynamics/Joints/b2RopeJoint.h" #include "Box2D/Dynamics/Joints/b2WeldJoint.h" #include "Box2D/Dynamics/Joints/b2WheelJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2TimeStep.h" +#include "Box2D/Dynamics/b2World.h" +#include "Box2D/Dynamics/b2WorldCallbacks.h" #endif diff --git a/flatland_box2d/include/Box2D/Collision/Shapes/b2ChainShape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2ChainShape.h index 8342ca1c..5facb1f5 100644 --- a/flatland_box2d/include/Box2D/Collision/Shapes/b2ChainShape.h +++ b/flatland_box2d/include/Box2D/Collision/Shapes/b2ChainShape.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_CHAIN_SHAPE_H #define B2_CHAIN_SHAPE_H @@ -24,82 +24,83 @@ class b2EdgeShape; /// A chain shape is a free form sequence of line segments. -/// The chain has two-sided collision, so you can use inside and outside collision. -/// Therefore, you may use any winding order. -/// Since there may be many vertices, they are allocated using b2Alloc. -/// Connectivity information is used to create smooth collisions. -/// WARNING: The chain will not collide properly if there are self-intersections. +/// The chain has two-sided collision, so you can use inside and outside +/// collision. Therefore, you may use any winding order. Since there may be many +/// vertices, they are allocated using b2Alloc. Connectivity information is used +/// to create smooth collisions. WARNING: The chain will not collide properly if +/// there are self-intersections. class b2ChainShape : public b2Shape { public: - b2ChainShape(); + b2ChainShape(); - /// The destructor frees the vertices using b2Free. - ~b2ChainShape(); + /// The destructor frees the vertices using b2Free. + ~b2ChainShape(); - /// Clear all data. - void Clear(); + /// Clear all data. + void Clear(); - /// Create a loop. This automatically adjusts connectivity. - /// @param vertices an array of vertices, these are copied - /// @param count the vertex count - void CreateLoop(const b2Vec2* vertices, int32 count); + /// Create a loop. This automatically adjusts connectivity. + /// @param vertices an array of vertices, these are copied + /// @param count the vertex count + void CreateLoop(const b2Vec2 * vertices, int32 count); - /// Create a chain with isolated end vertices. - /// @param vertices an array of vertices, these are copied - /// @param count the vertex count - void CreateChain(const b2Vec2* vertices, int32 count); + /// Create a chain with isolated end vertices. + /// @param vertices an array of vertices, these are copied + /// @param count the vertex count + void CreateChain(const b2Vec2 * vertices, int32 count); - /// Establish connectivity to a vertex that precedes the first vertex. - /// Don't call this for loops. - void SetPrevVertex(const b2Vec2& prevVertex); + /// Establish connectivity to a vertex that precedes the first vertex. + /// Don't call this for loops. + void SetPrevVertex(const b2Vec2 & prevVertex); - /// Establish connectivity to a vertex that follows the last vertex. - /// Don't call this for loops. - void SetNextVertex(const b2Vec2& nextVertex); + /// Establish connectivity to a vertex that follows the last vertex. + /// Don't call this for loops. + void SetNextVertex(const b2Vec2 & nextVertex); - /// Implement b2Shape. Vertices are cloned using b2Alloc. - b2Shape* Clone(b2BlockAllocator* allocator) const override; + /// Implement b2Shape. Vertices are cloned using b2Alloc. + b2Shape * Clone(b2BlockAllocator * allocator) const override; - /// @see b2Shape::GetChildCount - int32 GetChildCount() const override; + /// @see b2Shape::GetChildCount + int32 GetChildCount() const override; - /// Get a child edge. - void GetChildEdge(b2EdgeShape* edge, int32 index) const; + /// Get a child edge. + void GetChildEdge(b2EdgeShape * edge, int32 index) const; - /// This always return false. - /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; + /// This always return false. + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform & transform, const b2Vec2 & p) const override; - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const override; + /// Implement b2Shape. + bool RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, const b2Transform & transform, + int32 childIndex) const override; - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB * aabb, const b2Transform & transform, int32 childIndex) const override; - /// Chains have zero mass. - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float32 density) const override; + /// Chains have zero mass. + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData * massData, float32 density) const override; - /// The vertices. Owned by this class. - b2Vec2* m_vertices; + /// The vertices. Owned by this class. + b2Vec2 * m_vertices; - /// The vertex count. - int32 m_count; + /// The vertex count. + int32 m_count; - b2Vec2 m_prevVertex, m_nextVertex; - bool m_hasPrevVertex, m_hasNextVertex; + b2Vec2 m_prevVertex, m_nextVertex; + bool m_hasPrevVertex, m_hasNextVertex; }; inline b2ChainShape::b2ChainShape() { - m_type = e_chain; - m_radius = b2_polygonRadius; - m_vertices = nullptr; - m_count = 0; - m_hasPrevVertex = false; - m_hasNextVertex = false; + m_type = e_chain; + m_radius = b2_polygonRadius; + m_vertices = nullptr; + m_count = 0; + m_hasPrevVertex = false; + m_hasNextVertex = false; } #endif diff --git a/flatland_box2d/include/Box2D/Collision/Shapes/b2CircleShape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2CircleShape.h index 76bbe8ee..fe08e97a 100644 --- a/flatland_box2d/include/Box2D/Collision/Shapes/b2CircleShape.h +++ b/flatland_box2d/include/Box2D/Collision/Shapes/b2CircleShape.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_CIRCLE_SHAPE_H #define B2_CIRCLE_SHAPE_H @@ -25,36 +25,37 @@ class b2CircleShape : public b2Shape { public: - b2CircleShape(); + b2CircleShape(); - /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const override; + /// Implement b2Shape. + b2Shape * Clone(b2BlockAllocator * allocator) const override; - /// @see b2Shape::GetChildCount - int32 GetChildCount() const override; + /// @see b2Shape::GetChildCount + int32 GetChildCount() const override; - /// Implement b2Shape. - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; + /// Implement b2Shape. + bool TestPoint(const b2Transform & transform, const b2Vec2 & p) const override; - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const override; + /// Implement b2Shape. + bool RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, const b2Transform & transform, + int32 childIndex) const override; - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB * aabb, const b2Transform & transform, int32 childIndex) const override; - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float32 density) const override; + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData * massData, float32 density) const override; - /// Position - b2Vec2 m_p; + /// Position + b2Vec2 m_p; }; inline b2CircleShape::b2CircleShape() { - m_type = e_circle; - m_radius = 0.0f; - m_p.SetZero(); + m_type = e_circle; + m_radius = 0.0f; + m_p.SetZero(); } #endif diff --git a/flatland_box2d/include/Box2D/Collision/Shapes/b2EdgeShape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2EdgeShape.h index ab8057c9..18ad0a74 100644 --- a/flatland_box2d/include/Box2D/Collision/Shapes/b2EdgeShape.h +++ b/flatland_box2d/include/Box2D/Collision/Shapes/b2EdgeShape.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_EDGE_SHAPE_H #define B2_EDGE_SHAPE_H @@ -27,48 +27,49 @@ class b2EdgeShape : public b2Shape { public: - b2EdgeShape(); + b2EdgeShape(); - /// Set this as an isolated edge. - void Set(const b2Vec2& v1, const b2Vec2& v2); + /// Set this as an isolated edge. + void Set(const b2Vec2 & v1, const b2Vec2 & v2); - /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const override; + /// Implement b2Shape. + b2Shape * Clone(b2BlockAllocator * allocator) const override; - /// @see b2Shape::GetChildCount - int32 GetChildCount() const override; + /// @see b2Shape::GetChildCount + int32 GetChildCount() const override; - /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform & transform, const b2Vec2 & p) const override; - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const override; + /// Implement b2Shape. + bool RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, const b2Transform & transform, + int32 childIndex) const override; - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB * aabb, const b2Transform & transform, int32 childIndex) const override; - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float32 density) const override; - - /// These are the edge vertices - b2Vec2 m_vertex1, m_vertex2; + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData * massData, float32 density) const override; - /// Optional adjacent vertices. These are used for smooth collision. - b2Vec2 m_vertex0, m_vertex3; - bool m_hasVertex0, m_hasVertex3; + /// These are the edge vertices + b2Vec2 m_vertex1, m_vertex2; + + /// Optional adjacent vertices. These are used for smooth collision. + b2Vec2 m_vertex0, m_vertex3; + bool m_hasVertex0, m_hasVertex3; }; inline b2EdgeShape::b2EdgeShape() { - m_type = e_edge; - m_radius = b2_polygonRadius; - m_vertex0.x = 0.0f; - m_vertex0.y = 0.0f; - m_vertex3.x = 0.0f; - m_vertex3.y = 0.0f; - m_hasVertex0 = false; - m_hasVertex3 = false; + m_type = e_edge; + m_radius = b2_polygonRadius; + m_vertex0.x = 0.0f; + m_vertex0.y = 0.0f; + m_vertex3.x = 0.0f; + m_vertex3.y = 0.0f; + m_hasVertex0 = false; + m_hasVertex3 = false; } #endif diff --git a/flatland_box2d/include/Box2D/Collision/Shapes/b2PolygonShape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2PolygonShape.h index 5f6e6d6d..7712517a 100644 --- a/flatland_box2d/include/Box2D/Collision/Shapes/b2PolygonShape.h +++ b/flatland_box2d/include/Box2D/Collision/Shapes/b2PolygonShape.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_POLYGON_SHAPE_H #define B2_POLYGON_SHAPE_H @@ -28,62 +28,64 @@ class b2PolygonShape : public b2Shape { public: - b2PolygonShape(); - - /// Implement b2Shape. - b2Shape* Clone(b2BlockAllocator* allocator) const override; - - /// @see b2Shape::GetChildCount - int32 GetChildCount() const override; - - /// Create a convex hull from the given array of local points. - /// The count must be in the range [3, b2_maxPolygonVertices]. - /// @warning the points may be re-ordered, even if they form a convex polygon - /// @warning collinear points are handled but not removed. Collinear points - /// may lead to poor stacking behavior. - void Set(const b2Vec2* points, int32 count); - - /// Build vertices to represent an axis-aligned box centered on the local origin. - /// @param hx the half-width. - /// @param hy the half-height. - void SetAsBox(float32 hx, float32 hy); - - /// Build vertices to represent an oriented box. - /// @param hx the half-width. - /// @param hy the half-height. - /// @param center the center of the box in local coordinates. - /// @param angle the rotation of the box in local coordinates. - void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle); - - /// @see b2Shape::TestPoint - bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override; - - /// Implement b2Shape. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const override; - - /// @see b2Shape::ComputeAABB - void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override; - - /// @see b2Shape::ComputeMass - void ComputeMass(b2MassData* massData, float32 density) const override; - - /// Validate convexity. This is a very time consuming operation. - /// @returns true if valid - bool Validate() const; - - b2Vec2 m_centroid; - b2Vec2 m_vertices[b2_maxPolygonVertices]; - b2Vec2 m_normals[b2_maxPolygonVertices]; - int32 m_count; + b2PolygonShape(); + + /// Implement b2Shape. + b2Shape * Clone(b2BlockAllocator * allocator) const override; + + /// @see b2Shape::GetChildCount + int32 GetChildCount() const override; + + /// Create a convex hull from the given array of local points. + /// The count must be in the range [3, b2_maxPolygonVertices]. + /// @warning the points may be re-ordered, even if they form a convex polygon + /// @warning collinear points are handled but not removed. Collinear points + /// may lead to poor stacking behavior. + void Set(const b2Vec2 * points, int32 count); + + /// Build vertices to represent an axis-aligned box centered on the local + /// origin. + /// @param hx the half-width. + /// @param hy the half-height. + void SetAsBox(float32 hx, float32 hy); + + /// Build vertices to represent an oriented box. + /// @param hx the half-width. + /// @param hy the half-height. + /// @param center the center of the box in local coordinates. + /// @param angle the rotation of the box in local coordinates. + void SetAsBox(float32 hx, float32 hy, const b2Vec2 & center, float32 angle); + + /// @see b2Shape::TestPoint + bool TestPoint(const b2Transform & transform, const b2Vec2 & p) const override; + + /// Implement b2Shape. + bool RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, const b2Transform & transform, + int32 childIndex) const override; + + /// @see b2Shape::ComputeAABB + void ComputeAABB(b2AABB * aabb, const b2Transform & transform, int32 childIndex) const override; + + /// @see b2Shape::ComputeMass + void ComputeMass(b2MassData * massData, float32 density) const override; + + /// Validate convexity. This is a very time consuming operation. + /// @returns true if valid + bool Validate() const; + + b2Vec2 m_centroid; + b2Vec2 m_vertices[b2_maxPolygonVertices]; + b2Vec2 m_normals[b2_maxPolygonVertices]; + int32 m_count; }; inline b2PolygonShape::b2PolygonShape() { - m_type = e_polygon; - m_radius = b2_polygonRadius; - m_count = 0; - m_centroid.SetZero(); + m_type = e_polygon; + m_radius = b2_polygonRadius; + m_count = 0; + m_centroid.SetZero(); } #endif diff --git a/flatland_box2d/include/Box2D/Collision/Shapes/b2Shape.h b/flatland_box2d/include/Box2D/Collision/Shapes/b2Shape.h index 9eb3d234..677f808e 100644 --- a/flatland_box2d/include/Box2D/Collision/Shapes/b2Shape.h +++ b/flatland_box2d/include/Box2D/Collision/Shapes/b2Shape.h @@ -1,104 +1,97 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_SHAPE_H #define B2_SHAPE_H +#include "Box2D/Collision/b2Collision.h" #include "Box2D/Common/b2BlockAllocator.h" #include "Box2D/Common/b2Math.h" -#include "Box2D/Collision/b2Collision.h" /// This holds the mass data computed for a shape. struct b2MassData { - /// The mass of the shape, usually in kilograms. - float32 mass; + /// The mass of the shape, usually in kilograms. + float32 mass; - /// The position of the shape's centroid relative to the shape's origin. - b2Vec2 center; + /// The position of the shape's centroid relative to the shape's origin. + b2Vec2 center; - /// The rotational inertia of the shape about the local origin. - float32 I; + /// The rotational inertia of the shape about the local origin. + float32 I; }; -/// A shape is used for collision detection. You can create a shape however you like. -/// Shapes used for simulation in b2World are created automatically when a b2Fixture -/// is created. Shapes may encapsulate a one or more child shapes. +/// A shape is used for collision detection. You can create a shape however you +/// like. Shapes used for simulation in b2World are created automatically when a +/// b2Fixture is created. Shapes may encapsulate a one or more child shapes. class b2Shape { public: - - enum Type - { - e_circle = 0, - e_edge = 1, - e_polygon = 2, - e_chain = 3, - e_typeCount = 4 - }; - - virtual ~b2Shape() {} - - /// Clone the concrete shape using the provided allocator. - virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0; - - /// Get the type of this shape. You can use this to down cast to the concrete shape. - /// @return the shape type. - Type GetType() const; - - /// Get the number of child primitives. - virtual int32 GetChildCount() const = 0; - - /// Test a point for containment in this shape. This only works for convex shapes. - /// @param xf the shape world transform. - /// @param p a point in world coordinates. - virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0; - - /// Cast a ray against a child shape. - /// @param output the ray-cast results. - /// @param input the ray-cast input parameters. - /// @param transform the transform to be applied to the shape. - /// @param childIndex the child shape index - virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const = 0; - - /// Given a transform, compute the associated axis aligned bounding box for a child shape. - /// @param aabb returns the axis aligned box. - /// @param xf the world transform of the shape. - /// @param childIndex the child shape - virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0; - - /// Compute the mass properties of this shape using its dimensions and density. - /// The inertia tensor is computed about the local origin. - /// @param massData returns the mass data for this shape. - /// @param density the density in kilograms per meter squared. - virtual void ComputeMass(b2MassData* massData, float32 density) const = 0; - - Type m_type; - - /// Radius of a shape. For polygonal shapes this must be b2_polygonRadius. There is no support for - /// making rounded polygons. - float32 m_radius; + enum Type { e_circle = 0, e_edge = 1, e_polygon = 2, e_chain = 3, e_typeCount = 4 }; + + virtual ~b2Shape() {} + + /// Clone the concrete shape using the provided allocator. + virtual b2Shape * Clone(b2BlockAllocator * allocator) const = 0; + + /// Get the type of this shape. You can use this to down cast to the concrete + /// shape. + /// @return the shape type. + Type GetType() const; + + /// Get the number of child primitives. + virtual int32 GetChildCount() const = 0; + + /// Test a point for containment in this shape. This only works for convex + /// shapes. + /// @param xf the shape world transform. + /// @param p a point in world coordinates. + virtual bool TestPoint(const b2Transform & xf, const b2Vec2 & p) const = 0; + + /// Cast a ray against a child shape. + /// @param output the ray-cast results. + /// @param input the ray-cast input parameters. + /// @param transform the transform to be applied to the shape. + /// @param childIndex the child shape index + virtual bool RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, const b2Transform & transform, + int32 childIndex) const = 0; + + /// Given a transform, compute the associated axis aligned bounding box for a + /// child shape. + /// @param aabb returns the axis aligned box. + /// @param xf the world transform of the shape. + /// @param childIndex the child shape + virtual void ComputeAABB(b2AABB * aabb, const b2Transform & xf, int32 childIndex) const = 0; + + /// Compute the mass properties of this shape using its dimensions and + /// density. The inertia tensor is computed about the local origin. + /// @param massData returns the mass data for this shape. + /// @param density the density in kilograms per meter squared. + virtual void ComputeMass(b2MassData * massData, float32 density) const = 0; + + Type m_type; + + /// Radius of a shape. For polygonal shapes this must be b2_polygonRadius. + /// There is no support for making rounded polygons. + float32 m_radius; }; -inline b2Shape::Type b2Shape::GetType() const -{ - return m_type; -} +inline b2Shape::Type b2Shape::GetType() const { return m_type; } #endif diff --git a/flatland_box2d/include/Box2D/Collision/b2BroadPhase.h b/flatland_box2d/include/Box2D/Collision/b2BroadPhase.h index 85b35823..f0d95b55 100644 --- a/flatland_box2d/include/Box2D/Collision/b2BroadPhase.h +++ b/flatland_box2d/include/Box2D/Collision/b2BroadPhase.h @@ -1,257 +1,232 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_BROAD_PHASE_H #define B2_BROAD_PHASE_H -#include "Box2D/Common/b2Settings.h" +#include + #include "Box2D/Collision/b2Collision.h" #include "Box2D/Collision/b2DynamicTree.h" -#include +#include "Box2D/Common/b2Settings.h" struct b2Pair { - int32 proxyIdA; - int32 proxyIdB; + int32 proxyIdA; + int32 proxyIdB; }; -/// The broad-phase is used for computing pairs and performing volume queries and ray casts. -/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. -/// It is up to the client to consume the new pairs and to track subsequent overlap. +/// The broad-phase is used for computing pairs and performing volume queries +/// and ray casts. This broad-phase does not persist pairs. Instead, this +/// reports potentially new pairs. It is up to the client to consume the new +/// pairs and to track subsequent overlap. class b2BroadPhase { public: + enum { e_nullProxy = -1 }; - enum - { - e_nullProxy = -1 - }; + b2BroadPhase(); + ~b2BroadPhase(); - b2BroadPhase(); - ~b2BroadPhase(); + /// Create a proxy with an initial AABB. Pairs are not reported until + /// UpdatePairs is called. + int32 CreateProxy(const b2AABB & aabb, void * userData); - /// Create a proxy with an initial AABB. Pairs are not reported until - /// UpdatePairs is called. - int32 CreateProxy(const b2AABB& aabb, void* userData); + /// Destroy a proxy. It is up to the client to remove any pairs. + void DestroyProxy(int32 proxyId); - /// Destroy a proxy. It is up to the client to remove any pairs. - void DestroyProxy(int32 proxyId); + /// Call MoveProxy as many times as you like, then when you are done + /// call UpdatePairs to finalized the proxy pairs (for your time step). + void MoveProxy(int32 proxyId, const b2AABB & aabb, const b2Vec2 & displacement); - /// Call MoveProxy as many times as you like, then when you are done - /// call UpdatePairs to finalized the proxy pairs (for your time step). - void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement); + /// Call to trigger a re-processing of it's pairs on the next call to + /// UpdatePairs. + void TouchProxy(int32 proxyId); - /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs. - void TouchProxy(int32 proxyId); + /// Get the fat AABB for a proxy. + const b2AABB & GetFatAABB(int32 proxyId) const; - /// Get the fat AABB for a proxy. - const b2AABB& GetFatAABB(int32 proxyId) const; + /// Get user data from a proxy. Returns nullptr if the id is invalid. + void * GetUserData(int32 proxyId) const; - /// Get user data from a proxy. Returns nullptr if the id is invalid. - void* GetUserData(int32 proxyId) const; + /// Test overlap of fat AABBs. + bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const; - /// Test overlap of fat AABBs. - bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const; + /// Get the number of proxies. + int32 GetProxyCount() const; - /// Get the number of proxies. - int32 GetProxyCount() const; + /// Update the pairs. This results in pair callbacks. This can only add pairs. + template + void UpdatePairs(T * callback); - /// Update the pairs. This results in pair callbacks. This can only add pairs. - template - void UpdatePairs(T* callback); + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + template + void Query(T * callback, const b2AABB & aabb) const; - /// Query an AABB for overlapping proxies. The callback class - /// is called for each proxy that overlaps the supplied AABB. - template - void Query(T* callback, const b2AABB& aabb) const; + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a shape. + /// The callback also performs the any collision filtering. This has + /// performance roughly equal to k * log(n), where k is the number of + /// collisions and n is the number of proxies in the tree. + /// @param input the ray-cast input data. The ray extends from p1 to p1 + + /// maxFraction * (p2 - p1). + /// @param callback a callback class that is called for each proxy that is hit + /// by the ray. + template + void RayCast(T * callback, const b2RayCastInput & input) const; - /// Ray-cast against the proxies in the tree. This relies on the callback - /// to perform a exact ray-cast in the case were the proxy contains a shape. - /// The callback also performs the any collision filtering. This has performance - /// roughly equal to k * log(n), where k is the number of collisions and n is the - /// number of proxies in the tree. - /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). - /// @param callback a callback class that is called for each proxy that is hit by the ray. - template - void RayCast(T* callback, const b2RayCastInput& input) const; + /// Get the height of the embedded tree. + int32 GetTreeHeight() const; - /// Get the height of the embedded tree. - int32 GetTreeHeight() const; + /// Get the balance of the embedded tree. + int32 GetTreeBalance() const; - /// Get the balance of the embedded tree. - int32 GetTreeBalance() const; + /// Get the quality metric of the embedded tree. + float32 GetTreeQuality() const; - /// Get the quality metric of the embedded tree. - float32 GetTreeQuality() const; - - /// Shift the world origin. Useful for large worlds. - /// The shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); + /// Shift the world origin. Useful for large worlds. + /// The shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + void ShiftOrigin(const b2Vec2 & newOrigin); private: + friend class b2DynamicTree; - friend class b2DynamicTree; - - void BufferMove(int32 proxyId); - void UnBufferMove(int32 proxyId); + void BufferMove(int32 proxyId); + void UnBufferMove(int32 proxyId); - bool QueryCallback(int32 proxyId); + bool QueryCallback(int32 proxyId); - b2DynamicTree m_tree; + b2DynamicTree m_tree; - int32 m_proxyCount; + int32 m_proxyCount; - int32* m_moveBuffer; - int32 m_moveCapacity; - int32 m_moveCount; + int32 * m_moveBuffer; + int32 m_moveCapacity; + int32 m_moveCount; - b2Pair* m_pairBuffer; - int32 m_pairCapacity; - int32 m_pairCount; + b2Pair * m_pairBuffer; + int32 m_pairCapacity; + int32 m_pairCount; - int32 m_queryProxyId; + int32 m_queryProxyId; }; /// This is used to sort pairs. -inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2) +inline bool b2PairLessThan(const b2Pair & pair1, const b2Pair & pair2) { - if (pair1.proxyIdA < pair2.proxyIdA) - { - return true; - } + if (pair1.proxyIdA < pair2.proxyIdA) { + return true; + } - if (pair1.proxyIdA == pair2.proxyIdA) - { - return pair1.proxyIdB < pair2.proxyIdB; - } + if (pair1.proxyIdA == pair2.proxyIdA) { + return pair1.proxyIdB < pair2.proxyIdB; + } - return false; + return false; } -inline void* b2BroadPhase::GetUserData(int32 proxyId) const -{ - return m_tree.GetUserData(proxyId); -} +inline void * b2BroadPhase::GetUserData(int32 proxyId) const { return m_tree.GetUserData(proxyId); } inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const { - const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA); - const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB); - return b2TestOverlap(aabbA, aabbB); + const b2AABB & aabbA = m_tree.GetFatAABB(proxyIdA); + const b2AABB & aabbB = m_tree.GetFatAABB(proxyIdB); + return b2TestOverlap(aabbA, aabbB); } -inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const +inline const b2AABB & b2BroadPhase::GetFatAABB(int32 proxyId) const { - return m_tree.GetFatAABB(proxyId); + return m_tree.GetFatAABB(proxyId); } -inline int32 b2BroadPhase::GetProxyCount() const -{ - return m_proxyCount; -} +inline int32 b2BroadPhase::GetProxyCount() const { return m_proxyCount; } -inline int32 b2BroadPhase::GetTreeHeight() const -{ - return m_tree.GetHeight(); -} +inline int32 b2BroadPhase::GetTreeHeight() const { return m_tree.GetHeight(); } -inline int32 b2BroadPhase::GetTreeBalance() const -{ - return m_tree.GetMaxBalance(); -} +inline int32 b2BroadPhase::GetTreeBalance() const { return m_tree.GetMaxBalance(); } -inline float32 b2BroadPhase::GetTreeQuality() const -{ - return m_tree.GetAreaRatio(); -} +inline float32 b2BroadPhase::GetTreeQuality() const { return m_tree.GetAreaRatio(); } template -void b2BroadPhase::UpdatePairs(T* callback) +void b2BroadPhase::UpdatePairs(T * callback) { - // Reset pair buffer - m_pairCount = 0; - - // Perform tree queries for all moving proxies. - for (int32 i = 0; i < m_moveCount; ++i) - { - m_queryProxyId = m_moveBuffer[i]; - if (m_queryProxyId == e_nullProxy) - { - continue; - } - - // We have to query the tree with the fat AABB so that - // we don't fail to create a pair that may touch later. - const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId); - - // Query tree, create pairs and add them pair buffer. - m_tree.Query(this, fatAABB); - } - - // Reset move buffer - m_moveCount = 0; - - // Sort the pair buffer to expose duplicates. - std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan); - - // Send the pairs back to the client. - int32 i = 0; - while (i < m_pairCount) - { - b2Pair* primaryPair = m_pairBuffer + i; - void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA); - void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB); - - callback->AddPair(userDataA, userDataB); - ++i; - - // Skip any duplicate pairs. - while (i < m_pairCount) - { - b2Pair* pair = m_pairBuffer + i; - if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB) - { - break; - } - ++i; - } - } - - // Try to keep the tree balanced. - //m_tree.Rebalance(4); + // Reset pair buffer + m_pairCount = 0; + + // Perform tree queries for all moving proxies. + for (int32 i = 0; i < m_moveCount; ++i) { + m_queryProxyId = m_moveBuffer[i]; + if (m_queryProxyId == e_nullProxy) { + continue; + } + + // We have to query the tree with the fat AABB so that + // we don't fail to create a pair that may touch later. + const b2AABB & fatAABB = m_tree.GetFatAABB(m_queryProxyId); + + // Query tree, create pairs and add them pair buffer. + m_tree.Query(this, fatAABB); + } + + // Reset move buffer + m_moveCount = 0; + + // Sort the pair buffer to expose duplicates. + std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan); + + // Send the pairs back to the client. + int32 i = 0; + while (i < m_pairCount) { + b2Pair * primaryPair = m_pairBuffer + i; + void * userDataA = m_tree.GetUserData(primaryPair->proxyIdA); + void * userDataB = m_tree.GetUserData(primaryPair->proxyIdB); + + callback->AddPair(userDataA, userDataB); + ++i; + + // Skip any duplicate pairs. + while (i < m_pairCount) { + b2Pair * pair = m_pairBuffer + i; + if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB) { + break; + } + ++i; + } + } + + // Try to keep the tree balanced. + // m_tree.Rebalance(4); } template -inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const +inline void b2BroadPhase::Query(T * callback, const b2AABB & aabb) const { - m_tree.Query(callback, aabb); + m_tree.Query(callback, aabb); } template -inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const +inline void b2BroadPhase::RayCast(T * callback, const b2RayCastInput & input) const { - m_tree.RayCast(callback, input); + m_tree.RayCast(callback, input); } -inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin) -{ - m_tree.ShiftOrigin(newOrigin); -} +inline void b2BroadPhase::ShiftOrigin(const b2Vec2 & newOrigin) { m_tree.ShiftOrigin(newOrigin); } #endif diff --git a/flatland_box2d/include/Box2D/Collision/b2Collision.h b/flatland_box2d/include/Box2D/Collision/b2Collision.h index 19d1fef4..92ef9b77 100644 --- a/flatland_box2d/include/Box2D/Collision/b2Collision.h +++ b/flatland_box2d/include/Box2D/Collision/b2Collision.h @@ -1,27 +1,28 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_COLLISION_H #define B2_COLLISION_H -#include "Box2D/Common/b2Math.h" #include +#include "Box2D/Common/b2Math.h" + /// @file /// Structures and functions used for computing contact points, distance /// queries, and TOI queries. @@ -37,23 +38,18 @@ const uint8 b2_nullFeature = UCHAR_MAX; /// This must be 4 bytes or less. struct b2ContactFeature { - enum Type - { - e_vertex = 0, - e_face = 1 - }; - - uint8 indexA; ///< Feature index on shapeA - uint8 indexB; ///< Feature index on shapeB - uint8 typeA; ///< The feature type on shapeA - uint8 typeB; ///< The feature type on shapeB + enum Type { e_vertex = 0, e_face = 1 }; + + uint8 indexA; ///< Feature index on shapeA + uint8 indexB; ///< Feature index on shapeB + uint8 typeA; ///< The feature type on shapeA + uint8 typeB; ///< The feature type on shapeB }; /// Contact ids to facilitate warm starting. -union b2ContactID -{ - b2ContactFeature cf; - uint32 key; ///< Used to quickly compare contact ids. +union b2ContactID { + b2ContactFeature cf; + uint32 key; ///< Used to quickly compare contact ids. }; /// A manifold point is a contact point belonging to a contact @@ -68,10 +64,10 @@ union b2ContactID /// provide reliable contact forces, especially for high speed collisions. struct b2ManifoldPoint { - b2Vec2 localPoint; ///< usage depends on manifold type - float32 normalImpulse; ///< the non-penetration impulse - float32 tangentImpulse; ///< the friction impulse - b2ContactID id; ///< uniquely identifies a contact point between two shapes + b2Vec2 localPoint; ///< usage depends on manifold type + float32 normalImpulse; ///< the non-penetration impulse + float32 tangentImpulse; ///< the friction impulse + b2ContactID id; ///< uniquely identifies a contact point between two shapes }; /// A manifold for two touching convex shapes. @@ -92,186 +88,178 @@ struct b2ManifoldPoint /// This structure is stored across time steps, so we keep it small. struct b2Manifold { - enum Type - { - e_circles, - e_faceA, - e_faceB - }; - - b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact - b2Vec2 localNormal; ///< not use for Type::e_points - b2Vec2 localPoint; ///< usage depends on manifold type - Type type; - int32 pointCount; ///< the number of manifold points + enum Type { e_circles, e_faceA, e_faceB }; + + b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact + b2Vec2 localNormal; ///< not use for Type::e_points + b2Vec2 localPoint; ///< usage depends on manifold type + Type type; + int32 pointCount; ///< the number of manifold points }; /// This is used to compute the current state of a contact manifold. struct b2WorldManifold { - /// Evaluate the manifold with supplied transforms. This assumes - /// modest motion from the original state. This does not change the - /// point count, impulses, etc. The radii must come from the shapes - /// that generated the manifold. - void Initialize(const b2Manifold* manifold, - const b2Transform& xfA, float32 radiusA, - const b2Transform& xfB, float32 radiusB); - - b2Vec2 normal; ///< world vector pointing from A to B - b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection) - float32 separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters + /// Evaluate the manifold with supplied transforms. This assumes + /// modest motion from the original state. This does not change the + /// point count, impulses, etc. The radii must come from the shapes + /// that generated the manifold. + void Initialize( + const b2Manifold * manifold, const b2Transform & xfA, float32 radiusA, const b2Transform & xfB, + float32 radiusB); + + b2Vec2 normal; ///< world vector pointing from A to B + b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of + ///< intersection) + float32 separations[b2_maxManifoldPoints]; ///< a negative value indicates + ///< overlap, in meters }; /// This is used for determining the state of contact points. -enum b2PointState -{ - b2_nullState, ///< point does not exist - b2_addState, ///< point was added in the update - b2_persistState, ///< point persisted across the update - b2_removeState ///< point was removed in the update +enum b2PointState { + b2_nullState, ///< point does not exist + b2_addState, ///< point was added in the update + b2_persistState, ///< point persisted across the update + b2_removeState ///< point was removed in the update }; -/// Compute the point states given two manifolds. The states pertain to the transition from manifold1 -/// to manifold2. So state1 is either persist or remove while state2 is either add or persist. -void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], - const b2Manifold* manifold1, const b2Manifold* manifold2); +/// Compute the point states given two manifolds. The states pertain to the +/// transition from manifold1 to manifold2. So state1 is either persist or +/// remove while state2 is either add or persist. +void b2GetPointStates( + b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], + const b2Manifold * manifold1, const b2Manifold * manifold2); /// Used for computing contact manifolds. struct b2ClipVertex { - b2Vec2 v; - b2ContactID id; + b2Vec2 v; + b2ContactID id; }; -/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). +/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - +/// p1). struct b2RayCastInput { - b2Vec2 p1, p2; - float32 maxFraction; + b2Vec2 p1, p2; + float32 maxFraction; }; -/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2 -/// come from b2RayCastInput. +/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 +/// and p2 come from b2RayCastInput. struct b2RayCastOutput { - b2Vec2 normal; - float32 fraction; + b2Vec2 normal; + float32 fraction; }; /// An axis aligned bounding box. struct b2AABB { - /// Verify that the bounds are sorted. - bool IsValid() const; - - /// Get the center of the AABB. - b2Vec2 GetCenter() const - { - return 0.5f * (lowerBound + upperBound); - } - - /// Get the extents of the AABB (half-widths). - b2Vec2 GetExtents() const - { - return 0.5f * (upperBound - lowerBound); - } - - /// Get the perimeter length - float32 GetPerimeter() const - { - float32 wx = upperBound.x - lowerBound.x; - float32 wy = upperBound.y - lowerBound.y; - return 2.0f * (wx + wy); - } - - /// Combine an AABB into this one. - void Combine(const b2AABB& aabb) - { - lowerBound = b2Min(lowerBound, aabb.lowerBound); - upperBound = b2Max(upperBound, aabb.upperBound); - } - - /// Combine two AABBs into this one. - void Combine(const b2AABB& aabb1, const b2AABB& aabb2) - { - lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound); - upperBound = b2Max(aabb1.upperBound, aabb2.upperBound); - } - - /// Does this aabb contain the provided AABB. - bool Contains(const b2AABB& aabb) const - { - bool result = true; - result = result && lowerBound.x <= aabb.lowerBound.x; - result = result && lowerBound.y <= aabb.lowerBound.y; - result = result && aabb.upperBound.x <= upperBound.x; - result = result && aabb.upperBound.y <= upperBound.y; - return result; - } - - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const; - - b2Vec2 lowerBound; ///< the lower vertex - b2Vec2 upperBound; ///< the upper vertex + /// Verify that the bounds are sorted. + bool IsValid() const; + + /// Get the center of the AABB. + b2Vec2 GetCenter() const { return 0.5f * (lowerBound + upperBound); } + + /// Get the extents of the AABB (half-widths). + b2Vec2 GetExtents() const { return 0.5f * (upperBound - lowerBound); } + + /// Get the perimeter length + float32 GetPerimeter() const + { + float32 wx = upperBound.x - lowerBound.x; + float32 wy = upperBound.y - lowerBound.y; + return 2.0f * (wx + wy); + } + + /// Combine an AABB into this one. + void Combine(const b2AABB & aabb) + { + lowerBound = b2Min(lowerBound, aabb.lowerBound); + upperBound = b2Max(upperBound, aabb.upperBound); + } + + /// Combine two AABBs into this one. + void Combine(const b2AABB & aabb1, const b2AABB & aabb2) + { + lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound); + upperBound = b2Max(aabb1.upperBound, aabb2.upperBound); + } + + /// Does this aabb contain the provided AABB. + bool Contains(const b2AABB & aabb) const + { + bool result = true; + result = result && lowerBound.x <= aabb.lowerBound.x; + result = result && lowerBound.y <= aabb.lowerBound.y; + result = result && aabb.upperBound.x <= upperBound.x; + result = result && aabb.upperBound.y <= upperBound.y; + return result; + } + + bool RayCast(b2RayCastOutput * output, const b2RayCastInput & input) const; + + b2Vec2 lowerBound; ///< the lower vertex + b2Vec2 upperBound; ///< the upper vertex }; /// Compute the collision manifold between two circles. -void b2CollideCircles(b2Manifold* manifold, - const b2CircleShape* circleA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); +void b2CollideCircles( + b2Manifold * manifold, const b2CircleShape * circleA, const b2Transform & xfA, + const b2CircleShape * circleB, const b2Transform & xfB); /// Compute the collision manifold between a polygon and a circle. -void b2CollidePolygonAndCircle(b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); +void b2CollidePolygonAndCircle( + b2Manifold * manifold, const b2PolygonShape * polygonA, const b2Transform & xfA, + const b2CircleShape * circleB, const b2Transform & xfB); /// Compute the collision manifold between two polygons. -void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB); +void b2CollidePolygons( + b2Manifold * manifold, const b2PolygonShape * polygonA, const b2Transform & xfA, + const b2PolygonShape * polygonB, const b2Transform & xfB); /// Compute the collision manifold between an edge and a circle. -void b2CollideEdgeAndCircle(b2Manifold* manifold, - const b2EdgeShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB); +void b2CollideEdgeAndCircle( + b2Manifold * manifold, const b2EdgeShape * polygonA, const b2Transform & xfA, + const b2CircleShape * circleB, const b2Transform & xfB); /// Compute the collision manifold between an edge and a circle. -void b2CollideEdgeAndPolygon(b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* circleB, const b2Transform& xfB); +void b2CollideEdgeAndPolygon( + b2Manifold * manifold, const b2EdgeShape * edgeA, const b2Transform & xfA, + const b2PolygonShape * circleB, const b2Transform & xfB); /// Clipping for contact manifolds. -int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset, int32 vertexIndexA); +int32 b2ClipSegmentToLine( + b2ClipVertex vOut[2], const b2ClipVertex vIn[2], const b2Vec2 & normal, float32 offset, + int32 vertexIndexA); /// Determine if two generic shapes overlap. -bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, - const b2Shape* shapeB, int32 indexB, - const b2Transform& xfA, const b2Transform& xfB); +bool b2TestOverlap( + const b2Shape * shapeA, int32 indexA, const b2Shape * shapeB, int32 indexB, + const b2Transform & xfA, const b2Transform & xfB); // ---------------- Inline Functions ------------------------------------------ inline bool b2AABB::IsValid() const { - b2Vec2 d = upperBound - lowerBound; - bool valid = d.x >= 0.0f && d.y >= 0.0f; - valid = valid && lowerBound.IsValid() && upperBound.IsValid(); - return valid; + b2Vec2 d = upperBound - lowerBound; + bool valid = d.x >= 0.0f && d.y >= 0.0f; + valid = valid && lowerBound.IsValid() && upperBound.IsValid(); + return valid; } -inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b) +inline bool b2TestOverlap(const b2AABB & a, const b2AABB & b) { - b2Vec2 d1, d2; - d1 = b.lowerBound - a.upperBound; - d2 = a.lowerBound - b.upperBound; + b2Vec2 d1, d2; + d1 = b.lowerBound - a.upperBound; + d2 = a.lowerBound - b.upperBound; - if (d1.x > 0.0f || d1.y > 0.0f) - return false; + if (d1.x > 0.0f || d1.y > 0.0f) return false; - if (d2.x > 0.0f || d2.y > 0.0f) - return false; + if (d2.x > 0.0f || d2.y > 0.0f) return false; - return true; + return true; } #endif diff --git a/flatland_box2d/include/Box2D/Collision/b2Distance.h b/flatland_box2d/include/Box2D/Collision/b2Distance.h index 538997e4..2d6ea2eb 100644 --- a/flatland_box2d/include/Box2D/Collision/b2Distance.h +++ b/flatland_box2d/include/Box2D/Collision/b2Distance.h @@ -1,21 +1,21 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_DISTANCE_H #define B2_DISTANCE_H @@ -28,114 +28,104 @@ class b2Shape; /// It encapsulates any shape. struct b2DistanceProxy { - b2DistanceProxy() : m_vertices(nullptr), m_count(0), m_radius(0.0f) {} + b2DistanceProxy() : m_vertices(nullptr), m_count(0), m_radius(0.0f) {} - /// Initialize the proxy using the given shape. The shape - /// must remain in scope while the proxy is in use. - void Set(const b2Shape* shape, int32 index); + /// Initialize the proxy using the given shape. The shape + /// must remain in scope while the proxy is in use. + void Set(const b2Shape * shape, int32 index); - /// Get the supporting vertex index in the given direction. - int32 GetSupport(const b2Vec2& d) const; + /// Get the supporting vertex index in the given direction. + int32 GetSupport(const b2Vec2 & d) const; - /// Get the supporting vertex in the given direction. - const b2Vec2& GetSupportVertex(const b2Vec2& d) const; + /// Get the supporting vertex in the given direction. + const b2Vec2 & GetSupportVertex(const b2Vec2 & d) const; - /// Get the vertex count. - int32 GetVertexCount() const; + /// Get the vertex count. + int32 GetVertexCount() const; - /// Get a vertex by index. Used by b2Distance. - const b2Vec2& GetVertex(int32 index) const; + /// Get a vertex by index. Used by b2Distance. + const b2Vec2 & GetVertex(int32 index) const; - b2Vec2 m_buffer[2]; - const b2Vec2* m_vertices; - int32 m_count; - float32 m_radius; + b2Vec2 m_buffer[2]; + const b2Vec2 * m_vertices; + int32 m_count; + float32 m_radius; }; /// Used to warm start b2Distance. /// Set count to zero on first call. struct b2SimplexCache { - float32 metric; ///< length or area - uint16 count; - uint8 indexA[3]; ///< vertices on shape A - uint8 indexB[3]; ///< vertices on shape B + float32 metric; ///< length or area + uint16 count; + uint8 indexA[3]; ///< vertices on shape A + uint8 indexB[3]; ///< vertices on shape B }; /// Input for b2Distance. /// You have to option to use the shape radii -/// in the computation. Even +/// in the computation. Even struct b2DistanceInput { - b2DistanceProxy proxyA; - b2DistanceProxy proxyB; - b2Transform transformA; - b2Transform transformB; - bool useRadii; + b2DistanceProxy proxyA; + b2DistanceProxy proxyB; + b2Transform transformA; + b2Transform transformB; + bool useRadii; }; /// Output for b2Distance. struct b2DistanceOutput { - b2Vec2 pointA; ///< closest point on shapeA - b2Vec2 pointB; ///< closest point on shapeB - float32 distance; - int32 iterations; ///< number of GJK iterations used + b2Vec2 pointA; ///< closest point on shapeA + b2Vec2 pointB; ///< closest point on shapeB + float32 distance; + int32 iterations; ///< number of GJK iterations used }; /// Compute the closest points between two shapes. Supports any combination of: -/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output. -/// On the first call set b2SimplexCache.count to zero. -void b2Distance(b2DistanceOutput* output, - b2SimplexCache* cache, - const b2DistanceInput* input); - +/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is +/// input/output. On the first call set b2SimplexCache.count to zero. +void b2Distance(b2DistanceOutput * output, b2SimplexCache * cache, const b2DistanceInput * input); ////////////////////////////////////////////////////////////////////////// -inline int32 b2DistanceProxy::GetVertexCount() const -{ - return m_count; -} +inline int32 b2DistanceProxy::GetVertexCount() const { return m_count; } -inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const +inline const b2Vec2 & b2DistanceProxy::GetVertex(int32 index) const { - b2Assert(0 <= index && index < m_count); - return m_vertices[index]; + b2Assert(0 <= index && index < m_count); + return m_vertices[index]; } -inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const +inline int32 b2DistanceProxy::GetSupport(const b2Vec2 & d) const { - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_count; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return bestIndex; + int32 bestIndex = 0; + float32 bestValue = b2Dot(m_vertices[0], d); + for (int32 i = 1; i < m_count; ++i) { + float32 value = b2Dot(m_vertices[i], d); + if (value > bestValue) { + bestIndex = i; + bestValue = value; + } + } + + return bestIndex; } -inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const +inline const b2Vec2 & b2DistanceProxy::GetSupportVertex(const b2Vec2 & d) const { - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_vertices[0], d); - for (int32 i = 1; i < m_count; ++i) - { - float32 value = b2Dot(m_vertices[i], d); - if (value > bestValue) - { - bestIndex = i; - bestValue = value; - } - } - - return m_vertices[bestIndex]; + int32 bestIndex = 0; + float32 bestValue = b2Dot(m_vertices[0], d); + for (int32 i = 1; i < m_count; ++i) { + float32 value = b2Dot(m_vertices[i], d); + if (value > bestValue) { + bestIndex = i; + bestValue = value; + } + } + + return m_vertices[bestIndex]; } #endif diff --git a/flatland_box2d/include/Box2D/Collision/b2DynamicTree.h b/flatland_box2d/include/Box2D/Collision/b2DynamicTree.h index eea00e31..45076837 100644 --- a/flatland_box2d/include/Box2D/Collision/b2DynamicTree.h +++ b/flatland_box2d/include/Box2D/Collision/b2DynamicTree.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_DYNAMIC_TREE_H #define B2_DYNAMIC_TREE_H @@ -27,263 +27,245 @@ /// A node in the dynamic tree. The client does not interact with this directly. struct b2TreeNode { - bool IsLeaf() const - { - return child1 == b2_nullNode; - } + bool IsLeaf() const { return child1 == b2_nullNode; } - /// Enlarged AABB - b2AABB aabb; + /// Enlarged AABB + b2AABB aabb; - void* userData; + void * userData; - union - { - int32 parent; - int32 next; - }; + union { + int32 parent; + int32 next; + }; - int32 child1; - int32 child2; + int32 child1; + int32 child2; - // leaf = 0, free node = -1 - int32 height; + // leaf = 0, free node = -1 + int32 height; }; /// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. /// A dynamic tree arranges data in a binary tree to accelerate /// queries such as volume queries and ray casts. Leafs are proxies /// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor -/// so that the proxy AABB is bigger than the client object. This allows the client -/// object to move by small amounts without triggering a tree update. +/// so that the proxy AABB is bigger than the client object. This allows the +/// client object to move by small amounts without triggering a tree update. /// -/// Nodes are pooled and relocatable, so we use node indices rather than pointers. +/// Nodes are pooled and relocatable, so we use node indices rather than +/// pointers. class b2DynamicTree { public: - /// Constructing the tree initializes the node pool. - b2DynamicTree(); - - /// Destroy the tree, freeing the node pool. - ~b2DynamicTree(); - - /// Create a proxy. Provide a tight fitting AABB and a userData pointer. - int32 CreateProxy(const b2AABB& aabb, void* userData); - - /// Destroy a proxy. This asserts if the id is invalid. - void DestroyProxy(int32 proxyId); - - /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, - /// then the proxy is removed from the tree and re-inserted. Otherwise - /// the function returns immediately. - /// @return true if the proxy was re-inserted. - bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement); - - /// Get proxy user data. - /// @return the proxy user data or 0 if the id is invalid. - void* GetUserData(int32 proxyId) const; - - /// Get the fat AABB for a proxy. - const b2AABB& GetFatAABB(int32 proxyId) const; - - /// Query an AABB for overlapping proxies. The callback class - /// is called for each proxy that overlaps the supplied AABB. - template - void Query(T* callback, const b2AABB& aabb) const; - - /// Ray-cast against the proxies in the tree. This relies on the callback - /// to perform a exact ray-cast in the case were the proxy contains a shape. - /// The callback also performs the any collision filtering. This has performance - /// roughly equal to k * log(n), where k is the number of collisions and n is the - /// number of proxies in the tree. - /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). - /// @param callback a callback class that is called for each proxy that is hit by the ray. - template - void RayCast(T* callback, const b2RayCastInput& input) const; - - /// Validate this tree. For testing. - void Validate() const; - - /// Compute the height of the binary tree in O(N) time. Should not be - /// called often. - int32 GetHeight() const; - - /// Get the maximum balance of an node in the tree. The balance is the difference - /// in height of the two children of a node. - int32 GetMaxBalance() const; - - /// Get the ratio of the sum of the node areas to the root area. - float32 GetAreaRatio() const; - - /// Build an optimal tree. Very expensive. For testing. - void RebuildBottomUp(); - - /// Shift the world origin. Useful for large worlds. - /// The shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); + /// Constructing the tree initializes the node pool. + b2DynamicTree(); + + /// Destroy the tree, freeing the node pool. + ~b2DynamicTree(); + + /// Create a proxy. Provide a tight fitting AABB and a userData pointer. + int32 CreateProxy(const b2AABB & aabb, void * userData); + + /// Destroy a proxy. This asserts if the id is invalid. + void DestroyProxy(int32 proxyId); + + /// Move a proxy with a swepted AABB. If the proxy has moved outside of its + /// fattened AABB, then the proxy is removed from the tree and re-inserted. + /// Otherwise the function returns immediately. + /// @return true if the proxy was re-inserted. + bool MoveProxy(int32 proxyId, const b2AABB & aabb1, const b2Vec2 & displacement); + + /// Get proxy user data. + /// @return the proxy user data or 0 if the id is invalid. + void * GetUserData(int32 proxyId) const; + + /// Get the fat AABB for a proxy. + const b2AABB & GetFatAABB(int32 proxyId) const; + + /// Query an AABB for overlapping proxies. The callback class + /// is called for each proxy that overlaps the supplied AABB. + template + void Query(T * callback, const b2AABB & aabb) const; + + /// Ray-cast against the proxies in the tree. This relies on the callback + /// to perform a exact ray-cast in the case were the proxy contains a shape. + /// The callback also performs the any collision filtering. This has + /// performance roughly equal to k * log(n), where k is the number of + /// collisions and n is the number of proxies in the tree. + /// @param input the ray-cast input data. The ray extends from p1 to p1 + + /// maxFraction * (p2 - p1). + /// @param callback a callback class that is called for each proxy that is hit + /// by the ray. + template + void RayCast(T * callback, const b2RayCastInput & input) const; + + /// Validate this tree. For testing. + void Validate() const; + + /// Compute the height of the binary tree in O(N) time. Should not be + /// called often. + int32 GetHeight() const; + + /// Get the maximum balance of an node in the tree. The balance is the + /// difference in height of the two children of a node. + int32 GetMaxBalance() const; + + /// Get the ratio of the sum of the node areas to the root area. + float32 GetAreaRatio() const; + + /// Build an optimal tree. Very expensive. For testing. + void RebuildBottomUp(); + + /// Shift the world origin. Useful for large worlds. + /// The shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + void ShiftOrigin(const b2Vec2 & newOrigin); private: + int32 AllocateNode(); + void FreeNode(int32 node); - int32 AllocateNode(); - void FreeNode(int32 node); - - void InsertLeaf(int32 node); - void RemoveLeaf(int32 node); + void InsertLeaf(int32 node); + void RemoveLeaf(int32 node); - int32 Balance(int32 index); + int32 Balance(int32 index); - int32 ComputeHeight() const; - int32 ComputeHeight(int32 nodeId) const; + int32 ComputeHeight() const; + int32 ComputeHeight(int32 nodeId) const; - void ValidateStructure(int32 index) const; - void ValidateMetrics(int32 index) const; + void ValidateStructure(int32 index) const; + void ValidateMetrics(int32 index) const; - int32 m_root; + int32 m_root; - b2TreeNode* m_nodes; - int32 m_nodeCount; - int32 m_nodeCapacity; + b2TreeNode * m_nodes; + int32 m_nodeCount; + int32 m_nodeCapacity; - int32 m_freeList; + int32 m_freeList; - /// This is used to incrementally traverse the tree for re-balancing. - uint32 m_path; + /// This is used to incrementally traverse the tree for re-balancing. + uint32 m_path; - int32 m_insertionCount; + int32 m_insertionCount; }; -inline void* b2DynamicTree::GetUserData(int32 proxyId) const +inline void * b2DynamicTree::GetUserData(int32 proxyId) const { - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].userData; + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + return m_nodes[proxyId].userData; } -inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const +inline const b2AABB & b2DynamicTree::GetFatAABB(int32 proxyId) const { - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - return m_nodes[proxyId].aabb; + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + return m_nodes[proxyId].aabb; } template -inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const +inline void b2DynamicTree::Query(T * callback, const b2AABB & aabb) const { - b2GrowableStack stack; - stack.Push(m_root); - - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } - - const b2TreeNode* node = m_nodes + nodeId; - - if (b2TestOverlap(node->aabb, aabb)) - { - if (node->IsLeaf()) - { - bool proceed = callback->QueryCallback(nodeId); - if (proceed == false) - { - return; - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } - } - } + b2GrowableStack stack; + stack.Push(m_root); + + while (stack.GetCount() > 0) { + int32 nodeId = stack.Pop(); + if (nodeId == b2_nullNode) { + continue; + } + + const b2TreeNode * node = m_nodes + nodeId; + + if (b2TestOverlap(node->aabb, aabb)) { + if (node->IsLeaf()) { + bool proceed = callback->QueryCallback(nodeId); + if (proceed == false) { + return; + } + } else { + stack.Push(node->child1); + stack.Push(node->child2); + } + } + } } template -inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const +inline void b2DynamicTree::RayCast(T * callback, const b2RayCastInput & input) const { - b2Vec2 p1 = input.p1; - b2Vec2 p2 = input.p2; - b2Vec2 r = p2 - p1; - b2Assert(r.LengthSquared() > 0.0f); - r.Normalize(); - - // v is perpendicular to the segment. - b2Vec2 v = b2Cross(1.0f, r); - b2Vec2 abs_v = b2Abs(v); - - // Separating axis for segment (Gino, p80). - // |dot(v, p1 - c)| > dot(|v|, h) - - float32 maxFraction = input.maxFraction; - - // Build a bounding box for the segment. - b2AABB segmentAABB; - { - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); - } - - b2GrowableStack stack; - stack.Push(m_root); - - while (stack.GetCount() > 0) - { - int32 nodeId = stack.Pop(); - if (nodeId == b2_nullNode) - { - continue; - } - - const b2TreeNode* node = m_nodes + nodeId; - - if (b2TestOverlap(node->aabb, segmentAABB) == false) - { - continue; - } - - // Separating axis for segment (Gino, p80). - // |dot(v, p1 - c)| > dot(|v|, h) - b2Vec2 c = node->aabb.GetCenter(); - b2Vec2 h = node->aabb.GetExtents(); - float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); - if (separation > 0.0f) - { - continue; - } - - if (node->IsLeaf()) - { - b2RayCastInput subInput; - subInput.p1 = input.p1; - subInput.p2 = input.p2; - subInput.maxFraction = maxFraction; - - float32 value = callback->RayCastCallback(subInput, nodeId); - - if (value == 0.0f) - { - // The client has terminated the ray cast. - return; - } - - if (value > 0.0f) - { - // Update segment bounding box. - maxFraction = value; - b2Vec2 t = p1 + maxFraction * (p2 - p1); - segmentAABB.lowerBound = b2Min(p1, t); - segmentAABB.upperBound = b2Max(p1, t); - } - } - else - { - stack.Push(node->child1); - stack.Push(node->child2); - } - } + b2Vec2 p1 = input.p1; + b2Vec2 p2 = input.p2; + b2Vec2 r = p2 - p1; + b2Assert(r.LengthSquared() > 0.0f); + r.Normalize(); + + // v is perpendicular to the segment. + b2Vec2 v = b2Cross(1.0f, r); + b2Vec2 abs_v = b2Abs(v); + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float32 maxFraction = input.maxFraction; + + // Build a bounding box for the segment. + b2AABB segmentAABB; + { + b2Vec2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.lowerBound = b2Min(p1, t); + segmentAABB.upperBound = b2Max(p1, t); + } + + b2GrowableStack stack; + stack.Push(m_root); + + while (stack.GetCount() > 0) { + int32 nodeId = stack.Pop(); + if (nodeId == b2_nullNode) { + continue; + } + + const b2TreeNode * node = m_nodes + nodeId; + + if (b2TestOverlap(node->aabb, segmentAABB) == false) { + continue; + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + b2Vec2 c = node->aabb.GetCenter(); + b2Vec2 h = node->aabb.GetExtents(); + float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h); + if (separation > 0.0f) { + continue; + } + + if (node->IsLeaf()) { + b2RayCastInput subInput; + subInput.p1 = input.p1; + subInput.p2 = input.p2; + subInput.maxFraction = maxFraction; + + float32 value = callback->RayCastCallback(subInput, nodeId); + + if (value == 0.0f) { + // The client has terminated the ray cast. + return; + } + + if (value > 0.0f) { + // Update segment bounding box. + maxFraction = value; + b2Vec2 t = p1 + maxFraction * (p2 - p1); + segmentAABB.lowerBound = b2Min(p1, t); + segmentAABB.upperBound = b2Max(p1, t); + } + } else { + stack.Push(node->child1); + stack.Push(node->child2); + } + } } #endif diff --git a/flatland_box2d/include/Box2D/Collision/b2TimeOfImpact.h b/flatland_box2d/include/Box2D/Collision/b2TimeOfImpact.h index 5872fc25..1dd79da9 100644 --- a/flatland_box2d/include/Box2D/Collision/b2TimeOfImpact.h +++ b/flatland_box2d/include/Box2D/Collision/b2TimeOfImpact.h @@ -1,58 +1,51 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_TIME_OF_IMPACT_H #define B2_TIME_OF_IMPACT_H -#include "Box2D/Common/b2Math.h" #include "Box2D/Collision/b2Distance.h" +#include "Box2D/Common/b2Math.h" /// Input parameters for b2TimeOfImpact struct b2TOIInput { - b2DistanceProxy proxyA; - b2DistanceProxy proxyB; - b2Sweep sweepA; - b2Sweep sweepB; - float32 tMax; // defines sweep interval [0, tMax] + b2DistanceProxy proxyA; + b2DistanceProxy proxyB; + b2Sweep sweepA; + b2Sweep sweepB; + float32 tMax; // defines sweep interval [0, tMax] }; // Output parameters for b2TimeOfImpact. struct b2TOIOutput { - enum State - { - e_unknown, - e_failed, - e_overlapped, - e_touching, - e_separated - }; + enum State { e_unknown, e_failed, e_overlapped, e_touching, e_separated }; - State state; - float32 t; + State state; + float32 t; }; -/// Compute the upper bound on time before two shapes penetrate. Time is represented as -/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, -/// non-tunneling collision. If you change the time interval, you should call this function -/// again. -/// Note: use b2Distance to compute the contact point and normal at the time of impact. -void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input); +/// Compute the upper bound on time before two shapes penetrate. Time is +/// represented as a fraction between [0,tMax]. This uses a swept separating +/// axis and may miss some intermediate, non-tunneling collision. If you change +/// the time interval, you should call this function again. Note: use b2Distance +/// to compute the contact point and normal at the time of impact. +void b2TimeOfImpact(b2TOIOutput * output, const b2TOIInput * input); #endif diff --git a/flatland_box2d/include/Box2D/Common/b2BlockAllocator.h b/flatland_box2d/include/Box2D/Common/b2BlockAllocator.h index 09dbca9f..a467cafe 100644 --- a/flatland_box2d/include/Box2D/Common/b2BlockAllocator.h +++ b/flatland_box2d/include/Box2D/Common/b2BlockAllocator.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_BLOCK_ALLOCATOR_H #define B2_BLOCK_ALLOCATOR_H @@ -35,28 +35,29 @@ struct b2Chunk; class b2BlockAllocator { public: - b2BlockAllocator(); - ~b2BlockAllocator(); + b2BlockAllocator(); + ~b2BlockAllocator(); - /// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize. - void* Allocate(int32 size); + /// Allocate memory. This will use b2Alloc if the size is larger than + /// b2_maxBlockSize. + void * Allocate(int32 size); - /// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize. - void Free(void* p, int32 size); + /// Free memory. This will use b2Free if the size is larger than + /// b2_maxBlockSize. + void Free(void * p, int32 size); - void Clear(); + void Clear(); private: + b2Chunk * m_chunks; + int32 m_chunkCount; + int32 m_chunkSpace; - b2Chunk* m_chunks; - int32 m_chunkCount; - int32 m_chunkSpace; + b2Block * m_freeLists[b2_blockSizes]; - b2Block* m_freeLists[b2_blockSizes]; - - static int32 s_blockSizes[b2_blockSizes]; - static uint8 s_blockSizeLookup[b2_maxBlockSize + 1]; - static bool s_blockSizeLookupInitialized; + static int32 s_blockSizes[b2_blockSizes]; + static uint8 s_blockSizeLookup[b2_maxBlockSize + 1]; + static bool s_blockSizeLookupInitialized; }; #endif diff --git a/flatland_box2d/include/Box2D/Common/b2Draw.h b/flatland_box2d/include/Box2D/Common/b2Draw.h index 2a6885cb..aa39615a 100644 --- a/flatland_box2d/include/Box2D/Common/b2Draw.h +++ b/flatland_box2d/include/Box2D/Common/b2Draw.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2011 Erin Catto http://box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_DRAW_H #define B2_DRAW_H @@ -24,74 +24,81 @@ /// Color for debug drawing. Each value has the range [0,1]. struct b2Color { - b2Color() {} - b2Color(float32 rIn, float32 gIn, float32 bIn, float32 aIn = 1.0f) - { - r = rIn; g = gIn; b = bIn; a = aIn; - } - - void Set(float32 rIn, float32 gIn, float32 bIn, float32 aIn = 1.0f) - { - r = rIn; g = gIn; b = bIn; a = aIn; - } - - float32 r, g, b, a; + b2Color() {} + b2Color(float32 rIn, float32 gIn, float32 bIn, float32 aIn = 1.0f) + { + r = rIn; + g = gIn; + b = bIn; + a = aIn; + } + + void Set(float32 rIn, float32 gIn, float32 bIn, float32 aIn = 1.0f) + { + r = rIn; + g = gIn; + b = bIn; + a = aIn; + } + + float32 r, g, b, a; }; -/// Implement and register this class with a b2World to provide debug drawing of physics -/// entities in your game. +/// Implement and register this class with a b2World to provide debug drawing of +/// physics entities in your game. class b2Draw { public: - b2Draw(); + b2Draw(); - virtual ~b2Draw() {} + virtual ~b2Draw() {} - enum - { - e_shapeBit = 0x0001, ///< draw shapes - e_jointBit = 0x0002, ///< draw joint connections - e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes - e_pairBit = 0x0008, ///< draw broad-phase pairs - e_centerOfMassBit = 0x0010 ///< draw center of mass frame - }; + enum { + e_shapeBit = 0x0001, ///< draw shapes + e_jointBit = 0x0002, ///< draw joint connections + e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes + e_pairBit = 0x0008, ///< draw broad-phase pairs + e_centerOfMassBit = 0x0010 ///< draw center of mass frame + }; - /// Set the drawing flags. - void SetFlags(uint32 flags); + /// Set the drawing flags. + void SetFlags(uint32 flags); - /// Get the drawing flags. - uint32 GetFlags() const; - - /// Append flags to the current flags. - void AppendFlags(uint32 flags); + /// Get the drawing flags. + uint32 GetFlags() const; - /// Clear flags from the current flags. - void ClearFlags(uint32 flags); + /// Append flags to the current flags. + void AppendFlags(uint32 flags); - /// Draw a closed polygon provided in CCW order. - virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; + /// Clear flags from the current flags. + void ClearFlags(uint32 flags); - /// Draw a solid closed polygon provided in CCW order. - virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0; + /// Draw a closed polygon provided in CCW order. + virtual void DrawPolygon(const b2Vec2 * vertices, int32 vertexCount, const b2Color & color) = 0; - /// Draw a circle. - virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0; - - /// Draw a solid circle. - virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0; - - /// Draw a line segment. - virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0; + /// Draw a solid closed polygon provided in CCW order. + virtual void DrawSolidPolygon( + const b2Vec2 * vertices, int32 vertexCount, const b2Color & color) = 0; - /// Draw a transform. Choose your own length scale. - /// @param xf a transform. - virtual void DrawTransform(const b2Transform& xf) = 0; + /// Draw a circle. + virtual void DrawCircle(const b2Vec2 & center, float32 radius, const b2Color & color) = 0; - /// Draw a point. - virtual void DrawPoint(const b2Vec2& p, float32 size, const b2Color& color) = 0; + /// Draw a solid circle. + virtual void DrawSolidCircle( + const b2Vec2 & center, float32 radius, const b2Vec2 & axis, const b2Color & color) = 0; + + /// Draw a line segment. + virtual void DrawSegment(const b2Vec2 & p1, const b2Vec2 & p2, const b2Color & color) = 0; + + /// Draw a transform. Choose your own length scale. + /// @param xf a transform. + virtual void DrawTransform(const b2Transform & xf) = 0; + + /// Draw a point. + virtual void DrawPoint(const b2Vec2 & p, float32 size, const b2Color & color) = 0; protected: - uint32 m_drawFlags; + uint32 m_drawFlags; }; #endif diff --git a/flatland_box2d/include/Box2D/Common/b2GrowableStack.h b/flatland_box2d/include/Box2D/Common/b2GrowableStack.h index 8775ec74..ce7bbbd1 100644 --- a/flatland_box2d/include/Box2D/Common/b2GrowableStack.h +++ b/flatland_box2d/include/Box2D/Common/b2GrowableStack.h @@ -1,26 +1,27 @@ /* -* Copyright (c) 2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_GROWABLE_STACK_H #define B2_GROWABLE_STACK_H -#include "Box2D/Common/b2Settings.h" #include +#include "Box2D/Common/b2Settings.h" + /// This is a growable LIFO stack with an initial capacity of N. /// If the stack size exceeds the initial capacity, the heap is used /// to increase the size of the stack. @@ -28,58 +29,51 @@ template class b2GrowableStack { public: - b2GrowableStack() - { - m_stack = m_array; - m_count = 0; - m_capacity = N; - } + b2GrowableStack() + { + m_stack = m_array; + m_count = 0; + m_capacity = N; + } - ~b2GrowableStack() - { - if (m_stack != m_array) - { - b2Free(m_stack); - m_stack = nullptr; - } - } + ~b2GrowableStack() + { + if (m_stack != m_array) { + b2Free(m_stack); + m_stack = nullptr; + } + } - void Push(const T& element) - { - if (m_count == m_capacity) - { - T* old = m_stack; - m_capacity *= 2; - m_stack = (T*)b2Alloc(m_capacity * sizeof(T)); - memcpy(m_stack, old, m_count * sizeof(T)); - if (old != m_array) - { - b2Free(old); - } - } + void Push(const T & element) + { + if (m_count == m_capacity) { + T * old = m_stack; + m_capacity *= 2; + m_stack = (T *)b2Alloc(m_capacity * sizeof(T)); + memcpy(m_stack, old, m_count * sizeof(T)); + if (old != m_array) { + b2Free(old); + } + } - m_stack[m_count] = element; - ++m_count; - } + m_stack[m_count] = element; + ++m_count; + } - T Pop() - { - b2Assert(m_count > 0); - --m_count; - return m_stack[m_count]; - } + T Pop() + { + b2Assert(m_count > 0); + --m_count; + return m_stack[m_count]; + } - int32 GetCount() - { - return m_count; - } + int32 GetCount() { return m_count; } private: - T* m_stack; - T m_array[N]; - int32 m_count; - int32 m_capacity; + T * m_stack; + T m_array[N]; + int32 m_count; + int32 m_capacity; }; - #endif diff --git a/flatland_box2d/include/Box2D/Common/b2Math.h b/flatland_box2d/include/Box2D/Common/b2Math.h index 10a7908a..84f8058a 100644 --- a/flatland_box2d/include/Box2D/Common/b2Math.h +++ b/flatland_box2d/include/Box2D/Common/b2Math.h @@ -1,376 +1,392 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_MATH_H #define B2_MATH_H -#include "Box2D/Common/b2Settings.h" #include -/// This function is used to ensure that a floating point number is not a NaN or infinity. +#include "Box2D/Common/b2Settings.h" + +/// This function is used to ensure that a floating point number is not a NaN or +/// infinity. inline bool b2IsValid(float32 x) { - int32 ix = *reinterpret_cast(&x); - return (ix & 0x7f800000) != 0x7f800000; + int32 ix = *reinterpret_cast(&x); + return (ix & 0x7f800000) != 0x7f800000; } /// This is a approximate yet fast inverse square-root. inline float32 b2InvSqrt(float32 x) { - union - { - float32 x; - int32 i; - } convert; + union { + float32 x; + int32 i; + } convert; - convert.x = x; - float32 xhalf = 0.5f * x; - convert.i = 0x5f3759df - (convert.i >> 1); - x = convert.x; - x = x * (1.5f - xhalf * x * x); - return x; + convert.x = x; + float32 xhalf = 0.5f * x; + convert.i = 0x5f3759df - (convert.i >> 1); + x = convert.x; + x = x * (1.5f - xhalf * x * x); + return x; } -#define b2Sqrt(x) sqrtf(x) -#define b2Atan2(y, x) atan2f(y, x) +#define b2Sqrt(x) sqrtf(x) +#define b2Atan2(y, x) atan2f(y, x) /// A 2D column vector. struct b2Vec2 { - /// Default constructor does nothing (for performance). - b2Vec2() {} - - /// Construct using coordinates. - b2Vec2(float32 xIn, float32 yIn) : x(xIn), y(yIn) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float32 x_, float32 y_) { x = x_; y = y_; } - - /// Negate this vector. - b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; } - - /// Read from and indexed element. - float32 operator () (int32 i) const - { - return (&x)[i]; - } - - /// Write to an indexed element. - float32& operator () (int32 i) - { - return (&x)[i]; - } - - /// Add a vector to this vector. - void operator += (const b2Vec2& v) - { - x += v.x; y += v.y; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec2& v) - { - x -= v.x; y -= v.y; - } - - /// Multiply this vector by a scalar. - void operator *= (float32 a) - { - x *= a; y *= a; - } - - /// Get the length of this vector (the norm). - float32 Length() const - { - return b2Sqrt(x * x + y * y); - } - - /// Get the length squared. For performance, use this instead of - /// b2Vec2::Length (if possible). - float32 LengthSquared() const - { - return x * x + y * y; - } - - /// Convert this vector into a unit vector. Returns the length. - float32 Normalize() - { - float32 length = Length(); - if (length < b2_epsilon) - { - return 0.0f; - } - float32 invLength = 1.0f / length; - x *= invLength; - y *= invLength; - - return length; - } - - /// Does this vector contain finite coordinates? - bool IsValid() const - { - return b2IsValid(x) && b2IsValid(y); - } - - /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) - b2Vec2 Skew() const - { - return b2Vec2(-y, x); - } - - float32 x, y; + /// Default constructor does nothing (for performance). + b2Vec2() {} + + /// Construct using coordinates. + b2Vec2(float32 xIn, float32 yIn) : x(xIn), y(yIn) {} + + /// Set this vector to all zeros. + void SetZero() + { + x = 0.0f; + y = 0.0f; + } + + /// Set this vector to some specified coordinates. + void Set(float32 x_, float32 y_) + { + x = x_; + y = y_; + } + + /// Negate this vector. + b2Vec2 operator-() const + { + b2Vec2 v; + v.Set(-x, -y); + return v; + } + + /// Read from and indexed element. + float32 operator()(int32 i) const { return (&x)[i]; } + + /// Write to an indexed element. + float32 & operator()(int32 i) { return (&x)[i]; } + + /// Add a vector to this vector. + void operator+=(const b2Vec2 & v) + { + x += v.x; + y += v.y; + } + + /// Subtract a vector from this vector. + void operator-=(const b2Vec2 & v) + { + x -= v.x; + y -= v.y; + } + + /// Multiply this vector by a scalar. + void operator*=(float32 a) + { + x *= a; + y *= a; + } + + /// Get the length of this vector (the norm). + float32 Length() const { return b2Sqrt(x * x + y * y); } + + /// Get the length squared. For performance, use this instead of + /// b2Vec2::Length (if possible). + float32 LengthSquared() const { return x * x + y * y; } + + /// Convert this vector into a unit vector. Returns the length. + float32 Normalize() + { + float32 length = Length(); + if (length < b2_epsilon) { + return 0.0f; + } + float32 invLength = 1.0f / length; + x *= invLength; + y *= invLength; + + return length; + } + + /// Does this vector contain finite coordinates? + bool IsValid() const { return b2IsValid(x) && b2IsValid(y); } + + /// Get the skew vector such that dot(skew_vec, other) == cross(vec, other) + b2Vec2 Skew() const { return b2Vec2(-y, x); } + + float32 x, y; }; /// A 2D column vector with 3 elements. struct b2Vec3 { - /// Default constructor does nothing (for performance). - b2Vec3() {} - - /// Construct using coordinates. - b2Vec3(float32 xIn, float32 yIn, float32 zIn) : x(xIn), y(yIn), z(zIn) {} - - /// Set this vector to all zeros. - void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; } - - /// Set this vector to some specified coordinates. - void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; } - - /// Negate this vector. - b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; } - - /// Add a vector to this vector. - void operator += (const b2Vec3& v) - { - x += v.x; y += v.y; z += v.z; - } - - /// Subtract a vector from this vector. - void operator -= (const b2Vec3& v) - { - x -= v.x; y -= v.y; z -= v.z; - } - - /// Multiply this vector by a scalar. - void operator *= (float32 s) - { - x *= s; y *= s; z *= s; - } - - float32 x, y, z; + /// Default constructor does nothing (for performance). + b2Vec3() {} + + /// Construct using coordinates. + b2Vec3(float32 xIn, float32 yIn, float32 zIn) : x(xIn), y(yIn), z(zIn) {} + + /// Set this vector to all zeros. + void SetZero() + { + x = 0.0f; + y = 0.0f; + z = 0.0f; + } + + /// Set this vector to some specified coordinates. + void Set(float32 x_, float32 y_, float32 z_) + { + x = x_; + y = y_; + z = z_; + } + + /// Negate this vector. + b2Vec3 operator-() const + { + b2Vec3 v; + v.Set(-x, -y, -z); + return v; + } + + /// Add a vector to this vector. + void operator+=(const b2Vec3 & v) + { + x += v.x; + y += v.y; + z += v.z; + } + + /// Subtract a vector from this vector. + void operator-=(const b2Vec3 & v) + { + x -= v.x; + y -= v.y; + z -= v.z; + } + + /// Multiply this vector by a scalar. + void operator*=(float32 s) + { + x *= s; + y *= s; + z *= s; + } + + float32 x, y, z; }; /// A 2-by-2 matrix. Stored in column-major order. struct b2Mat22 { - /// The default constructor does nothing (for performance). - b2Mat22() {} - - /// Construct this matrix using columns. - b2Mat22(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Construct this matrix using scalars. - b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22) - { - ex.x = a11; ex.y = a21; - ey.x = a12; ey.y = a22; - } - - /// Initialize this matrix using columns. - void Set(const b2Vec2& c1, const b2Vec2& c2) - { - ex = c1; - ey = c2; - } - - /// Set this to the identity matrix. - void SetIdentity() - { - ex.x = 1.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 1.0f; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.x = 0.0f; ey.x = 0.0f; - ex.y = 0.0f; ey.y = 0.0f; - } - - b2Mat22 GetInverse() const - { - float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; - b2Mat22 B; - float32 det = a * d - b * c; - if (det != 0.0f) - { - det = 1.0f / det; - } - B.ex.x = det * d; B.ey.x = -det * b; - B.ex.y = -det * c; B.ey.y = det * a; - return B; - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec2 Solve(const b2Vec2& b) const - { - float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; - float32 det = a11 * a22 - a12 * a21; - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec2 x; - x.x = det * (a22 * b.x - a12 * b.y); - x.y = det * (a11 * b.y - a21 * b.x); - return x; - } - - b2Vec2 ex, ey; + /// The default constructor does nothing (for performance). + b2Mat22() {} + + /// Construct this matrix using columns. + b2Mat22(const b2Vec2 & c1, const b2Vec2 & c2) + { + ex = c1; + ey = c2; + } + + /// Construct this matrix using scalars. + b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22) + { + ex.x = a11; + ex.y = a21; + ey.x = a12; + ey.y = a22; + } + + /// Initialize this matrix using columns. + void Set(const b2Vec2 & c1, const b2Vec2 & c2) + { + ex = c1; + ey = c2; + } + + /// Set this to the identity matrix. + void SetIdentity() + { + ex.x = 1.0f; + ey.x = 0.0f; + ex.y = 0.0f; + ey.y = 1.0f; + } + + /// Set this matrix to all zeros. + void SetZero() + { + ex.x = 0.0f; + ey.x = 0.0f; + ex.y = 0.0f; + ey.y = 0.0f; + } + + b2Mat22 GetInverse() const + { + float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; + b2Mat22 B; + float32 det = a * d - b * c; + if (det != 0.0f) { + det = 1.0f / det; + } + B.ex.x = det * d; + B.ey.x = -det * b; + B.ex.y = -det * c; + B.ey.y = det * a; + return B; + } + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + b2Vec2 Solve(const b2Vec2 & b) const + { + float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; + float32 det = a11 * a22 - a12 * a21; + if (det != 0.0f) { + det = 1.0f / det; + } + b2Vec2 x; + x.x = det * (a22 * b.x - a12 * b.y); + x.y = det * (a11 * b.y - a21 * b.x); + return x; + } + + b2Vec2 ex, ey; }; /// A 3-by-3 matrix. Stored in column-major order. struct b2Mat33 { - /// The default constructor does nothing (for performance). - b2Mat33() {} - - /// Construct this matrix using columns. - b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3) - { - ex = c1; - ey = c2; - ez = c3; - } - - /// Set this matrix to all zeros. - void SetZero() - { - ex.SetZero(); - ey.SetZero(); - ez.SetZero(); - } - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. - b2Vec3 Solve33(const b2Vec3& b) const; - - /// Solve A * x = b, where b is a column vector. This is more efficient - /// than computing the inverse in one-shot cases. Solve only the upper - /// 2-by-2 matrix equation. - b2Vec2 Solve22(const b2Vec2& b) const; - - /// Get the inverse of this matrix as a 2-by-2. - /// Returns the zero matrix if singular. - void GetInverse22(b2Mat33* M) const; - - /// Get the symmetric inverse of this matrix as a 3-by-3. - /// Returns the zero matrix if singular. - void GetSymInverse33(b2Mat33* M) const; - - b2Vec3 ex, ey, ez; + /// The default constructor does nothing (for performance). + b2Mat33() {} + + /// Construct this matrix using columns. + b2Mat33(const b2Vec3 & c1, const b2Vec3 & c2, const b2Vec3 & c3) + { + ex = c1; + ey = c2; + ez = c3; + } + + /// Set this matrix to all zeros. + void SetZero() + { + ex.SetZero(); + ey.SetZero(); + ez.SetZero(); + } + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. + b2Vec3 Solve33(const b2Vec3 & b) const; + + /// Solve A * x = b, where b is a column vector. This is more efficient + /// than computing the inverse in one-shot cases. Solve only the upper + /// 2-by-2 matrix equation. + b2Vec2 Solve22(const b2Vec2 & b) const; + + /// Get the inverse of this matrix as a 2-by-2. + /// Returns the zero matrix if singular. + void GetInverse22(b2Mat33 * M) const; + + /// Get the symmetric inverse of this matrix as a 3-by-3. + /// Returns the zero matrix if singular. + void GetSymInverse33(b2Mat33 * M) const; + + b2Vec3 ex, ey, ez; }; /// Rotation struct b2Rot { - b2Rot() {} - - /// Initialize from an angle in radians - explicit b2Rot(float32 angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set using an angle in radians. - void Set(float32 angle) - { - /// TODO_ERIN optimize - s = sinf(angle); - c = cosf(angle); - } - - /// Set to the identity rotation - void SetIdentity() - { - s = 0.0f; - c = 1.0f; - } - - /// Get the angle in radians - float32 GetAngle() const - { - return b2Atan2(s, c); - } - - /// Get the x-axis - b2Vec2 GetXAxis() const - { - return b2Vec2(c, s); - } - - /// Get the u-axis - b2Vec2 GetYAxis() const - { - return b2Vec2(-s, c); - } - - /// Sine and cosine - float32 s, c; + b2Rot() {} + + /// Initialize from an angle in radians + explicit b2Rot(float32 angle) + { + /// TODO_ERIN optimize + s = sinf(angle); + c = cosf(angle); + } + + /// Set using an angle in radians. + void Set(float32 angle) + { + /// TODO_ERIN optimize + s = sinf(angle); + c = cosf(angle); + } + + /// Set to the identity rotation + void SetIdentity() + { + s = 0.0f; + c = 1.0f; + } + + /// Get the angle in radians + float32 GetAngle() const { return b2Atan2(s, c); } + + /// Get the x-axis + b2Vec2 GetXAxis() const { return b2Vec2(c, s); } + + /// Get the u-axis + b2Vec2 GetYAxis() const { return b2Vec2(-s, c); } + + /// Sine and cosine + float32 s, c; }; /// A transform contains translation and rotation. It is used to represent /// the position and orientation of rigid frames. struct b2Transform { - /// The default constructor does nothing. - b2Transform() {} + /// The default constructor does nothing. + b2Transform() {} - /// Initialize using a position vector and a rotation. - b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {} + /// Initialize using a position vector and a rotation. + b2Transform(const b2Vec2 & position, const b2Rot & rotation) : p(position), q(rotation) {} - /// Set this to the identity transform. - void SetIdentity() - { - p.SetZero(); - q.SetIdentity(); - } + /// Set this to the identity transform. + void SetIdentity() + { + p.SetZero(); + q.SetIdentity(); + } - /// Set this based on the position and angle. - void Set(const b2Vec2& position, float32 angle) - { - p = position; - q.Set(angle); - } + /// Set this based on the position and angle. + void Set(const b2Vec2 & position, float32 angle) + { + p = position; + q.Set(angle); + } - b2Vec2 p; - b2Rot q; + b2Vec2 p; + b2Rot q; }; /// This describes the motion of a body/shape for TOI computation. @@ -379,347 +395,314 @@ struct b2Transform /// we must interpolate the center of mass position. struct b2Sweep { - /// Get the interpolated transform at a specific time. - /// @param beta is a factor in [0,1], where 0 indicates alpha0. - void GetTransform(b2Transform* xfb, float32 beta) const; + /// Get the interpolated transform at a specific time. + /// @param beta is a factor in [0,1], where 0 indicates alpha0. + void GetTransform(b2Transform * xfb, float32 beta) const; - /// Advance the sweep forward, yielding a new initial state. - /// @param alpha the new initial time. - void Advance(float32 alpha); + /// Advance the sweep forward, yielding a new initial state. + /// @param alpha the new initial time. + void Advance(float32 alpha); - /// Normalize the angles. - void Normalize(); + /// Normalize the angles. + void Normalize(); - b2Vec2 localCenter; ///< local center of mass position - b2Vec2 c0, c; ///< center world positions - float32 a0, a; ///< world angles + b2Vec2 localCenter; ///< local center of mass position + b2Vec2 c0, c; ///< center world positions + float32 a0, a; ///< world angles - /// Fraction of the current time step in the range [0,1] - /// c0 and a0 are the positions at alpha0. - float32 alpha0; + /// Fraction of the current time step in the range [0,1] + /// c0 and a0 are the positions at alpha0. + float32 alpha0; }; /// Useful constant extern const b2Vec2 b2Vec2_zero; /// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.x + a.y * b.y; -} +inline float32 b2Dot(const b2Vec2 & a, const b2Vec2 & b) { return a.x * b.x + a.y * b.y; } /// Perform the cross product on two vectors. In 2D this produces a scalar. -inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b) -{ - return a.x * b.y - a.y * b.x; -} +inline float32 b2Cross(const b2Vec2 & a, const b2Vec2 & b) { return a.x * b.y - a.y * b.x; } /// Perform the cross product on a vector and a scalar. In 2D this produces /// a vector. -inline b2Vec2 b2Cross(const b2Vec2& a, float32 s) -{ - return b2Vec2(s * a.y, -s * a.x); -} +inline b2Vec2 b2Cross(const b2Vec2 & a, float32 s) { return b2Vec2(s * a.y, -s * a.x); } /// Perform the cross product on a scalar and a vector. In 2D this produces /// a vector. -inline b2Vec2 b2Cross(float32 s, const b2Vec2& a) -{ - return b2Vec2(-s * a.y, s * a.x); -} +inline b2Vec2 b2Cross(float32 s, const b2Vec2 & a) { return b2Vec2(-s * a.y, s * a.x); } /// Multiply a matrix times a vector. If a rotation matrix is provided, /// then this transforms the vector from one frame to another. -inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v) +inline b2Vec2 b2Mul(const b2Mat22 & A, const b2Vec2 & v) { - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); + return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); } -/// Multiply a matrix transpose times a vector. If a rotation matrix is provided, -/// then this transforms the vector from one frame to another (inverse transform). -inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v) +/// Multiply a matrix transpose times a vector. If a rotation matrix is +/// provided, then this transforms the vector from one frame to another (inverse +/// transform). +inline b2Vec2 b2MulT(const b2Mat22 & A, const b2Vec2 & v) { - return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); + return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey)); } /// Add two vectors component-wise. -inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x + b.x, a.y + b.y); -} +inline b2Vec2 operator+(const b2Vec2 & a, const b2Vec2 & b) { return b2Vec2(a.x + b.x, a.y + b.y); } /// Subtract two vectors component-wise. -inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b) -{ - return b2Vec2(a.x - b.x, a.y - b.y); -} +inline b2Vec2 operator-(const b2Vec2 & a, const b2Vec2 & b) { return b2Vec2(a.x - b.x, a.y - b.y); } -inline b2Vec2 operator * (float32 s, const b2Vec2& a) -{ - return b2Vec2(s * a.x, s * a.y); -} +inline b2Vec2 operator*(float32 s, const b2Vec2 & a) { return b2Vec2(s * a.x, s * a.y); } -inline bool operator == (const b2Vec2& a, const b2Vec2& b) -{ - return a.x == b.x && a.y == b.y; -} +inline bool operator==(const b2Vec2 & a, const b2Vec2 & b) { return a.x == b.x && a.y == b.y; } -inline bool operator != (const b2Vec2& a, const b2Vec2& b) -{ - return a.x != b.x || a.y != b.y; -} +inline bool operator!=(const b2Vec2 & a, const b2Vec2 & b) { return a.x != b.x || a.y != b.y; } -inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b) +inline float32 b2Distance(const b2Vec2 & a, const b2Vec2 & b) { - b2Vec2 c = a - b; - return c.Length(); + b2Vec2 c = a - b; + return c.Length(); } -inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b) +inline float32 b2DistanceSquared(const b2Vec2 & a, const b2Vec2 & b) { - b2Vec2 c = a - b; - return b2Dot(c, c); + b2Vec2 c = a - b; + return b2Dot(c, c); } -inline b2Vec3 operator * (float32 s, const b2Vec3& a) -{ - return b2Vec3(s * a.x, s * a.y, s * a.z); -} +inline b2Vec3 operator*(float32 s, const b2Vec3 & a) { return b2Vec3(s * a.x, s * a.y, s * a.z); } /// Add two vectors component-wise. -inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b) +inline b2Vec3 operator+(const b2Vec3 & a, const b2Vec3 & b) { - return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z); + return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z); } /// Subtract two vectors component-wise. -inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b) +inline b2Vec3 operator-(const b2Vec3 & a, const b2Vec3 & b) { - return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z); + return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z); } /// Perform the dot product on two vectors. -inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b) +inline float32 b2Dot(const b2Vec3 & a, const b2Vec3 & b) { - return a.x * b.x + a.y * b.y + a.z * b.z; + return a.x * b.x + a.y * b.y + a.z * b.z; } /// Perform the cross product on two vectors. -inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b) +inline b2Vec3 b2Cross(const b2Vec3 & a, const b2Vec3 & b) { - return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); + return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); } -inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B) +inline b2Mat22 operator+(const b2Mat22 & A, const b2Mat22 & B) { - return b2Mat22(A.ex + B.ex, A.ey + B.ey); + return b2Mat22(A.ex + B.ex, A.ey + B.ey); } // A * B -inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B) +inline b2Mat22 b2Mul(const b2Mat22 & A, const b2Mat22 & B) { - return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); + return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey)); } // A^T * B -inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B) +inline b2Mat22 b2MulT(const b2Mat22 & A, const b2Mat22 & B) { - b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); - b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); - return b2Mat22(c1, c2); + b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex)); + b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey)); + return b2Mat22(c1, c2); } /// Multiply a matrix times a vector. -inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v) +inline b2Vec3 b2Mul(const b2Mat33 & A, const b2Vec3 & v) { - return v.x * A.ex + v.y * A.ey + v.z * A.ez; + return v.x * A.ex + v.y * A.ey + v.z * A.ez; } /// Multiply a matrix times a vector. -inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v) +inline b2Vec2 b2Mul22(const b2Mat33 & A, const b2Vec2 & v) { - return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); + return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y); } /// Multiply two rotations: q * r -inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r) +inline b2Rot b2Mul(const b2Rot & q, const b2Rot & r) { - // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] - // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] - // s = qs * rc + qc * rs - // c = qc * rc - qs * rs - b2Rot qr; - qr.s = q.s * r.c + q.c * r.s; - qr.c = q.c * r.c - q.s * r.s; - return qr; + // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] + // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] + // s = qs * rc + qc * rs + // c = qc * rc - qs * rs + b2Rot qr; + qr.s = q.s * r.c + q.c * r.s; + qr.c = q.c * r.c - q.s * r.s; + return qr; } /// Transpose multiply two rotations: qT * r -inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r) +inline b2Rot b2MulT(const b2Rot & q, const b2Rot & r) { - // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] - // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] - // s = qc * rs - qs * rc - // c = qc * rc + qs * rs - b2Rot qr; - qr.s = q.c * r.s - q.s * r.c; - qr.c = q.c * r.c + q.s * r.s; - return qr; + // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] + // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] + // s = qc * rs - qs * rc + // c = qc * rc + qs * rs + b2Rot qr; + qr.s = q.c * r.s - q.s * r.c; + qr.c = q.c * r.c + q.s * r.s; + return qr; } /// Rotate a vector -inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v) +inline b2Vec2 b2Mul(const b2Rot & q, const b2Vec2 & v) { - return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); + return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y); } /// Inverse rotate a vector -inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v) +inline b2Vec2 b2MulT(const b2Rot & q, const b2Vec2 & v) { - return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); + return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y); } -inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v) +inline b2Vec2 b2Mul(const b2Transform & T, const b2Vec2 & v) { - float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; - float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; + float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; + float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; - return b2Vec2(x, y); + return b2Vec2(x, y); } -inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v) +inline b2Vec2 b2MulT(const b2Transform & T, const b2Vec2 & v) { - float32 px = v.x - T.p.x; - float32 py = v.y - T.p.y; - float32 x = (T.q.c * px + T.q.s * py); - float32 y = (-T.q.s * px + T.q.c * py); + float32 px = v.x - T.p.x; + float32 py = v.y - T.p.y; + float32 x = (T.q.c * px + T.q.s * py); + float32 y = (-T.q.s * px + T.q.c * py); - return b2Vec2(x, y); + return b2Vec2(x, y); } // v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p -inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B) +inline b2Transform b2Mul(const b2Transform & A, const b2Transform & B) { - b2Transform C; - C.q = b2Mul(A.q, B.q); - C.p = b2Mul(A.q, B.p) + A.p; - return C; + b2Transform C; + C.q = b2Mul(A.q, B.q); + C.p = b2Mul(A.q, B.p) + A.p; + return C; } // v2 = A.q' * (B.q * v1 + B.p - A.p) // = A.q' * B.q * v1 + A.q' * (B.p - A.p) -inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B) +inline b2Transform b2MulT(const b2Transform & A, const b2Transform & B) { - b2Transform C; - C.q = b2MulT(A.q, B.q); - C.p = b2MulT(A.q, B.p - A.p); - return C; + b2Transform C; + C.q = b2MulT(A.q, B.q); + C.p = b2MulT(A.q, B.p - A.p); + return C; } template inline T b2Abs(T a) { - return a > T(0) ? a : -a; + return a > T(0) ? a : -a; } -inline b2Vec2 b2Abs(const b2Vec2& a) -{ - return b2Vec2(b2Abs(a.x), b2Abs(a.y)); -} +inline b2Vec2 b2Abs(const b2Vec2 & a) { return b2Vec2(b2Abs(a.x), b2Abs(a.y)); } -inline b2Mat22 b2Abs(const b2Mat22& A) -{ - return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); -} +inline b2Mat22 b2Abs(const b2Mat22 & A) { return b2Mat22(b2Abs(A.ex), b2Abs(A.ey)); } template inline T b2Min(T a, T b) { - return a < b ? a : b; + return a < b ? a : b; } -inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b) +inline b2Vec2 b2Min(const b2Vec2 & a, const b2Vec2 & b) { - return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y)); + return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y)); } template inline T b2Max(T a, T b) { - return a > b ? a : b; + return a > b ? a : b; } -inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b) +inline b2Vec2 b2Max(const b2Vec2 & a, const b2Vec2 & b) { - return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y)); + return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y)); } template inline T b2Clamp(T a, T low, T high) { - return b2Max(low, b2Min(a, high)); + return b2Max(low, b2Min(a, high)); } -inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high) +inline b2Vec2 b2Clamp(const b2Vec2 & a, const b2Vec2 & low, const b2Vec2 & high) { - return b2Max(low, b2Min(a, high)); + return b2Max(low, b2Min(a, high)); } -template inline void b2Swap(T& a, T& b) +template +inline void b2Swap(T & a, T & b) { - T tmp = a; - a = b; - b = tmp; + T tmp = a; + a = b; + b = tmp; } /// "Next Largest Power of 2 -/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm -/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with -/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next -/// largest power of 2. For a 32-bit value:" +/// Given a binary integer value x, the next largest power of 2 can be computed +/// by a SWAR algorithm that recursively "folds" the upper bits into the lower +/// bits. This process yields a bit vector with the same most significant 1 as +/// x, but all 1's below it. Adding 1 to that value yields the next largest +/// power of 2. For a 32-bit value:" inline uint32 b2NextPowerOfTwo(uint32 x) { - x |= (x >> 1); - x |= (x >> 2); - x |= (x >> 4); - x |= (x >> 8); - x |= (x >> 16); - return x + 1; + x |= (x >> 1); + x |= (x >> 2); + x |= (x >> 4); + x |= (x >> 8); + x |= (x >> 16); + return x + 1; } inline bool b2IsPowerOfTwo(uint32 x) { - bool result = x > 0 && (x & (x - 1)) == 0; - return result; + bool result = x > 0 && (x & (x - 1)) == 0; + return result; } -inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const +inline void b2Sweep::GetTransform(b2Transform * xf, float32 beta) const { - xf->p = (1.0f - beta) * c0 + beta * c; - float32 angle = (1.0f - beta) * a0 + beta * a; - xf->q.Set(angle); + xf->p = (1.0f - beta) * c0 + beta * c; + float32 angle = (1.0f - beta) * a0 + beta * a; + xf->q.Set(angle); - // Shift to origin - xf->p -= b2Mul(xf->q, localCenter); + // Shift to origin + xf->p -= b2Mul(xf->q, localCenter); } inline void b2Sweep::Advance(float32 alpha) { - b2Assert(alpha0 < 1.0f); - float32 beta = (alpha - alpha0) / (1.0f - alpha0); - c0 += beta * (c - c0); - a0 += beta * (a - a0); - alpha0 = alpha; + b2Assert(alpha0 < 1.0f); + float32 beta = (alpha - alpha0) / (1.0f - alpha0); + c0 += beta * (c - c0); + a0 += beta * (a - a0); + alpha0 = alpha; } /// Normalize an angle in radians to be between -pi and pi inline void b2Sweep::Normalize() { - float32 twoPi = 2.0f * b2_pi; - float32 d = twoPi * floorf(a0 / twoPi); - a0 -= d; - a -= d; + float32 twoPi = 2.0f * b2_pi; + float32 d = twoPi * floorf(a0 / twoPi); + a0 -= d; + a -= d; } #endif diff --git a/flatland_box2d/include/Box2D/Common/b2Settings.h b/flatland_box2d/include/Box2D/Common/b2Settings.h index 1fab8149..2a3ae3c2 100644 --- a/flatland_box2d/include/Box2D/Common/b2Settings.h +++ b/flatland_box2d/include/Box2D/Common/b2Settings.h @@ -1,36 +1,36 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_SETTINGS_H #define B2_SETTINGS_H -#include #include #include +#include #if !defined(NDEBUG) - #define b2DEBUG +#define b2DEBUG #endif #define B2_NOT_USED(x) ((void)(x)) #define b2Assert(A) assert(A) -typedef signed char int8; +typedef signed char int8; typedef signed short int16; typedef signed int int32; typedef unsigned char uint8; @@ -39,9 +39,9 @@ typedef unsigned int uint32; typedef float float32; typedef double float64; -#define b2_maxFloat FLT_MAX -#define b2_epsilon FLT_EPSILON -#define b2_pi 3.14159265359f +#define b2_maxFloat FLT_MAX +#define b2_epsilon FLT_EPSILON +#define b2_pi 3.14159265359f /// @file /// Global tuning constants based on meters-kilograms-seconds (MKS) units. @@ -51,102 +51,101 @@ typedef double float64; /// The maximum number of contact points between two convex shapes. Do /// not change this value. -#define b2_maxManifoldPoints 2 +#define b2_maxManifoldPoints 2 /// The maximum number of vertices on a convex polygon. You cannot increase /// this too much because b2BlockAllocator has a maximum object size. -#define b2_maxPolygonVertices 8 +#define b2_maxPolygonVertices 8 /// This is used to fatten AABBs in the dynamic tree. This allows proxies /// to move by a small amount without triggering a tree adjustment. /// This is in meters. -#define b2_aabbExtension 0.1f +#define b2_aabbExtension 0.1f /// This is used to fatten AABBs in the dynamic tree. This is used to predict /// the future position based on the current displacement. /// This is a dimensionless multiplier. -#define b2_aabbMultiplier 2.0f +#define b2_aabbMultiplier 2.0f /// A small length used as a collision and constraint tolerance. Usually it is /// chosen to be numerically significant, but visually insignificant. -#define b2_linearSlop 0.005f +#define b2_linearSlop 0.005f /// A small angle used as a collision and constraint tolerance. Usually it is /// chosen to be numerically significant, but visually insignificant. -#define b2_angularSlop (2.0f / 180.0f * b2_pi) +#define b2_angularSlop (2.0f / 180.0f * b2_pi) -/// The radius of the polygon/edge shape skin. This should not be modified. Making -/// this smaller means polygons will have an insufficient buffer for continuous collision. -/// Making it larger may create artifacts for vertex collision. -#define b2_polygonRadius (2.0f * b2_linearSlop) +/// The radius of the polygon/edge shape skin. This should not be modified. +/// Making this smaller means polygons will have an insufficient buffer for +/// continuous collision. Making it larger may create artifacts for vertex +/// collision. +#define b2_polygonRadius (2.0f * b2_linearSlop) /// Maximum number of sub-steps per contact in continuous physics simulation. -#define b2_maxSubSteps 8 - +#define b2_maxSubSteps 8 // Dynamics /// Maximum number of contacts to be handled to solve a TOI impact. -#define b2_maxTOIContacts 32 +#define b2_maxTOIContacts 32 -/// A velocity threshold for elastic collisions. Any collision with a relative linear -/// velocity below this threshold will be treated as inelastic. -#define b2_velocityThreshold 1.0f +/// A velocity threshold for elastic collisions. Any collision with a relative +/// linear velocity below this threshold will be treated as inelastic. +#define b2_velocityThreshold 1.0f -/// The maximum linear position correction used when solving constraints. This helps to -/// prevent overshoot. -#define b2_maxLinearCorrection 0.2f +/// The maximum linear position correction used when solving constraints. This +/// helps to prevent overshoot. +#define b2_maxLinearCorrection 0.2f -/// The maximum angular position correction used when solving constraints. This helps to -/// prevent overshoot. -#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi) +/// The maximum angular position correction used when solving constraints. This +/// helps to prevent overshoot. +#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi) /// The maximum linear velocity of a body. This limit is very large and is used /// to prevent numerical problems. You shouldn't need to adjust this. -#define b2_maxTranslation 2.0f -#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation) +#define b2_maxTranslation 2.0f +#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation) /// The maximum angular velocity of a body. This limit is very large and is used /// to prevent numerical problems. You shouldn't need to adjust this. -#define b2_maxRotation (0.5f * b2_pi) -#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation) - -/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so -/// that overlap is removed in one time step. However using values close to 1 often lead -/// to overshoot. -#define b2_baumgarte 0.2f -#define b2_toiBaugarte 0.75f +#define b2_maxRotation (0.5f * b2_pi) +#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation) +/// This scale factor controls how fast overlap is resolved. Ideally this would +/// be 1 so that overlap is removed in one time step. However using values close +/// to 1 often lead to overshoot. +#define b2_baumgarte 0.2f +#define b2_toiBaugarte 0.75f // Sleep /// The time that a body must be still before it will go to sleep. -#define b2_timeToSleep 0.5f +#define b2_timeToSleep 0.5f /// A body cannot sleep if its linear velocity is above this tolerance. -#define b2_linearSleepTolerance 0.01f +#define b2_linearSleepTolerance 0.01f /// A body cannot sleep if its angular velocity is above this tolerance. -#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi) +#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi) // Memory Allocation /// Implement this function to use your own memory allocator. -void* b2Alloc(int32 size); +void * b2Alloc(int32 size); /// If you implement b2Alloc, you should also implement this function. -void b2Free(void* mem); +void b2Free(void * mem); /// Logging function. -void b2Log(const char* string, ...); +void b2Log(const char * string, ...); /// Version numbering scheme. /// See http://en.wikipedia.org/wiki/Software_versioning struct b2Version { - int32 major; ///< significant changes - int32 minor; ///< incremental changes - int32 revision; ///< bug fixes + int32 major; ///< significant changes + int32 minor; ///< incremental changes + int32 revision; ///< bug fixes }; /// Current version. diff --git a/flatland_box2d/include/Box2D/Common/b2StackAllocator.h b/flatland_box2d/include/Box2D/Common/b2StackAllocator.h index 1b1cc76c..e2f76b1a 100644 --- a/flatland_box2d/include/Box2D/Common/b2StackAllocator.h +++ b/flatland_box2d/include/Box2D/Common/b2StackAllocator.h @@ -1,34 +1,34 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_STACK_ALLOCATOR_H #define B2_STACK_ALLOCATOR_H #include "Box2D/Common/b2Settings.h" -const int32 b2_stackSize = 100 * 1024; // 100k +const int32 b2_stackSize = 100 * 1024; // 100k const int32 b2_maxStackEntries = 32; struct b2StackEntry { - char* data; - int32 size; - bool usedMalloc; + char * data; + int32 size; + bool usedMalloc; }; // This is a stack allocator used for fast per step allocations. @@ -37,24 +37,23 @@ struct b2StackEntry class b2StackAllocator { public: - b2StackAllocator(); - ~b2StackAllocator(); + b2StackAllocator(); + ~b2StackAllocator(); - void* Allocate(int32 size); - void Free(void* p); + void * Allocate(int32 size); + void Free(void * p); - int32 GetMaxAllocation() const; + int32 GetMaxAllocation() const; private: + char m_data[b2_stackSize]; + int32 m_index; - char m_data[b2_stackSize]; - int32 m_index; + int32 m_allocation; + int32 m_maxAllocation; - int32 m_allocation; - int32 m_maxAllocation; - - b2StackEntry m_entries[b2_maxStackEntries]; - int32 m_entryCount; + b2StackEntry m_entries[b2_maxStackEntries]; + int32 m_entryCount; }; #endif diff --git a/flatland_box2d/include/Box2D/Common/b2Timer.h b/flatland_box2d/include/Box2D/Common/b2Timer.h index d46ff430..cc9d6cae 100644 --- a/flatland_box2d/include/Box2D/Common/b2Timer.h +++ b/flatland_box2d/include/Box2D/Common/b2Timer.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2011 Erin Catto http://box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_TIMER_H #define B2_TIMER_H @@ -26,24 +26,22 @@ class b2Timer { public: + /// Constructor + b2Timer(); - /// Constructor - b2Timer(); + /// Reset the timer. + void Reset(); - /// Reset the timer. - void Reset(); - - /// Get the time since construction or the last reset. - float32 GetMilliseconds() const; + /// Get the time since construction or the last reset. + float32 GetMilliseconds() const; private: - #if defined(_WIN32) - float64 m_start; - static float64 s_invFrequency; -#elif defined(__linux__) || defined (__APPLE__) - unsigned long m_start_sec; - unsigned long m_start_usec; + float64 m_start; + static float64 s_invFrequency; +#elif defined(__linux__) || defined(__APPLE__) + unsigned long m_start_sec; + unsigned long m_start_usec; #endif }; diff --git a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h index fa484ec9..da36de65 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h +++ b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_CHAIN_AND_CIRCLE_CONTACT_H #define B2_CHAIN_AND_CIRCLE_CONTACT_H @@ -26,14 +26,15 @@ class b2BlockAllocator; class b2ChainAndCircleContact : public b2Contact { public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + static b2Contact * Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator); + static void Destroy(b2Contact * contact, b2BlockAllocator * allocator); - b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - ~b2ChainAndCircleContact() {} + b2ChainAndCircleContact(b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB); + ~b2ChainAndCircleContact() {} - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override; + void Evaluate(b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) override; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h index c69409af..b653c758 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h +++ b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_CHAIN_AND_POLYGON_CONTACT_H #define B2_CHAIN_AND_POLYGON_CONTACT_H @@ -26,14 +26,15 @@ class b2BlockAllocator; class b2ChainAndPolygonContact : public b2Contact { public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + static b2Contact * Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator); + static void Destroy(b2Contact * contact, b2BlockAllocator * allocator); - b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - ~b2ChainAndPolygonContact() {} + b2ChainAndPolygonContact(b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB); + ~b2ChainAndPolygonContact() {} - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override; + void Evaluate(b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) override; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2CircleContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2CircleContact.h index 5d5261d1..9b2e8635 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2CircleContact.h +++ b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2CircleContact.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_CIRCLE_CONTACT_H #define B2_CIRCLE_CONTACT_H @@ -26,14 +26,15 @@ class b2BlockAllocator; class b2CircleContact : public b2Contact { public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + static b2Contact * Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator); + static void Destroy(b2Contact * contact, b2BlockAllocator * allocator); - b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2CircleContact() {} + b2CircleContact(b2Fixture * fixtureA, b2Fixture * fixtureB); + ~b2CircleContact() {} - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override; + void Evaluate(b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) override; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2Contact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2Contact.h index 579049c4..3f235ba4 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2Contact.h +++ b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2Contact.h @@ -1,27 +1,27 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_CONTACT_H #define B2_CONTACT_H -#include "Box2D/Common/b2Math.h" -#include "Box2D/Collision/b2Collision.h" #include "Box2D/Collision/Shapes/b2Shape.h" +#include "Box2D/Collision/b2Collision.h" +#include "Box2D/Common/b2Math.h" #include "Box2D/Dynamics/b2Fixture.h" class b2Body; @@ -32,30 +32,30 @@ class b2BlockAllocator; class b2StackAllocator; class b2ContactListener; -/// Friction mixing law. The idea is to allow either fixture to drive the friction to zero. -/// For example, anything slides on ice. +/// Friction mixing law. The idea is to allow either fixture to drive the +/// friction to zero. For example, anything slides on ice. inline float32 b2MixFriction(float32 friction1, float32 friction2) { - return b2Sqrt(friction1 * friction2); + return b2Sqrt(friction1 * friction2); } -/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface. -/// For example, a superball bounces on anything. +/// Restitution mixing law. The idea is allow for anything to bounce off an +/// inelastic surface. For example, a superball bounces on anything. inline float32 b2MixRestitution(float32 restitution1, float32 restitution2) { - return restitution1 > restitution2 ? restitution1 : restitution2; + return restitution1 > restitution2 ? restitution1 : restitution2; } -typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, - b2BlockAllocator* allocator); -typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator); +typedef b2Contact * b2ContactCreateFcn( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator); +typedef void b2ContactDestroyFcn(b2Contact * contact, b2BlockAllocator * allocator); struct b2ContactRegister { - b2ContactCreateFcn* createFcn; - b2ContactDestroyFcn* destroyFcn; - bool primary; + b2ContactCreateFcn * createFcn; + b2ContactDestroyFcn * destroyFcn; + bool primary; }; /// A contact edge is used to connect bodies and contacts together @@ -65,285 +65,230 @@ struct b2ContactRegister /// nodes, one for each attached body. struct b2ContactEdge { - b2Body* other; ///< provides quick access to the other body attached. - b2Contact* contact; ///< the contact - b2ContactEdge* prev; ///< the previous contact edge in the body's contact list - b2ContactEdge* next; ///< the next contact edge in the body's contact list + b2Body * other; ///< provides quick access to the other body attached. + b2Contact * contact; ///< the contact + b2ContactEdge * prev; ///< the previous contact edge in the body's contact list + b2ContactEdge * next; ///< the next contact edge in the body's contact list }; -/// The class manages contact between two shapes. A contact exists for each overlapping -/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist -/// that has no contact points. +/// The class manages contact between two shapes. A contact exists for each +/// overlapping AABB in the broad-phase (except if filtered). Therefore a +/// contact object may exist that has no contact points. class b2Contact { public: + /// Get the contact manifold. Do not modify the manifold unless you understand + /// the internals of Box2D. + b2Manifold * GetManifold(); + const b2Manifold * GetManifold() const; - /// Get the contact manifold. Do not modify the manifold unless you understand the - /// internals of Box2D. - b2Manifold* GetManifold(); - const b2Manifold* GetManifold() const; - - /// Get the world manifold. - void GetWorldManifold(b2WorldManifold* worldManifold) const; + /// Get the world manifold. + void GetWorldManifold(b2WorldManifold * worldManifold) const; - /// Is this contact touching? - bool IsTouching() const; + /// Is this contact touching? + bool IsTouching() const; - /// Enable/disable this contact. This can be used inside the pre-solve - /// contact listener. The contact is only disabled for the current - /// time step (or sub-step in continuous collisions). - void SetEnabled(bool flag); + /// Enable/disable this contact. This can be used inside the pre-solve + /// contact listener. The contact is only disabled for the current + /// time step (or sub-step in continuous collisions). + void SetEnabled(bool flag); - /// Has this contact been disabled? - bool IsEnabled() const; + /// Has this contact been disabled? + bool IsEnabled() const; - /// Get the next contact in the world's contact list. - b2Contact* GetNext(); - const b2Contact* GetNext() const; + /// Get the next contact in the world's contact list. + b2Contact * GetNext(); + const b2Contact * GetNext() const; - /// Get fixture A in this contact. - b2Fixture* GetFixtureA(); - const b2Fixture* GetFixtureA() const; + /// Get fixture A in this contact. + b2Fixture * GetFixtureA(); + const b2Fixture * GetFixtureA() const; - /// Get the child primitive index for fixture A. - int32 GetChildIndexA() const; + /// Get the child primitive index for fixture A. + int32 GetChildIndexA() const; - /// Get fixture B in this contact. - b2Fixture* GetFixtureB(); - const b2Fixture* GetFixtureB() const; + /// Get fixture B in this contact. + b2Fixture * GetFixtureB(); + const b2Fixture * GetFixtureB() const; - /// Get the child primitive index for fixture B. - int32 GetChildIndexB() const; + /// Get the child primitive index for fixture B. + int32 GetChildIndexB() const; - /// Override the default friction mixture. You can call this in b2ContactListener::PreSolve. - /// This value persists until set or reset. - void SetFriction(float32 friction); + /// Override the default friction mixture. You can call this in + /// b2ContactListener::PreSolve. This value persists until set or reset. + void SetFriction(float32 friction); - /// Get the friction. - float32 GetFriction() const; + /// Get the friction. + float32 GetFriction() const; - /// Reset the friction mixture to the default value. - void ResetFriction(); + /// Reset the friction mixture to the default value. + void ResetFriction(); - /// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve. - /// The value persists until you set or reset. - void SetRestitution(float32 restitution); + /// Override the default restitution mixture. You can call this in + /// b2ContactListener::PreSolve. The value persists until you set or reset. + void SetRestitution(float32 restitution); - /// Get the restitution. - float32 GetRestitution() const; + /// Get the restitution. + float32 GetRestitution() const; - /// Reset the restitution to the default value. - void ResetRestitution(); + /// Reset the restitution to the default value. + void ResetRestitution(); - /// Set the desired tangent speed for a conveyor belt behavior. In meters per second. - void SetTangentSpeed(float32 speed); + /// Set the desired tangent speed for a conveyor belt behavior. In meters per + /// second. + void SetTangentSpeed(float32 speed); - /// Get the desired tangent speed. In meters per second. - float32 GetTangentSpeed() const; + /// Get the desired tangent speed. In meters per second. + float32 GetTangentSpeed() const; - /// Evaluate this contact with your own manifold and transforms. - virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0; + /// Evaluate this contact with your own manifold and transforms. + virtual void Evaluate( + b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) = 0; protected: - friend class b2ContactManager; - friend class b2World; - friend class b2ContactSolver; - friend class b2Body; - friend class b2Fixture; + friend class b2ContactManager; + friend class b2World; + friend class b2ContactSolver; + friend class b2Body; + friend class b2Fixture; - // Flags stored in m_flags - enum - { - // Used when crawling contact graph when forming islands. - e_islandFlag = 0x0001, + // Flags stored in m_flags + enum { + // Used when crawling contact graph when forming islands. + e_islandFlag = 0x0001, - // Set when the shapes are touching. - e_touchingFlag = 0x0002, + // Set when the shapes are touching. + e_touchingFlag = 0x0002, - // This contact can be disabled (by user) - e_enabledFlag = 0x0004, + // This contact can be disabled (by user) + e_enabledFlag = 0x0004, - // This contact needs filtering because a fixture filter was changed. - e_filterFlag = 0x0008, + // This contact needs filtering because a fixture filter was changed. + e_filterFlag = 0x0008, - // This bullet contact had a TOI event - e_bulletHitFlag = 0x0010, + // This bullet contact had a TOI event + e_bulletHitFlag = 0x0010, - // This contact has a valid TOI in m_toi - e_toiFlag = 0x0020 - }; + // This contact has a valid TOI in m_toi + e_toiFlag = 0x0020 + }; - /// Flag this contact for filtering. Filtering will occur the next time step. - void FlagForFiltering(); + /// Flag this contact for filtering. Filtering will occur the next time step. + void FlagForFiltering(); - static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn, - b2Shape::Type typeA, b2Shape::Type typeB); - static void InitializeRegisters(); - static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + static void AddType( + b2ContactCreateFcn * createFcn, b2ContactDestroyFcn * destroyFcn, b2Shape::Type typeA, + b2Shape::Type typeB); + static void InitializeRegisters(); + static b2Contact * Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator); + static void Destroy( + b2Contact * contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator * allocator); + static void Destroy(b2Contact * contact, b2BlockAllocator * allocator); - b2Contact() : m_fixtureA(nullptr), m_fixtureB(nullptr) {} - b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB); - virtual ~b2Contact() {} + b2Contact() : m_fixtureA(nullptr), m_fixtureB(nullptr) {} + b2Contact(b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB); + virtual ~b2Contact() {} - void Update(b2ContactListener* listener); + void Update(b2ContactListener * listener); - static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount]; - static bool s_initialized; + static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount]; + static bool s_initialized; - uint32 m_flags; + uint32 m_flags; - // World pool and list pointers. - b2Contact* m_prev; - b2Contact* m_next; + // World pool and list pointers. + b2Contact * m_prev; + b2Contact * m_next; - // Nodes for connecting bodies. - b2ContactEdge m_nodeA; - b2ContactEdge m_nodeB; + // Nodes for connecting bodies. + b2ContactEdge m_nodeA; + b2ContactEdge m_nodeB; - b2Fixture* m_fixtureA; - b2Fixture* m_fixtureB; + b2Fixture * m_fixtureA; + b2Fixture * m_fixtureB; - int32 m_indexA; - int32 m_indexB; + int32 m_indexA; + int32 m_indexB; - b2Manifold m_manifold; + b2Manifold m_manifold; - int32 m_toiCount; - float32 m_toi; + int32 m_toiCount; + float32 m_toi; - float32 m_friction; - float32 m_restitution; + float32 m_friction; + float32 m_restitution; - float32 m_tangentSpeed; + float32 m_tangentSpeed; }; -inline b2Manifold* b2Contact::GetManifold() -{ - return &m_manifold; -} +inline b2Manifold * b2Contact::GetManifold() { return &m_manifold; } -inline const b2Manifold* b2Contact::GetManifold() const -{ - return &m_manifold; -} +inline const b2Manifold * b2Contact::GetManifold() const { return &m_manifold; } -inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const +inline void b2Contact::GetWorldManifold(b2WorldManifold * worldManifold) const { - const b2Body* bodyA = m_fixtureA->GetBody(); - const b2Body* bodyB = m_fixtureB->GetBody(); - const b2Shape* shapeA = m_fixtureA->GetShape(); - const b2Shape* shapeB = m_fixtureB->GetShape(); + const b2Body * bodyA = m_fixtureA->GetBody(); + const b2Body * bodyB = m_fixtureB->GetBody(); + const b2Shape * shapeA = m_fixtureA->GetShape(); + const b2Shape * shapeB = m_fixtureB->GetShape(); - worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius); + worldManifold->Initialize( + &m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius); } inline void b2Contact::SetEnabled(bool flag) { - if (flag) - { - m_flags |= e_enabledFlag; - } - else - { - m_flags &= ~e_enabledFlag; - } + if (flag) { + m_flags |= e_enabledFlag; + } else { + m_flags &= ~e_enabledFlag; + } } -inline bool b2Contact::IsEnabled() const -{ - return (m_flags & e_enabledFlag) == e_enabledFlag; -} +inline bool b2Contact::IsEnabled() const { return (m_flags & e_enabledFlag) == e_enabledFlag; } -inline bool b2Contact::IsTouching() const -{ - return (m_flags & e_touchingFlag) == e_touchingFlag; -} +inline bool b2Contact::IsTouching() const { return (m_flags & e_touchingFlag) == e_touchingFlag; } -inline b2Contact* b2Contact::GetNext() -{ - return m_next; -} +inline b2Contact * b2Contact::GetNext() { return m_next; } -inline const b2Contact* b2Contact::GetNext() const -{ - return m_next; -} +inline const b2Contact * b2Contact::GetNext() const { return m_next; } -inline b2Fixture* b2Contact::GetFixtureA() -{ - return m_fixtureA; -} +inline b2Fixture * b2Contact::GetFixtureA() { return m_fixtureA; } -inline const b2Fixture* b2Contact::GetFixtureA() const -{ - return m_fixtureA; -} +inline const b2Fixture * b2Contact::GetFixtureA() const { return m_fixtureA; } -inline b2Fixture* b2Contact::GetFixtureB() -{ - return m_fixtureB; -} +inline b2Fixture * b2Contact::GetFixtureB() { return m_fixtureB; } -inline int32 b2Contact::GetChildIndexA() const -{ - return m_indexA; -} +inline int32 b2Contact::GetChildIndexA() const { return m_indexA; } -inline const b2Fixture* b2Contact::GetFixtureB() const -{ - return m_fixtureB; -} +inline const b2Fixture * b2Contact::GetFixtureB() const { return m_fixtureB; } -inline int32 b2Contact::GetChildIndexB() const -{ - return m_indexB; -} +inline int32 b2Contact::GetChildIndexB() const { return m_indexB; } -inline void b2Contact::FlagForFiltering() -{ - m_flags |= e_filterFlag; -} +inline void b2Contact::FlagForFiltering() { m_flags |= e_filterFlag; } -inline void b2Contact::SetFriction(float32 friction) -{ - m_friction = friction; -} +inline void b2Contact::SetFriction(float32 friction) { m_friction = friction; } -inline float32 b2Contact::GetFriction() const -{ - return m_friction; -} +inline float32 b2Contact::GetFriction() const { return m_friction; } inline void b2Contact::ResetFriction() { - m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); + m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); } -inline void b2Contact::SetRestitution(float32 restitution) -{ - m_restitution = restitution; -} +inline void b2Contact::SetRestitution(float32 restitution) { m_restitution = restitution; } -inline float32 b2Contact::GetRestitution() const -{ - return m_restitution; -} +inline float32 b2Contact::GetRestitution() const { return m_restitution; } inline void b2Contact::ResetRestitution() { - m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); + m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); } -inline void b2Contact::SetTangentSpeed(float32 speed) -{ - m_tangentSpeed = speed; -} +inline void b2Contact::SetTangentSpeed(float32 speed) { m_tangentSpeed = speed; } -inline float32 b2Contact::GetTangentSpeed() const -{ - return m_tangentSpeed; -} +inline float32 b2Contact::GetTangentSpeed() const { return m_tangentSpeed; } #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ContactSolver.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ContactSolver.h index 1bceb6d5..9c208345 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ContactSolver.h +++ b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2ContactSolver.h @@ -1,26 +1,26 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_CONTACT_SOLVER_H #define B2_CONTACT_SOLVER_H -#include "Box2D/Common/b2Math.h" #include "Box2D/Collision/b2Collision.h" +#include "Box2D/Common/b2Math.h" #include "Box2D/Dynamics/b2TimeStep.h" class b2Contact; @@ -30,66 +30,65 @@ struct b2ContactPositionConstraint; struct b2VelocityConstraintPoint { - b2Vec2 rA; - b2Vec2 rB; - float32 normalImpulse; - float32 tangentImpulse; - float32 normalMass; - float32 tangentMass; - float32 velocityBias; + b2Vec2 rA; + b2Vec2 rB; + float32 normalImpulse; + float32 tangentImpulse; + float32 normalMass; + float32 tangentMass; + float32 velocityBias; }; struct b2ContactVelocityConstraint { - b2VelocityConstraintPoint points[b2_maxManifoldPoints]; - b2Vec2 normal; - b2Mat22 normalMass; - b2Mat22 K; - int32 indexA; - int32 indexB; - float32 invMassA, invMassB; - float32 invIA, invIB; - float32 friction; - float32 restitution; - float32 tangentSpeed; - int32 pointCount; - int32 contactIndex; + b2VelocityConstraintPoint points[b2_maxManifoldPoints]; + b2Vec2 normal; + b2Mat22 normalMass; + b2Mat22 K; + int32 indexA; + int32 indexB; + float32 invMassA, invMassB; + float32 invIA, invIB; + float32 friction; + float32 restitution; + float32 tangentSpeed; + int32 pointCount; + int32 contactIndex; }; struct b2ContactSolverDef { - b2TimeStep step; - b2Contact** contacts; - int32 count; - b2Position* positions; - b2Velocity* velocities; - b2StackAllocator* allocator; + b2TimeStep step; + b2Contact ** contacts; + int32 count; + b2Position * positions; + b2Velocity * velocities; + b2StackAllocator * allocator; }; class b2ContactSolver { public: - b2ContactSolver(b2ContactSolverDef* def); - ~b2ContactSolver(); + b2ContactSolver(b2ContactSolverDef * def); + ~b2ContactSolver(); - void InitializeVelocityConstraints(); + void InitializeVelocityConstraints(); - void WarmStart(); - void SolveVelocityConstraints(); - void StoreImpulses(); + void WarmStart(); + void SolveVelocityConstraints(); + void StoreImpulses(); - bool SolvePositionConstraints(); - bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB); + bool SolvePositionConstraints(); + bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB); - b2TimeStep m_step; - b2Position* m_positions; - b2Velocity* m_velocities; - b2StackAllocator* m_allocator; - b2ContactPositionConstraint* m_positionConstraints; - b2ContactVelocityConstraint* m_velocityConstraints; - b2Contact** m_contacts; - int m_count; + b2TimeStep m_step; + b2Position * m_positions; + b2Velocity * m_velocities; + b2StackAllocator * m_allocator; + b2ContactPositionConstraint * m_positionConstraints; + b2ContactVelocityConstraint * m_velocityConstraints; + b2Contact ** m_contacts; + int m_count; }; #endif - diff --git a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h index f94280ad..93a8cd8e 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h +++ b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_EDGE_AND_CIRCLE_CONTACT_H #define B2_EDGE_AND_CIRCLE_CONTACT_H @@ -26,14 +26,15 @@ class b2BlockAllocator; class b2EdgeAndCircleContact : public b2Contact { public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + static b2Contact * Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator); + static void Destroy(b2Contact * contact, b2BlockAllocator * allocator); - b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2EdgeAndCircleContact() {} + b2EdgeAndCircleContact(b2Fixture * fixtureA, b2Fixture * fixtureB); + ~b2EdgeAndCircleContact() {} - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override; + void Evaluate(b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) override; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h index 219aa0f3..7cfd09c6 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h +++ b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_EDGE_AND_POLYGON_CONTACT_H #define B2_EDGE_AND_POLYGON_CONTACT_H @@ -26,14 +26,15 @@ class b2BlockAllocator; class b2EdgeAndPolygonContact : public b2Contact { public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + static b2Contact * Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator); + static void Destroy(b2Contact * contact, b2BlockAllocator * allocator); - b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2EdgeAndPolygonContact() {} + b2EdgeAndPolygonContact(b2Fixture * fixtureA, b2Fixture * fixtureB); + ~b2EdgeAndPolygonContact() {} - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override; + void Evaluate(b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) override; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h index 83cafa92..3724b743 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h +++ b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_POLYGON_AND_CIRCLE_CONTACT_H #define B2_POLYGON_AND_CIRCLE_CONTACT_H @@ -26,13 +26,15 @@ class b2BlockAllocator; class b2PolygonAndCircleContact : public b2Contact { public: - static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + static b2Contact * Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator); + static void Destroy(b2Contact * contact, b2BlockAllocator * allocator); - b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2PolygonAndCircleContact() {} + b2PolygonAndCircleContact(b2Fixture * fixtureA, b2Fixture * fixtureB); + ~b2PolygonAndCircleContact() {} - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override; + void Evaluate(b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) override; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonContact.h b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonContact.h index b03260a6..9cc7987f 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonContact.h +++ b/flatland_box2d/include/Box2D/Dynamics/Contacts/b2PolygonContact.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_POLYGON_CONTACT_H #define B2_POLYGON_CONTACT_H @@ -26,14 +26,15 @@ class b2BlockAllocator; class b2PolygonContact : public b2Contact { public: - static b2Contact* Create( b2Fixture* fixtureA, int32 indexA, - b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator); - static void Destroy(b2Contact* contact, b2BlockAllocator* allocator); + static b2Contact * Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator); + static void Destroy(b2Contact * contact, b2BlockAllocator * allocator); - b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB); - ~b2PolygonContact() {} + b2PolygonContact(b2Fixture * fixtureA, b2Fixture * fixtureB); + ~b2PolygonContact() {} - void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override; + void Evaluate(b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) override; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2DistanceJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2DistanceJoint.h index b52392c5..9c3fddbc 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2DistanceJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2DistanceJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_DISTANCE_JOINT_H #define B2_DISTANCE_JOINT_H @@ -29,36 +29,35 @@ /// @warning Do not use a zero or short length. struct b2DistanceJointDef : public b2JointDef { - b2DistanceJointDef() - { - type = e_distanceJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - length = 1.0f; - frequencyHz = 0.0f; - dampingRatio = 0.0f; - } - - /// Initialize the bodies, anchors, and length using the world - /// anchors. - void Initialize(b2Body* bodyA, b2Body* bodyB, - const b2Vec2& anchorA, const b2Vec2& anchorB); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The natural length between the anchor points. - float32 length; - - /// The mass-spring-damper frequency in Hertz. A value of 0 - /// disables softness. - float32 frequencyHz; - - /// The damping ratio. 0 = no damping, 1 = critical damping. - float32 dampingRatio; + b2DistanceJointDef() + { + type = e_distanceJoint; + localAnchorA.Set(0.0f, 0.0f); + localAnchorB.Set(0.0f, 0.0f); + length = 1.0f; + frequencyHz = 0.0f; + dampingRatio = 0.0f; + } + + /// Initialize the bodies, anchors, and length using the world + /// anchors. + void Initialize(b2Body * bodyA, b2Body * bodyB, const b2Vec2 & anchorA, const b2Vec2 & anchorB); + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The natural length between the anchor points. + float32 length; + + /// The mass-spring-damper frequency in Hertz. A value of 0 + /// disables softness. + float32 frequencyHz; + + /// The damping ratio. 0 = no damping, 1 = critical damping. + float32 dampingRatio; }; /// A distance joint constrains two points on two bodies @@ -67,103 +66,84 @@ struct b2DistanceJointDef : public b2JointDef class b2DistanceJoint : public b2Joint { public: + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + /// Get the reaction force given the inverse time step. + /// Unit is N. + b2Vec2 GetReactionForce(float32 inv_dt) const override; - /// Get the reaction force given the inverse time step. - /// Unit is N. - b2Vec2 GetReactionForce(float32 inv_dt) const override; + /// Get the reaction torque given the inverse time step. + /// Unit is N*m. This is always zero for a distance joint. + float32 GetReactionTorque(float32 inv_dt) const override; - /// Get the reaction torque given the inverse time step. - /// Unit is N*m. This is always zero for a distance joint. - float32 GetReactionTorque(float32 inv_dt) const override; + /// The local anchor point relative to bodyA's origin. + const b2Vec2 & GetLocalAnchorA() const { return m_localAnchorA; } - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + /// The local anchor point relative to bodyB's origin. + const b2Vec2 & GetLocalAnchorB() const { return m_localAnchorB; } - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + /// Set/get the natural length. + /// Manipulating the length can lead to non-physical behavior when the + /// frequency is zero. + void SetLength(float32 length); + float32 GetLength() const; - /// Set/get the natural length. - /// Manipulating the length can lead to non-physical behavior when the frequency is zero. - void SetLength(float32 length); - float32 GetLength() const; + /// Set/get frequency in Hz. + void SetFrequency(float32 hz); + float32 GetFrequency() const; - /// Set/get frequency in Hz. - void SetFrequency(float32 hz); - float32 GetFrequency() const; + /// Set/get damping ratio. + void SetDampingRatio(float32 ratio); + float32 GetDampingRatio() const; - /// Set/get damping ratio. - void SetDampingRatio(float32 ratio); - float32 GetDampingRatio() const; - - /// Dump joint to dmLog - void Dump(); + /// Dump joint to dmLog + void Dump(); protected: - - friend class b2Joint; - b2DistanceJoint(const b2DistanceJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - float32 m_frequencyHz; - float32 m_dampingRatio; - float32 m_bias; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_gamma; - float32 m_impulse; - float32 m_length; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_u; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; + friend class b2Joint; + b2DistanceJoint(const b2DistanceJointDef * data); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + float32 m_frequencyHz; + float32 m_dampingRatio; + float32 m_bias; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float32 m_gamma; + float32 m_impulse; + float32 m_length; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_u; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + float32 m_mass; }; -inline void b2DistanceJoint::SetLength(float32 length) -{ - m_length = length; -} +inline void b2DistanceJoint::SetLength(float32 length) { m_length = length; } -inline float32 b2DistanceJoint::GetLength() const -{ - return m_length; -} +inline float32 b2DistanceJoint::GetLength() const { return m_length; } -inline void b2DistanceJoint::SetFrequency(float32 hz) -{ - m_frequencyHz = hz; -} +inline void b2DistanceJoint::SetFrequency(float32 hz) { m_frequencyHz = hz; } -inline float32 b2DistanceJoint::GetFrequency() const -{ - return m_frequencyHz; -} +inline float32 b2DistanceJoint::GetFrequency() const { return m_frequencyHz; } -inline void b2DistanceJoint::SetDampingRatio(float32 ratio) -{ - m_dampingRatio = ratio; -} +inline void b2DistanceJoint::SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; } -inline float32 b2DistanceJoint::GetDampingRatio() const -{ - return m_dampingRatio; -} +inline float32 b2DistanceJoint::GetDampingRatio() const { return m_dampingRatio; } #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2FrictionJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2FrictionJoint.h index ff191f20..3a6c8752 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2FrictionJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2FrictionJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_FRICTION_JOINT_H #define B2_FRICTION_JOINT_H @@ -24,30 +24,30 @@ /// Friction joint definition. struct b2FrictionJointDef : public b2JointDef { - b2FrictionJointDef() - { - type = e_frictionJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - maxForce = 0.0f; - maxTorque = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The maximum friction force in N. - float32 maxForce; - - /// The maximum friction torque in N-m. - float32 maxTorque; + b2FrictionJointDef() + { + type = e_frictionJoint; + localAnchorA.SetZero(); + localAnchorB.SetZero(); + maxForce = 0.0f; + maxTorque = 0.0f; + } + + /// Initialize the bodies, anchors, axis, and reference angle using the world + /// anchor and world axis. + void Initialize(b2Body * bodyA, b2Body * bodyB, const b2Vec2 & anchor); + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The maximum friction force in N. + float32 maxForce; + + /// The maximum friction torque in N-m. + float32 maxTorque; }; /// Friction joint. This is used for top-down friction. @@ -55,65 +55,64 @@ struct b2FrictionJointDef : public b2JointDef class b2FrictionJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const override; - float32 GetReactionTorque(float32 inv_dt) const override; + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + /// The local anchor point relative to bodyA's origin. + const b2Vec2 & GetLocalAnchorA() const { return m_localAnchorA; } - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + /// The local anchor point relative to bodyB's origin. + const b2Vec2 & GetLocalAnchorB() const { return m_localAnchorB; } - /// Set the maximum friction force in N. - void SetMaxForce(float32 force); + /// Set the maximum friction force in N. + void SetMaxForce(float32 force); - /// Get the maximum friction force in N. - float32 GetMaxForce() const; + /// Get the maximum friction force in N. + float32 GetMaxForce() const; - /// Set the maximum friction torque in N*m. - void SetMaxTorque(float32 torque); + /// Set the maximum friction torque in N*m. + void SetMaxTorque(float32 torque); - /// Get the maximum friction torque in N*m. - float32 GetMaxTorque() const; + /// Get the maximum friction torque in N*m. + float32 GetMaxTorque() const; - /// Dump joint to dmLog - void Dump() override; + /// Dump joint to dmLog + void Dump() override; protected: - - friend class b2Joint; - - b2FrictionJoint(const b2FrictionJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - - // Solver shared - b2Vec2 m_linearImpulse; - float32 m_angularImpulse; - float32 m_maxForce; - float32 m_maxTorque; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat22 m_linearMass; - float32 m_angularMass; + friend class b2Joint; + + b2FrictionJoint(const b2FrictionJointDef * def); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + + // Solver shared + b2Vec2 m_linearImpulse; + float32 m_angularImpulse; + float32 m_maxForce; + float32 m_maxTorque; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + b2Mat22 m_linearMass; + float32 m_angularMass; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2GearJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2GearJoint.h index fb37eeff..218e9f77 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2GearJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2GearJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_GEAR_JOINT_H #define B2_GEAR_JOINT_H @@ -25,23 +25,23 @@ /// revolute or prismatic joints (any combination will work). struct b2GearJointDef : public b2JointDef { - b2GearJointDef() - { - type = e_gearJoint; - joint1 = nullptr; - joint2 = nullptr; - ratio = 1.0f; - } - - /// The first revolute/prismatic joint attached to the gear joint. - b2Joint* joint1; - - /// The second revolute/prismatic joint attached to the gear joint. - b2Joint* joint2; - - /// The gear ratio. - /// @see b2GearJoint for explanation. - float32 ratio; + b2GearJointDef() + { + type = e_gearJoint; + joint1 = nullptr; + joint2 = nullptr; + ratio = 1.0f; + } + + /// The first revolute/prismatic joint attached to the gear joint. + b2Joint * joint1; + + /// The second revolute/prismatic joint attached to the gear joint. + b2Joint * joint2; + + /// The gear ratio. + /// @see b2GearJoint for explanation. + float32 ratio; }; /// A gear joint is used to connect two joints together. Either joint @@ -56,70 +56,69 @@ struct b2GearJointDef : public b2JointDef class b2GearJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const override; - float32 GetReactionTorque(float32 inv_dt) const override; + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; - /// Get the first joint. - b2Joint* GetJoint1() { return m_joint1; } + /// Get the first joint. + b2Joint * GetJoint1() { return m_joint1; } - /// Get the second joint. - b2Joint* GetJoint2() { return m_joint2; } + /// Get the second joint. + b2Joint * GetJoint2() { return m_joint2; } - /// Set/Get the gear ratio. - void SetRatio(float32 ratio); - float32 GetRatio() const; + /// Set/Get the gear ratio. + void SetRatio(float32 ratio); + float32 GetRatio() const; - /// Dump joint to dmLog - void Dump() override; + /// Dump joint to dmLog + void Dump() override; protected: + friend class b2Joint; + b2GearJoint(const b2GearJointDef * data); - friend class b2Joint; - b2GearJoint(const b2GearJointDef* data); + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; + b2Joint * m_joint1; + b2Joint * m_joint2; - b2Joint* m_joint1; - b2Joint* m_joint2; + b2JointType m_typeA; + b2JointType m_typeB; - b2JointType m_typeA; - b2JointType m_typeB; + // Body A is connected to body C + // Body B is connected to body D + b2Body * m_bodyC; + b2Body * m_bodyD; - // Body A is connected to body C - // Body B is connected to body D - b2Body* m_bodyC; - b2Body* m_bodyD; + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec2 m_localAnchorC; + b2Vec2 m_localAnchorD; - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localAnchorC; - b2Vec2 m_localAnchorD; + b2Vec2 m_localAxisC; + b2Vec2 m_localAxisD; - b2Vec2 m_localAxisC; - b2Vec2 m_localAxisD; + float32 m_referenceAngleA; + float32 m_referenceAngleB; - float32 m_referenceAngleA; - float32 m_referenceAngleB; + float32 m_constant; + float32 m_ratio; - float32 m_constant; - float32 m_ratio; + float32 m_impulse; - float32 m_impulse; - - // Solver temp - int32 m_indexA, m_indexB, m_indexC, m_indexD; - b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD; - float32 m_mA, m_mB, m_mC, m_mD; - float32 m_iA, m_iB, m_iC, m_iD; - b2Vec2 m_JvAC, m_JvBD; - float32 m_JwA, m_JwB, m_JwC, m_JwD; - float32 m_mass; + // Solver temp + int32 m_indexA, m_indexB, m_indexC, m_indexD; + b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD; + float32 m_mA, m_mB, m_mC, m_mD; + float32 m_iA, m_iB, m_iC, m_iD; + b2Vec2 m_JvAC, m_JvBD; + float32 m_JwA, m_JwB, m_JwC, m_JwD; + float32 m_mass; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2Joint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2Joint.h index 6b6aadc3..2d9398e9 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2Joint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2Joint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_JOINT_H #define B2_JOINT_H @@ -26,35 +26,28 @@ class b2Joint; struct b2SolverData; class b2BlockAllocator; -enum b2JointType -{ - e_unknownJoint, - e_revoluteJoint, - e_prismaticJoint, - e_distanceJoint, - e_pulleyJoint, - e_mouseJoint, - e_gearJoint, - e_wheelJoint, - e_weldJoint, - e_frictionJoint, - e_ropeJoint, - e_motorJoint +enum b2JointType { + e_unknownJoint, + e_revoluteJoint, + e_prismaticJoint, + e_distanceJoint, + e_pulleyJoint, + e_mouseJoint, + e_gearJoint, + e_wheelJoint, + e_weldJoint, + e_frictionJoint, + e_ropeJoint, + e_motorJoint }; -enum b2LimitState -{ - e_inactiveLimit, - e_atLowerLimit, - e_atUpperLimit, - e_equalLimits -}; +enum b2LimitState { e_inactiveLimit, e_atLowerLimit, e_atUpperLimit, e_equalLimits }; struct b2Jacobian { - b2Vec2 linear; - float32 angularA; - float32 angularB; + b2Vec2 linear; + float32 angularA; + float32 angularB; }; /// A joint edge is used to connect bodies and joints together @@ -64,38 +57,38 @@ struct b2Jacobian /// nodes, one for each attached body. struct b2JointEdge { - b2Body* other; ///< provides quick access to the other body attached. - b2Joint* joint; ///< the joint - b2JointEdge* prev; ///< the previous joint edge in the body's joint list - b2JointEdge* next; ///< the next joint edge in the body's joint list + b2Body * other; ///< provides quick access to the other body attached. + b2Joint * joint; ///< the joint + b2JointEdge * prev; ///< the previous joint edge in the body's joint list + b2JointEdge * next; ///< the next joint edge in the body's joint list }; /// Joint definitions are used to construct joints. struct b2JointDef { - b2JointDef() - { - type = e_unknownJoint; - userData = nullptr; - bodyA = nullptr; - bodyB = nullptr; - collideConnected = false; - } + b2JointDef() + { + type = e_unknownJoint; + userData = nullptr; + bodyA = nullptr; + bodyB = nullptr; + collideConnected = false; + } - /// The joint type is set automatically for concrete joint types. - b2JointType type; + /// The joint type is set automatically for concrete joint types. + b2JointType type; - /// Use this to attach application specific data to your joints. - void* userData; + /// Use this to attach application specific data to your joints. + void * userData; - /// The first attached body. - b2Body* bodyA; + /// The first attached body. + b2Body * bodyA; - /// The second attached body. - b2Body* bodyB; + /// The second attached body. + b2Body * bodyB; - /// Set this flag to true if the attached bodies should collide. - bool collideConnected; + /// Set this flag to true if the attached bodies should collide. + bool collideConnected; }; /// The base joint class. Joints are used to constraint two bodies together in @@ -103,124 +96,99 @@ struct b2JointDef class b2Joint { public: + /// Get the type of the concrete joint. + b2JointType GetType() const; - /// Get the type of the concrete joint. - b2JointType GetType() const; + /// Get the first body attached to this joint. + b2Body * GetBodyA(); - /// Get the first body attached to this joint. - b2Body* GetBodyA(); + /// Get the second body attached to this joint. + b2Body * GetBodyB(); - /// Get the second body attached to this joint. - b2Body* GetBodyB(); + /// Get the anchor point on bodyA in world coordinates. + virtual b2Vec2 GetAnchorA() const = 0; - /// Get the anchor point on bodyA in world coordinates. - virtual b2Vec2 GetAnchorA() const = 0; + /// Get the anchor point on bodyB in world coordinates. + virtual b2Vec2 GetAnchorB() const = 0; - /// Get the anchor point on bodyB in world coordinates. - virtual b2Vec2 GetAnchorB() const = 0; + /// Get the reaction force on bodyB at the joint anchor in Newtons. + virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0; - /// Get the reaction force on bodyB at the joint anchor in Newtons. - virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0; + /// Get the reaction torque on bodyB in N*m. + virtual float32 GetReactionTorque(float32 inv_dt) const = 0; - /// Get the reaction torque on bodyB in N*m. - virtual float32 GetReactionTorque(float32 inv_dt) const = 0; + /// Get the next joint the world joint list. + b2Joint * GetNext(); + const b2Joint * GetNext() const; - /// Get the next joint the world joint list. - b2Joint* GetNext(); - const b2Joint* GetNext() const; + /// Get the user data pointer. + void * GetUserData() const; - /// Get the user data pointer. - void* GetUserData() const; + /// Set the user data pointer. + void SetUserData(void * data); - /// Set the user data pointer. - void SetUserData(void* data); + /// Short-cut function to determine if either body is inactive. + bool IsActive() const; - /// Short-cut function to determine if either body is inactive. - bool IsActive() const; + /// Get collide connected. + /// Note: modifying the collide connect flag won't work correctly because + /// the flag is only checked when fixture AABBs begin to overlap. + bool GetCollideConnected() const; - /// Get collide connected. - /// Note: modifying the collide connect flag won't work correctly because - /// the flag is only checked when fixture AABBs begin to overlap. - bool GetCollideConnected() const; + /// Dump this joint to the log file. + virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); } - /// Dump this joint to the log file. - virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); } - - /// Shift the origin for any points stored in world coordinates. - virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); } + /// Shift the origin for any points stored in world coordinates. + virtual void ShiftOrigin(const b2Vec2 & newOrigin) { B2_NOT_USED(newOrigin); } protected: - friend class b2World; - friend class b2Body; - friend class b2Island; - friend class b2GearJoint; + friend class b2World; + friend class b2Body; + friend class b2Island; + friend class b2GearJoint; - static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator); - static void Destroy(b2Joint* joint, b2BlockAllocator* allocator); + static b2Joint * Create(const b2JointDef * def, b2BlockAllocator * allocator); + static void Destroy(b2Joint * joint, b2BlockAllocator * allocator); - b2Joint(const b2JointDef* def); - virtual ~b2Joint() {} + b2Joint(const b2JointDef * def); + virtual ~b2Joint() {} - virtual void InitVelocityConstraints(const b2SolverData& data) = 0; - virtual void SolveVelocityConstraints(const b2SolverData& data) = 0; + virtual void InitVelocityConstraints(const b2SolverData & data) = 0; + virtual void SolveVelocityConstraints(const b2SolverData & data) = 0; - // This returns true if the position errors are within tolerance. - virtual bool SolvePositionConstraints(const b2SolverData& data) = 0; + // This returns true if the position errors are within tolerance. + virtual bool SolvePositionConstraints(const b2SolverData & data) = 0; - b2JointType m_type; - b2Joint* m_prev; - b2Joint* m_next; - b2JointEdge m_edgeA; - b2JointEdge m_edgeB; - b2Body* m_bodyA; - b2Body* m_bodyB; + b2JointType m_type; + b2Joint * m_prev; + b2Joint * m_next; + b2JointEdge m_edgeA; + b2JointEdge m_edgeB; + b2Body * m_bodyA; + b2Body * m_bodyB; - int32 m_index; + int32 m_index; - bool m_islandFlag; - bool m_collideConnected; + bool m_islandFlag; + bool m_collideConnected; - void* m_userData; + void * m_userData; }; -inline b2JointType b2Joint::GetType() const -{ - return m_type; -} +inline b2JointType b2Joint::GetType() const { return m_type; } -inline b2Body* b2Joint::GetBodyA() -{ - return m_bodyA; -} +inline b2Body * b2Joint::GetBodyA() { return m_bodyA; } -inline b2Body* b2Joint::GetBodyB() -{ - return m_bodyB; -} +inline b2Body * b2Joint::GetBodyB() { return m_bodyB; } -inline b2Joint* b2Joint::GetNext() -{ - return m_next; -} +inline b2Joint * b2Joint::GetNext() { return m_next; } -inline const b2Joint* b2Joint::GetNext() const -{ - return m_next; -} +inline const b2Joint * b2Joint::GetNext() const { return m_next; } -inline void* b2Joint::GetUserData() const -{ - return m_userData; -} +inline void * b2Joint::GetUserData() const { return m_userData; } -inline void b2Joint::SetUserData(void* data) -{ - m_userData = data; -} +inline void b2Joint::SetUserData(void * data) { m_userData = data; } -inline bool b2Joint::GetCollideConnected() const -{ - return m_collideConnected; -} +inline bool b2Joint::GetCollideConnected() const { return m_collideConnected; } #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2MotorJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2MotorJoint.h index 0734a39b..02da1a0e 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2MotorJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2MotorJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2012 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_MOTOR_JOINT_H #define B2_MOTOR_JOINT_H @@ -24,33 +24,34 @@ /// Motor joint definition. struct b2MotorJointDef : public b2JointDef { - b2MotorJointDef() - { - type = e_motorJoint; - linearOffset.SetZero(); - angularOffset = 0.0f; - maxForce = 1.0f; - maxTorque = 1.0f; - correctionFactor = 0.3f; - } - - /// Initialize the bodies and offsets using the current transforms. - void Initialize(b2Body* bodyA, b2Body* bodyB); - - /// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters. - b2Vec2 linearOffset; - - /// The bodyB angle minus bodyA angle in radians. - float32 angularOffset; - - /// The maximum motor force in N. - float32 maxForce; - - /// The maximum motor torque in N-m. - float32 maxTorque; - - /// Position correction factor in the range [0,1]. - float32 correctionFactor; + b2MotorJointDef() + { + type = e_motorJoint; + linearOffset.SetZero(); + angularOffset = 0.0f; + maxForce = 1.0f; + maxTorque = 1.0f; + correctionFactor = 0.3f; + } + + /// Initialize the bodies and offsets using the current transforms. + void Initialize(b2Body * bodyA, b2Body * bodyB); + + /// Position of bodyB minus the position of bodyA, in bodyA's frame, in + /// meters. + b2Vec2 linearOffset; + + /// The bodyB angle minus bodyA angle in radians. + float32 angularOffset; + + /// The maximum motor force in N. + float32 maxForce; + + /// The maximum motor torque in N-m. + float32 maxTorque; + + /// Position correction factor in the range [0,1]. + float32 correctionFactor; }; /// A motor joint is used to control the relative motion @@ -59,75 +60,74 @@ struct b2MotorJointDef : public b2JointDef class b2MotorJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const override; - float32 GetReactionTorque(float32 inv_dt) const override; + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; - /// Set/get the target linear offset, in frame A, in meters. - void SetLinearOffset(const b2Vec2& linearOffset); - const b2Vec2& GetLinearOffset() const; + /// Set/get the target linear offset, in frame A, in meters. + void SetLinearOffset(const b2Vec2 & linearOffset); + const b2Vec2 & GetLinearOffset() const; - /// Set/get the target angular offset, in radians. - void SetAngularOffset(float32 angularOffset); - float32 GetAngularOffset() const; + /// Set/get the target angular offset, in radians. + void SetAngularOffset(float32 angularOffset); + float32 GetAngularOffset() const; - /// Set the maximum friction force in N. - void SetMaxForce(float32 force); + /// Set the maximum friction force in N. + void SetMaxForce(float32 force); - /// Get the maximum friction force in N. - float32 GetMaxForce() const; + /// Get the maximum friction force in N. + float32 GetMaxForce() const; - /// Set the maximum friction torque in N*m. - void SetMaxTorque(float32 torque); + /// Set the maximum friction torque in N*m. + void SetMaxTorque(float32 torque); - /// Get the maximum friction torque in N*m. - float32 GetMaxTorque() const; + /// Get the maximum friction torque in N*m. + float32 GetMaxTorque() const; - /// Set the position correction factor in the range [0,1]. - void SetCorrectionFactor(float32 factor); + /// Set the position correction factor in the range [0,1]. + void SetCorrectionFactor(float32 factor); - /// Get the position correction factor in the range [0,1]. - float32 GetCorrectionFactor() const; + /// Get the position correction factor in the range [0,1]. + float32 GetCorrectionFactor() const; - /// Dump to b2Log - void Dump() override; + /// Dump to b2Log + void Dump() override; protected: - - friend class b2Joint; - - b2MotorJoint(const b2MotorJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - // Solver shared - b2Vec2 m_linearOffset; - float32 m_angularOffset; - b2Vec2 m_linearImpulse; - float32 m_angularImpulse; - float32 m_maxForce; - float32 m_maxTorque; - float32 m_correctionFactor; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - b2Vec2 m_linearError; - float32 m_angularError; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat22 m_linearMass; - float32 m_angularMass; + friend class b2Joint; + + b2MotorJoint(const b2MotorJointDef * def); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + // Solver shared + b2Vec2 m_linearOffset; + float32 m_angularOffset; + b2Vec2 m_linearImpulse; + float32 m_angularImpulse; + float32 m_maxForce; + float32 m_maxTorque; + float32 m_correctionFactor; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + b2Vec2 m_linearError; + float32 m_angularError; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + b2Mat22 m_linearMass; + float32 m_angularMass; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2MouseJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2MouseJoint.h index 2606f2f7..fe8bad1f 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2MouseJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2MouseJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_MOUSE_JOINT_H #define B2_MOUSE_JOINT_H @@ -25,29 +25,29 @@ /// tuning parameters, and the time step. struct b2MouseJointDef : public b2JointDef { - b2MouseJointDef() - { - type = e_mouseJoint; - target.Set(0.0f, 0.0f); - maxForce = 0.0f; - frequencyHz = 5.0f; - dampingRatio = 0.7f; - } - - /// The initial world target point. This is assumed - /// to coincide with the body anchor initially. - b2Vec2 target; - - /// The maximum constraint force that can be exerted - /// to move the candidate body. Usually you will express - /// as some multiple of the weight (multiplier * mass * gravity). - float32 maxForce; - - /// The response speed. - float32 frequencyHz; - - /// The damping ratio. 0 = no damping, 1 = critical damping. - float32 dampingRatio; + b2MouseJointDef() + { + type = e_mouseJoint; + target.Set(0.0f, 0.0f); + maxForce = 0.0f; + frequencyHz = 5.0f; + dampingRatio = 0.7f; + } + + /// The initial world target point. This is assumed + /// to coincide with the body anchor initially. + b2Vec2 target; + + /// The maximum constraint force that can be exerted + /// to move the candidate body. Usually you will express + /// as some multiple of the weight (multiplier * mass * gravity). + float32 maxForce; + + /// The response speed. + float32 frequencyHz; + + /// The damping ratio. 0 = no damping, 1 = critical damping. + float32 dampingRatio; }; /// A mouse joint is used to make a point on a body track a @@ -60,70 +60,69 @@ struct b2MouseJointDef : public b2JointDef class b2MouseJoint : public b2Joint { public: + /// Implements b2Joint. + b2Vec2 GetAnchorA() const override; - /// Implements b2Joint. - b2Vec2 GetAnchorA() const override; + /// Implements b2Joint. + b2Vec2 GetAnchorB() const override; - /// Implements b2Joint. - b2Vec2 GetAnchorB() const override; + /// Implements b2Joint. + b2Vec2 GetReactionForce(float32 inv_dt) const override; - /// Implements b2Joint. - b2Vec2 GetReactionForce(float32 inv_dt) const override; + /// Implements b2Joint. + float32 GetReactionTorque(float32 inv_dt) const override; - /// Implements b2Joint. - float32 GetReactionTorque(float32 inv_dt) const override; + /// Use this to update the target point. + void SetTarget(const b2Vec2 & target); + const b2Vec2 & GetTarget() const; - /// Use this to update the target point. - void SetTarget(const b2Vec2& target); - const b2Vec2& GetTarget() const; + /// Set/get the maximum force in Newtons. + void SetMaxForce(float32 force); + float32 GetMaxForce() const; - /// Set/get the maximum force in Newtons. - void SetMaxForce(float32 force); - float32 GetMaxForce() const; + /// Set/get the frequency in Hertz. + void SetFrequency(float32 hz); + float32 GetFrequency() const; - /// Set/get the frequency in Hertz. - void SetFrequency(float32 hz); - float32 GetFrequency() const; + /// Set/get the damping ratio (dimensionless). + void SetDampingRatio(float32 ratio); + float32 GetDampingRatio() const; - /// Set/get the damping ratio (dimensionless). - void SetDampingRatio(float32 ratio); - float32 GetDampingRatio() const; + /// The mouse joint does not support dumping. + void Dump() override { b2Log("Mouse joint dumping is not supported.\n"); } - /// The mouse joint does not support dumping. - void Dump() override { b2Log("Mouse joint dumping is not supported.\n"); } - - /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin) override; + /// Implement b2Joint::ShiftOrigin + void ShiftOrigin(const b2Vec2 & newOrigin) override; protected: - friend class b2Joint; - - b2MouseJoint(const b2MouseJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - b2Vec2 m_localAnchorB; - b2Vec2 m_targetA; - float32 m_frequencyHz; - float32 m_dampingRatio; - float32 m_beta; - - // Solver shared - b2Vec2 m_impulse; - float32 m_maxForce; - float32 m_gamma; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rB; - b2Vec2 m_localCenterB; - float32 m_invMassB; - float32 m_invIB; - b2Mat22 m_mass; - b2Vec2 m_C; + friend class b2Joint; + + b2MouseJoint(const b2MouseJointDef * def); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + b2Vec2 m_localAnchorB; + b2Vec2 m_targetA; + float32 m_frequencyHz; + float32 m_dampingRatio; + float32 m_beta; + + // Solver shared + b2Vec2 m_impulse; + float32 m_maxForce; + float32 m_gamma; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rB; + b2Vec2 m_localCenterB; + float32 m_invMassB; + float32 m_invIB; + b2Mat22 m_mass; + b2Vec2 m_C; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2PrismaticJoint.h index 05047fe4..f98aea5a 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2PrismaticJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2PrismaticJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_PRISMATIC_JOINT_H #define B2_PRISMATIC_JOINT_H @@ -29,54 +29,54 @@ /// anchors and a local axis helps when saving and loading a game. struct b2PrismaticJointDef : public b2JointDef { - b2PrismaticJointDef() - { - type = e_prismaticJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - localAxisA.Set(1.0f, 0.0f); - referenceAngle = 0.0f; - enableLimit = false; - lowerTranslation = 0.0f; - upperTranslation = 0.0f; - enableMotor = false; - maxMotorForce = 0.0f; - motorSpeed = 0.0f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and unit world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The local translation unit axis in bodyA. - b2Vec2 localAxisA; - - /// The constrained angle between the bodies: bodyB_angle - bodyA_angle. - float32 referenceAngle; - - /// Enable/disable the joint limit. - bool enableLimit; - - /// The lower translation limit, usually in meters. - float32 lowerTranslation; - - /// The upper translation limit, usually in meters. - float32 upperTranslation; - - /// Enable/disable the joint motor. - bool enableMotor; - - /// The maximum motor torque, usually in N-m. - float32 maxMotorForce; - - /// The desired motor speed in radians per second. - float32 motorSpeed; + b2PrismaticJointDef() + { + type = e_prismaticJoint; + localAnchorA.SetZero(); + localAnchorB.SetZero(); + localAxisA.Set(1.0f, 0.0f); + referenceAngle = 0.0f; + enableLimit = false; + lowerTranslation = 0.0f; + upperTranslation = 0.0f; + enableMotor = false; + maxMotorForce = 0.0f; + motorSpeed = 0.0f; + } + + /// Initialize the bodies, anchors, axis, and reference angle using the world + /// anchor and unit world axis. + void Initialize(b2Body * bodyA, b2Body * bodyB, const b2Vec2 & anchor, const b2Vec2 & axis); + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The local translation unit axis in bodyA. + b2Vec2 localAxisA; + + /// The constrained angle between the bodies: bodyB_angle - bodyA_angle. + float32 referenceAngle; + + /// Enable/disable the joint limit. + bool enableLimit; + + /// The lower translation limit, usually in meters. + float32 lowerTranslation; + + /// The upper translation limit, usually in meters. + float32 upperTranslation; + + /// Enable/disable the joint motor. + bool enableMotor; + + /// The maximum motor torque, usually in N-m. + float32 maxMotorForce; + + /// The desired motor speed in radians per second. + float32 motorSpeed; }; /// A prismatic joint. This joint provides one degree of freedom: translation @@ -86,111 +86,108 @@ struct b2PrismaticJointDef : public b2JointDef class b2PrismaticJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const override; - float32 GetReactionTorque(float32 inv_dt) const override; + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + /// The local anchor point relative to bodyA's origin. + const b2Vec2 & GetLocalAnchorA() const { return m_localAnchorA; } - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + /// The local anchor point relative to bodyB's origin. + const b2Vec2 & GetLocalAnchorB() const { return m_localAnchorB; } - /// The local joint axis relative to bodyA. - const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } + /// The local joint axis relative to bodyA. + const b2Vec2 & GetLocalAxisA() const { return m_localXAxisA; } - /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } + /// Get the reference angle. + float32 GetReferenceAngle() const { return m_referenceAngle; } - /// Get the current joint translation, usually in meters. - float32 GetJointTranslation() const; + /// Get the current joint translation, usually in meters. + float32 GetJointTranslation() const; - /// Get the current joint translation speed, usually in meters per second. - float32 GetJointSpeed() const; + /// Get the current joint translation speed, usually in meters per second. + float32 GetJointSpeed() const; - /// Is the joint limit enabled? - bool IsLimitEnabled() const; + /// Is the joint limit enabled? + bool IsLimitEnabled() const; - /// Enable/disable the joint limit. - void EnableLimit(bool flag); + /// Enable/disable the joint limit. + void EnableLimit(bool flag); - /// Get the lower joint limit, usually in meters. - float32 GetLowerLimit() const; + /// Get the lower joint limit, usually in meters. + float32 GetLowerLimit() const; - /// Get the upper joint limit, usually in meters. - float32 GetUpperLimit() const; + /// Get the upper joint limit, usually in meters. + float32 GetUpperLimit() const; - /// Set the joint limits, usually in meters. - void SetLimits(float32 lower, float32 upper); + /// Set the joint limits, usually in meters. + void SetLimits(float32 lower, float32 upper); - /// Is the joint motor enabled? - bool IsMotorEnabled() const; + /// Is the joint motor enabled? + bool IsMotorEnabled() const; - /// Enable/disable the joint motor. - void EnableMotor(bool flag); + /// Enable/disable the joint motor. + void EnableMotor(bool flag); - /// Set the motor speed, usually in meters per second. - void SetMotorSpeed(float32 speed); + /// Set the motor speed, usually in meters per second. + void SetMotorSpeed(float32 speed); - /// Get the motor speed, usually in meters per second. - float32 GetMotorSpeed() const; + /// Get the motor speed, usually in meters per second. + float32 GetMotorSpeed() const; - /// Set the maximum motor force, usually in N. - void SetMaxMotorForce(float32 force); - float32 GetMaxMotorForce() const { return m_maxMotorForce; } + /// Set the maximum motor force, usually in N. + void SetMaxMotorForce(float32 force); + float32 GetMaxMotorForce() const { return m_maxMotorForce; } - /// Get the current motor force given the inverse time step, usually in N. - float32 GetMotorForce(float32 inv_dt) const; + /// Get the current motor force given the inverse time step, usually in N. + float32 GetMotorForce(float32 inv_dt) const; - /// Dump to b2Log - void Dump() override; + /// Dump to b2Log + void Dump() override; protected: - friend class b2Joint; - friend class b2GearJoint; - b2PrismaticJoint(const b2PrismaticJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localXAxisA; - b2Vec2 m_localYAxisA; - float32 m_referenceAngle; - b2Vec3 m_impulse; - float32 m_motorImpulse; - float32 m_lowerTranslation; - float32 m_upperTranslation; - float32 m_maxMotorForce; - float32 m_motorSpeed; - bool m_enableLimit; - bool m_enableMotor; - b2LimitState m_limitState; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Vec2 m_axis, m_perp; - float32 m_s1, m_s2; - float32 m_a1, m_a2; - b2Mat33 m_K; - float32 m_motorMass; + friend class b2Joint; + friend class b2GearJoint; + b2PrismaticJoint(const b2PrismaticJointDef * def); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec2 m_localXAxisA; + b2Vec2 m_localYAxisA; + float32 m_referenceAngle; + b2Vec3 m_impulse; + float32 m_motorImpulse; + float32 m_lowerTranslation; + float32 m_upperTranslation; + float32 m_maxMotorForce; + float32 m_motorSpeed; + bool m_enableLimit; + bool m_enableMotor; + b2LimitState m_limitState; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + b2Vec2 m_axis, m_perp; + float32 m_s1, m_s2; + float32 m_a1, m_a2; + b2Mat33 m_K; + float32 m_motorMass; }; -inline float32 b2PrismaticJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} +inline float32 b2PrismaticJoint::GetMotorSpeed() const { return m_motorSpeed; } #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2PulleyJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2PulleyJoint.h index 3eb5f4a1..f6f3c024 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2PulleyJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2PulleyJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_PULLEY_JOINT_H #define B2_PULLEY_JOINT_H @@ -27,45 +27,45 @@ const float32 b2_minPulleyLength = 2.0f; /// two dynamic body anchor points, and a pulley ratio. struct b2PulleyJointDef : public b2JointDef { - b2PulleyJointDef() - { - type = e_pulleyJoint; - groundAnchorA.Set(-1.0f, 1.0f); - groundAnchorB.Set(1.0f, 1.0f); - localAnchorA.Set(-1.0f, 0.0f); - localAnchorB.Set(1.0f, 0.0f); - lengthA = 0.0f; - lengthB = 0.0f; - ratio = 1.0f; - collideConnected = true; - } - - /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. - void Initialize(b2Body* bodyA, b2Body* bodyB, - const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB, - const b2Vec2& anchorA, const b2Vec2& anchorB, - float32 ratio); - - /// The first ground anchor in world coordinates. This point never moves. - b2Vec2 groundAnchorA; - - /// The second ground anchor in world coordinates. This point never moves. - b2Vec2 groundAnchorB; - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The a reference length for the segment attached to bodyA. - float32 lengthA; - - /// The a reference length for the segment attached to bodyB. - float32 lengthB; - - /// The pulley ratio, used to simulate a block-and-tackle. - float32 ratio; + b2PulleyJointDef() + { + type = e_pulleyJoint; + groundAnchorA.Set(-1.0f, 1.0f); + groundAnchorB.Set(1.0f, 1.0f); + localAnchorA.Set(-1.0f, 0.0f); + localAnchorB.Set(1.0f, 0.0f); + lengthA = 0.0f; + lengthB = 0.0f; + ratio = 1.0f; + collideConnected = true; + } + + /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the + /// world anchors. + void Initialize( + b2Body * bodyA, b2Body * bodyB, const b2Vec2 & groundAnchorA, const b2Vec2 & groundAnchorB, + const b2Vec2 & anchorA, const b2Vec2 & anchorB, float32 ratio); + + /// The first ground anchor in world coordinates. This point never moves. + b2Vec2 groundAnchorA; + + /// The second ground anchor in world coordinates. This point never moves. + b2Vec2 groundAnchorB; + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The a reference length for the segment attached to bodyA. + float32 lengthA; + + /// The a reference length for the segment attached to bodyB. + float32 lengthB; + + /// The pulley ratio, used to simulate a block-and-tackle. + float32 ratio; }; /// The pulley joint is connected to two bodies and two fixed ground points. @@ -79,74 +79,73 @@ struct b2PulleyJointDef : public b2JointDef class b2PulleyJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const override; - float32 GetReactionTorque(float32 inv_dt) const override; + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; - /// Get the first ground anchor. - b2Vec2 GetGroundAnchorA() const; + /// Get the first ground anchor. + b2Vec2 GetGroundAnchorA() const; - /// Get the second ground anchor. - b2Vec2 GetGroundAnchorB() const; + /// Get the second ground anchor. + b2Vec2 GetGroundAnchorB() const; - /// Get the current length of the segment attached to bodyA. - float32 GetLengthA() const; + /// Get the current length of the segment attached to bodyA. + float32 GetLengthA() const; - /// Get the current length of the segment attached to bodyB. - float32 GetLengthB() const; + /// Get the current length of the segment attached to bodyB. + float32 GetLengthB() const; - /// Get the pulley ratio. - float32 GetRatio() const; + /// Get the pulley ratio. + float32 GetRatio() const; - /// Get the current length of the segment attached to bodyA. - float32 GetCurrentLengthA() const; + /// Get the current length of the segment attached to bodyA. + float32 GetCurrentLengthA() const; - /// Get the current length of the segment attached to bodyB. - float32 GetCurrentLengthB() const; + /// Get the current length of the segment attached to bodyB. + float32 GetCurrentLengthB() const; - /// Dump joint to dmLog - void Dump() override; + /// Dump joint to dmLog + void Dump() override; - /// Implement b2Joint::ShiftOrigin - void ShiftOrigin(const b2Vec2& newOrigin) override; + /// Implement b2Joint::ShiftOrigin + void ShiftOrigin(const b2Vec2 & newOrigin) override; protected: - - friend class b2Joint; - b2PulleyJoint(const b2PulleyJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - b2Vec2 m_groundAnchorA; - b2Vec2 m_groundAnchorB; - float32 m_lengthA; - float32 m_lengthB; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_constant; - float32 m_ratio; - float32 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_uA; - b2Vec2 m_uB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; + friend class b2Joint; + b2PulleyJoint(const b2PulleyJointDef * data); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + b2Vec2 m_groundAnchorA; + b2Vec2 m_groundAnchorB; + float32 m_lengthA; + float32 m_lengthB; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float32 m_constant; + float32 m_ratio; + float32 m_impulse; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_uA; + b2Vec2 m_uB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + float32 m_mass; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2RevoluteJoint.h index a349d7d5..59b46e11 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2RevoluteJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2RevoluteJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_REVOLUTE_JOINT_H #define B2_REVOLUTE_JOINT_H @@ -34,171 +34,167 @@ /// the joints will be broken. struct b2RevoluteJointDef : public b2JointDef { - b2RevoluteJointDef() - { - type = e_revoluteJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - referenceAngle = 0.0f; - lowerAngle = 0.0f; - upperAngle = 0.0f; - maxMotorTorque = 0.0f; - motorSpeed = 0.0f; - enableLimit = false; - enableMotor = false; - } - - /// Initialize the bodies, anchors, and reference angle using a world - /// anchor point. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The bodyB angle minus bodyA angle in the reference state (radians). - float32 referenceAngle; - - /// A flag to enable joint limits. - bool enableLimit; - - /// The lower angle for the joint limit (radians). - float32 lowerAngle; - - /// The upper angle for the joint limit (radians). - float32 upperAngle; - - /// A flag to enable the joint motor. - bool enableMotor; - - /// The desired motor speed. Usually in radians per second. - float32 motorSpeed; - - /// The maximum motor torque used to achieve the desired motor speed. - /// Usually in N-m. - float32 maxMotorTorque; + b2RevoluteJointDef() + { + type = e_revoluteJoint; + localAnchorA.Set(0.0f, 0.0f); + localAnchorB.Set(0.0f, 0.0f); + referenceAngle = 0.0f; + lowerAngle = 0.0f; + upperAngle = 0.0f; + maxMotorTorque = 0.0f; + motorSpeed = 0.0f; + enableLimit = false; + enableMotor = false; + } + + /// Initialize the bodies, anchors, and reference angle using a world + /// anchor point. + void Initialize(b2Body * bodyA, b2Body * bodyB, const b2Vec2 & anchor); + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The bodyB angle minus bodyA angle in the reference state (radians). + float32 referenceAngle; + + /// A flag to enable joint limits. + bool enableLimit; + + /// The lower angle for the joint limit (radians). + float32 lowerAngle; + + /// The upper angle for the joint limit (radians). + float32 upperAngle; + + /// A flag to enable the joint motor. + bool enableMotor; + + /// The desired motor speed. Usually in radians per second. + float32 motorSpeed; + + /// The maximum motor torque used to achieve the desired motor speed. + /// Usually in N-m. + float32 maxMotorTorque; }; /// A revolute joint constrains two bodies to share a common point while they /// are free to rotate about the point. The relative rotation about the shared /// point is the joint angle. You can limit the relative rotation with /// a joint limit that specifies a lower and upper angle. You can use a motor -/// to drive the relative rotation about the shared point. A maximum motor torque -/// is provided so that infinite forces are not generated. +/// to drive the relative rotation about the shared point. A maximum motor +/// torque is provided so that infinite forces are not generated. class b2RevoluteJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + /// The local anchor point relative to bodyA's origin. + const b2Vec2 & GetLocalAnchorA() const { return m_localAnchorA; } - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + /// The local anchor point relative to bodyB's origin. + const b2Vec2 & GetLocalAnchorB() const { return m_localAnchorB; } - /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } + /// Get the reference angle. + float32 GetReferenceAngle() const { return m_referenceAngle; } - /// Get the current joint angle in radians. - float32 GetJointAngle() const; + /// Get the current joint angle in radians. + float32 GetJointAngle() const; - /// Get the current joint angle speed in radians per second. - float32 GetJointSpeed() const; + /// Get the current joint angle speed in radians per second. + float32 GetJointSpeed() const; - /// Is the joint limit enabled? - bool IsLimitEnabled() const; + /// Is the joint limit enabled? + bool IsLimitEnabled() const; - /// Enable/disable the joint limit. - void EnableLimit(bool flag); + /// Enable/disable the joint limit. + void EnableLimit(bool flag); - /// Get the lower joint limit in radians. - float32 GetLowerLimit() const; + /// Get the lower joint limit in radians. + float32 GetLowerLimit() const; - /// Get the upper joint limit in radians. - float32 GetUpperLimit() const; + /// Get the upper joint limit in radians. + float32 GetUpperLimit() const; - /// Set the joint limits in radians. - void SetLimits(float32 lower, float32 upper); + /// Set the joint limits in radians. + void SetLimits(float32 lower, float32 upper); - /// Is the joint motor enabled? - bool IsMotorEnabled() const; + /// Is the joint motor enabled? + bool IsMotorEnabled() const; - /// Enable/disable the joint motor. - void EnableMotor(bool flag); + /// Enable/disable the joint motor. + void EnableMotor(bool flag); - /// Set the motor speed in radians per second. - void SetMotorSpeed(float32 speed); + /// Set the motor speed in radians per second. + void SetMotorSpeed(float32 speed); - /// Get the motor speed in radians per second. - float32 GetMotorSpeed() const; + /// Get the motor speed in radians per second. + float32 GetMotorSpeed() const; - /// Set the maximum motor torque, usually in N-m. - void SetMaxMotorTorque(float32 torque); - float32 GetMaxMotorTorque() const { return m_maxMotorTorque; } + /// Set the maximum motor torque, usually in N-m. + void SetMaxMotorTorque(float32 torque); + float32 GetMaxMotorTorque() const { return m_maxMotorTorque; } - /// Get the reaction force given the inverse time step. - /// Unit is N. - b2Vec2 GetReactionForce(float32 inv_dt) const override; + /// Get the reaction force given the inverse time step. + /// Unit is N. + b2Vec2 GetReactionForce(float32 inv_dt) const override; - /// Get the reaction torque due to the joint limit given the inverse time step. - /// Unit is N*m. - float32 GetReactionTorque(float32 inv_dt) const override; + /// Get the reaction torque due to the joint limit given the inverse time + /// step. Unit is N*m. + float32 GetReactionTorque(float32 inv_dt) const override; - /// Get the current motor torque given the inverse time step. - /// Unit is N*m. - float32 GetMotorTorque(float32 inv_dt) const; + /// Get the current motor torque given the inverse time step. + /// Unit is N*m. + float32 GetMotorTorque(float32 inv_dt) const; - /// Dump to b2Log. - void Dump() override; + /// Dump to b2Log. + void Dump() override; protected: - - friend class b2Joint; - friend class b2GearJoint; - - b2RevoluteJoint(const b2RevoluteJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec3 m_impulse; - float32 m_motorImpulse; - - bool m_enableMotor; - float32 m_maxMotorTorque; - float32 m_motorSpeed; - - bool m_enableLimit; - float32 m_referenceAngle; - float32 m_lowerAngle; - float32 m_upperAngle; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat33 m_mass; // effective mass for point-to-point constraint. - float32 m_motorMass; // effective mass for motor/limit angular constraint. - b2LimitState m_limitState; + friend class b2Joint; + friend class b2GearJoint; + + b2RevoluteJoint(const b2RevoluteJointDef * def); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec3 m_impulse; + float32 m_motorImpulse; + + bool m_enableMotor; + float32 m_maxMotorTorque; + float32 m_motorSpeed; + + bool m_enableLimit; + float32 m_referenceAngle; + float32 m_lowerAngle; + float32 m_upperAngle; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + b2Mat33 m_mass; // effective mass for point-to-point constraint. + float32 m_motorMass; // effective mass for motor/limit angular constraint. + b2LimitState m_limitState; }; -inline float32 b2RevoluteJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} +inline float32 b2RevoluteJoint::GetMotorSpeed() const { return m_motorSpeed; } #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2RopeJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2RopeJoint.h index 8f43f9d3..6aa5646c 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2RopeJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2RopeJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_ROPE_JOINT_H #define B2_ROPE_JOINT_H @@ -27,24 +27,24 @@ /// see collideConnected in b2JointDef. struct b2RopeJointDef : public b2JointDef { - b2RopeJointDef() - { - type = e_ropeJoint; - localAnchorA.Set(-1.0f, 0.0f); - localAnchorB.Set(1.0f, 0.0f); - maxLength = 0.0f; - } - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The maximum length of the rope. - /// Warning: this must be larger than b2_linearSlop or - /// the joint will have no effect. - float32 maxLength; + b2RopeJointDef() + { + type = e_ropeJoint; + localAnchorA.Set(-1.0f, 0.0f); + localAnchorB.Set(1.0f, 0.0f); + maxLength = 0.0f; + } + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The maximum length of the rope. + /// Warning: this must be larger than b2_linearSlop or + /// the joint will have no effect. + float32 maxLength; }; /// A rope joint enforces a maximum distance between two points @@ -58,57 +58,56 @@ struct b2RopeJointDef : public b2JointDef class b2RopeJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const override; - float32 GetReactionTorque(float32 inv_dt) const override; + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + /// The local anchor point relative to bodyA's origin. + const b2Vec2 & GetLocalAnchorA() const { return m_localAnchorA; } - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + /// The local anchor point relative to bodyB's origin. + const b2Vec2 & GetLocalAnchorB() const { return m_localAnchorB; } - /// Set/Get the maximum length of the rope. - void SetMaxLength(float32 length) { m_maxLength = length; } - float32 GetMaxLength() const; + /// Set/Get the maximum length of the rope. + void SetMaxLength(float32 length) { m_maxLength = length; } + float32 GetMaxLength() const; - b2LimitState GetLimitState() const; + b2LimitState GetLimitState() const; - /// Dump joint to dmLog - void Dump() override; + /// Dump joint to dmLog + void Dump() override; protected: - - friend class b2Joint; - b2RopeJoint(const b2RopeJointDef* data); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_maxLength; - float32 m_length; - float32 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_u; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - float32 m_mass; - b2LimitState m_state; + friend class b2Joint; + b2RopeJoint(const b2RopeJointDef * data); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float32 m_maxLength; + float32 m_length; + float32 m_impulse; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_u; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + float32 m_mass; + b2LimitState m_state; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2WeldJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2WeldJoint.h index 913385b6..765a94bf 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2WeldJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2WeldJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_WELD_JOINT_H #define B2_WELD_JOINT_H @@ -26,35 +26,35 @@ /// of the anchor points is important for computing the reaction torque. struct b2WeldJointDef : public b2JointDef { - b2WeldJointDef() - { - type = e_weldJoint; - localAnchorA.Set(0.0f, 0.0f); - localAnchorB.Set(0.0f, 0.0f); - referenceAngle = 0.0f; - frequencyHz = 0.0f; - dampingRatio = 0.0f; - } - - /// Initialize the bodies, anchors, and reference angle using a world - /// anchor point. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The bodyB angle minus bodyA angle in the reference state (radians). - float32 referenceAngle; - - /// The mass-spring-damper frequency in Hertz. Rotation only. - /// Disable softness with a value of 0. - float32 frequencyHz; - - /// The damping ratio. 0 = no damping, 1 = critical damping. - float32 dampingRatio; + b2WeldJointDef() + { + type = e_weldJoint; + localAnchorA.Set(0.0f, 0.0f); + localAnchorB.Set(0.0f, 0.0f); + referenceAngle = 0.0f; + frequencyHz = 0.0f; + dampingRatio = 0.0f; + } + + /// Initialize the bodies, anchors, and reference angle using a world + /// anchor point. + void Initialize(b2Body * bodyA, b2Body * bodyB, const b2Vec2 & anchor); + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The bodyB angle minus bodyA angle in the reference state (radians). + float32 referenceAngle; + + /// The mass-spring-damper frequency in Hertz. Rotation only. + /// Disable softness with a value of 0. + float32 frequencyHz; + + /// The damping ratio. 0 = no damping, 1 = critical damping. + float32 dampingRatio; }; /// A weld joint essentially glues two bodies together. A weld joint may @@ -62,65 +62,64 @@ struct b2WeldJointDef : public b2JointDef class b2WeldJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const override; - float32 GetReactionTorque(float32 inv_dt) const override; + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + /// The local anchor point relative to bodyA's origin. + const b2Vec2 & GetLocalAnchorA() const { return m_localAnchorA; } - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + /// The local anchor point relative to bodyB's origin. + const b2Vec2 & GetLocalAnchorB() const { return m_localAnchorB; } - /// Get the reference angle. - float32 GetReferenceAngle() const { return m_referenceAngle; } + /// Get the reference angle. + float32 GetReferenceAngle() const { return m_referenceAngle; } - /// Set/get frequency in Hz. - void SetFrequency(float32 hz) { m_frequencyHz = hz; } - float32 GetFrequency() const { return m_frequencyHz; } + /// Set/get frequency in Hz. + void SetFrequency(float32 hz) { m_frequencyHz = hz; } + float32 GetFrequency() const { return m_frequencyHz; } - /// Set/get damping ratio. - void SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; } - float32 GetDampingRatio() const { return m_dampingRatio; } + /// Set/get damping ratio. + void SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; } + float32 GetDampingRatio() const { return m_dampingRatio; } - /// Dump to b2Log - void Dump() override; + /// Dump to b2Log + void Dump() override; protected: - - friend class b2Joint; - - b2WeldJoint(const b2WeldJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - float32 m_frequencyHz; - float32 m_dampingRatio; - float32 m_bias; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - float32 m_referenceAngle; - float32 m_gamma; - b2Vec3 m_impulse; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_rA; - b2Vec2 m_rB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - b2Mat33 m_mass; + friend class b2Joint; + + b2WeldJoint(const b2WeldJointDef * def); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + float32 m_frequencyHz; + float32 m_dampingRatio; + float32 m_bias; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + float32 m_referenceAngle; + float32 m_gamma; + b2Vec3 m_impulse; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_rA; + b2Vec2 m_rB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + b2Mat33 m_mass; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/Joints/b2WheelJoint.h b/flatland_box2d/include/Box2D/Dynamics/Joints/b2WheelJoint.h index f5a43874..5c1412f4 100644 --- a/flatland_box2d/include/Box2D/Dynamics/Joints/b2WheelJoint.h +++ b/flatland_box2d/include/Box2D/Dynamics/Joints/b2WheelJoint.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_WHEEL_JOINT_H #define B2_WHEEL_JOINT_H @@ -29,188 +29,170 @@ /// anchors and a local axis helps when saving and loading a game. struct b2WheelJointDef : public b2JointDef { - b2WheelJointDef() - { - type = e_wheelJoint; - localAnchorA.SetZero(); - localAnchorB.SetZero(); - localAxisA.Set(1.0f, 0.0f); - enableMotor = false; - maxMotorTorque = 0.0f; - motorSpeed = 0.0f; - frequencyHz = 2.0f; - dampingRatio = 0.7f; - } - - /// Initialize the bodies, anchors, axis, and reference angle using the world - /// anchor and world axis. - void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis); - - /// The local anchor point relative to bodyA's origin. - b2Vec2 localAnchorA; - - /// The local anchor point relative to bodyB's origin. - b2Vec2 localAnchorB; - - /// The local translation axis in bodyA. - b2Vec2 localAxisA; - - /// Enable/disable the joint motor. - bool enableMotor; - - /// The maximum motor torque, usually in N-m. - float32 maxMotorTorque; - - /// The desired motor speed in radians per second. - float32 motorSpeed; - - /// Suspension frequency, zero indicates no suspension - float32 frequencyHz; - - /// Suspension damping ratio, one indicates critical damping - float32 dampingRatio; + b2WheelJointDef() + { + type = e_wheelJoint; + localAnchorA.SetZero(); + localAnchorB.SetZero(); + localAxisA.Set(1.0f, 0.0f); + enableMotor = false; + maxMotorTorque = 0.0f; + motorSpeed = 0.0f; + frequencyHz = 2.0f; + dampingRatio = 0.7f; + } + + /// Initialize the bodies, anchors, axis, and reference angle using the world + /// anchor and world axis. + void Initialize(b2Body * bodyA, b2Body * bodyB, const b2Vec2 & anchor, const b2Vec2 & axis); + + /// The local anchor point relative to bodyA's origin. + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin. + b2Vec2 localAnchorB; + + /// The local translation axis in bodyA. + b2Vec2 localAxisA; + + /// Enable/disable the joint motor. + bool enableMotor; + + /// The maximum motor torque, usually in N-m. + float32 maxMotorTorque; + + /// The desired motor speed in radians per second. + float32 motorSpeed; + + /// Suspension frequency, zero indicates no suspension + float32 frequencyHz; + + /// Suspension damping ratio, one indicates critical damping + float32 dampingRatio; }; /// A wheel joint. This joint provides two degrees of freedom: translation -/// along an axis fixed in bodyA and rotation in the plane. In other words, it is a point to -/// line constraint with a rotational motor and a linear spring/damper. -/// This joint is designed for vehicle suspensions. +/// along an axis fixed in bodyA and rotation in the plane. In other words, it +/// is a point to line constraint with a rotational motor and a linear +/// spring/damper. This joint is designed for vehicle suspensions. class b2WheelJoint : public b2Joint { public: - b2Vec2 GetAnchorA() const override; - b2Vec2 GetAnchorB() const override; + b2Vec2 GetAnchorA() const override; + b2Vec2 GetAnchorB() const override; - b2Vec2 GetReactionForce(float32 inv_dt) const override; - float32 GetReactionTorque(float32 inv_dt) const override; + b2Vec2 GetReactionForce(float32 inv_dt) const override; + float32 GetReactionTorque(float32 inv_dt) const override; - /// The local anchor point relative to bodyA's origin. - const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; } + /// The local anchor point relative to bodyA's origin. + const b2Vec2 & GetLocalAnchorA() const { return m_localAnchorA; } - /// The local anchor point relative to bodyB's origin. - const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; } + /// The local anchor point relative to bodyB's origin. + const b2Vec2 & GetLocalAnchorB() const { return m_localAnchorB; } - /// The local joint axis relative to bodyA. - const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; } + /// The local joint axis relative to bodyA. + const b2Vec2 & GetLocalAxisA() const { return m_localXAxisA; } - /// Get the current joint translation, usually in meters. - float32 GetJointTranslation() const; + /// Get the current joint translation, usually in meters. + float32 GetJointTranslation() const; - /// Get the current joint linear speed, usually in meters per second. - float32 GetJointLinearSpeed() const; + /// Get the current joint linear speed, usually in meters per second. + float32 GetJointLinearSpeed() const; - /// Get the current joint angle in radians. - float32 GetJointAngle() const; + /// Get the current joint angle in radians. + float32 GetJointAngle() const; - /// Get the current joint angular speed in radians per second. - float32 GetJointAngularSpeed() const; + /// Get the current joint angular speed in radians per second. + float32 GetJointAngularSpeed() const; - /// Is the joint motor enabled? - bool IsMotorEnabled() const; + /// Is the joint motor enabled? + bool IsMotorEnabled() const; - /// Enable/disable the joint motor. - void EnableMotor(bool flag); + /// Enable/disable the joint motor. + void EnableMotor(bool flag); - /// Set the motor speed, usually in radians per second. - void SetMotorSpeed(float32 speed); + /// Set the motor speed, usually in radians per second. + void SetMotorSpeed(float32 speed); - /// Get the motor speed, usually in radians per second. - float32 GetMotorSpeed() const; + /// Get the motor speed, usually in radians per second. + float32 GetMotorSpeed() const; - /// Set/Get the maximum motor force, usually in N-m. - void SetMaxMotorTorque(float32 torque); - float32 GetMaxMotorTorque() const; + /// Set/Get the maximum motor force, usually in N-m. + void SetMaxMotorTorque(float32 torque); + float32 GetMaxMotorTorque() const; - /// Get the current motor torque given the inverse time step, usually in N-m. - float32 GetMotorTorque(float32 inv_dt) const; + /// Get the current motor torque given the inverse time step, usually in N-m. + float32 GetMotorTorque(float32 inv_dt) const; - /// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring. - void SetSpringFrequencyHz(float32 hz); - float32 GetSpringFrequencyHz() const; + /// Set/Get the spring frequency in hertz. Setting the frequency to zero + /// disables the spring. + void SetSpringFrequencyHz(float32 hz); + float32 GetSpringFrequencyHz() const; - /// Set/Get the spring damping ratio - void SetSpringDampingRatio(float32 ratio); - float32 GetSpringDampingRatio() const; + /// Set/Get the spring damping ratio + void SetSpringDampingRatio(float32 ratio); + float32 GetSpringDampingRatio() const; - /// Dump to b2Log - void Dump() override; + /// Dump to b2Log + void Dump() override; protected: - - friend class b2Joint; - b2WheelJoint(const b2WheelJointDef* def); - - void InitVelocityConstraints(const b2SolverData& data) override; - void SolveVelocityConstraints(const b2SolverData& data) override; - bool SolvePositionConstraints(const b2SolverData& data) override; - - float32 m_frequencyHz; - float32 m_dampingRatio; - - // Solver shared - b2Vec2 m_localAnchorA; - b2Vec2 m_localAnchorB; - b2Vec2 m_localXAxisA; - b2Vec2 m_localYAxisA; - - float32 m_impulse; - float32 m_motorImpulse; - float32 m_springImpulse; - - float32 m_maxMotorTorque; - float32 m_motorSpeed; - bool m_enableMotor; - - // Solver temp - int32 m_indexA; - int32 m_indexB; - b2Vec2 m_localCenterA; - b2Vec2 m_localCenterB; - float32 m_invMassA; - float32 m_invMassB; - float32 m_invIA; - float32 m_invIB; - - b2Vec2 m_ax, m_ay; - float32 m_sAx, m_sBx; - float32 m_sAy, m_sBy; - - float32 m_mass; - float32 m_motorMass; - float32 m_springMass; - - float32 m_bias; - float32 m_gamma; + friend class b2Joint; + b2WheelJoint(const b2WheelJointDef * def); + + void InitVelocityConstraints(const b2SolverData & data) override; + void SolveVelocityConstraints(const b2SolverData & data) override; + bool SolvePositionConstraints(const b2SolverData & data) override; + + float32 m_frequencyHz; + float32 m_dampingRatio; + + // Solver shared + b2Vec2 m_localAnchorA; + b2Vec2 m_localAnchorB; + b2Vec2 m_localXAxisA; + b2Vec2 m_localYAxisA; + + float32 m_impulse; + float32 m_motorImpulse; + float32 m_springImpulse; + + float32 m_maxMotorTorque; + float32 m_motorSpeed; + bool m_enableMotor; + + // Solver temp + int32 m_indexA; + int32 m_indexB; + b2Vec2 m_localCenterA; + b2Vec2 m_localCenterB; + float32 m_invMassA; + float32 m_invMassB; + float32 m_invIA; + float32 m_invIB; + + b2Vec2 m_ax, m_ay; + float32 m_sAx, m_sBx; + float32 m_sAy, m_sBy; + + float32 m_mass; + float32 m_motorMass; + float32 m_springMass; + + float32 m_bias; + float32 m_gamma; }; -inline float32 b2WheelJoint::GetMotorSpeed() const -{ - return m_motorSpeed; -} +inline float32 b2WheelJoint::GetMotorSpeed() const { return m_motorSpeed; } -inline float32 b2WheelJoint::GetMaxMotorTorque() const -{ - return m_maxMotorTorque; -} +inline float32 b2WheelJoint::GetMaxMotorTorque() const { return m_maxMotorTorque; } -inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz) -{ - m_frequencyHz = hz; -} +inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz) { m_frequencyHz = hz; } -inline float32 b2WheelJoint::GetSpringFrequencyHz() const -{ - return m_frequencyHz; -} +inline float32 b2WheelJoint::GetSpringFrequencyHz() const { return m_frequencyHz; } -inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio) -{ - m_dampingRatio = ratio; -} +inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio) { m_dampingRatio = ratio; } -inline float32 b2WheelJoint::GetSpringDampingRatio() const -{ - return m_dampingRatio; -} +inline float32 b2WheelJoint::GetSpringDampingRatio() const { return m_dampingRatio; } #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/b2Body.h b/flatland_box2d/include/Box2D/Dynamics/b2Body.h index 0e302638..cc812891 100644 --- a/flatland_box2d/include/Box2D/Dynamics/b2Body.h +++ b/flatland_box2d/include/Box2D/Dynamics/b2Body.h @@ -1,28 +1,29 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_BODY_H #define B2_BODY_H -#include "Box2D/Common/b2Math.h" -#include "Box2D/Collision/Shapes/b2Shape.h" #include +#include "Box2D/Collision/Shapes/b2Shape.h" +#include "Box2D/Common/b2Math.h" + class b2Fixture; class b2Joint; class b2Contact; @@ -35,848 +36,732 @@ struct b2ContactEdge; /// The body type. /// static: zero mass, zero velocity, may be manually moved /// kinematic: zero mass, non-zero velocity set by user, moved by solver -/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver -enum b2BodyType -{ - b2_staticBody = 0, - b2_kinematicBody, - b2_dynamicBody - - // TODO_ERIN - //b2_bulletBody, +/// dynamic: positive mass, non-zero velocity determined by forces, moved by +/// solver +enum b2BodyType { + b2_staticBody = 0, + b2_kinematicBody, + b2_dynamicBody + + // TODO_ERIN + // b2_bulletBody, }; /// A body definition holds all the data needed to construct a rigid body. -/// You can safely re-use body definitions. Shapes are added to a body after construction. +/// You can safely re-use body definitions. Shapes are added to a body after +/// construction. struct b2BodyDef { - /// This constructor sets the body definition default values. - b2BodyDef() - { - userData = nullptr; - position.Set(0.0f, 0.0f); - angle = 0.0f; - linearVelocity.Set(0.0f, 0.0f); - angularVelocity = 0.0f; - linearDamping = 0.0f; - angularDamping = 0.0f; - allowSleep = true; - awake = true; - fixedRotation = false; - bullet = false; - type = b2_staticBody; - active = true; - gravityScale = 1.0f; - } - - /// The body type: static, kinematic, or dynamic. - /// Note: if a dynamic body would have zero mass, the mass is set to one. - b2BodyType type; - - /// The world position of the body. Avoid creating bodies at the origin - /// since this can lead to many overlapping shapes. - b2Vec2 position; - - /// The world angle of the body in radians. - float32 angle; - - /// The linear velocity of the body's origin in world co-ordinates. - b2Vec2 linearVelocity; - - /// The angular velocity of the body. - float32 angularVelocity; - - /// Linear damping is use to reduce the linear velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - /// Units are 1/time - float32 linearDamping; - - /// Angular damping is use to reduce the angular velocity. The damping parameter - /// can be larger than 1.0f but the damping effect becomes sensitive to the - /// time step when the damping parameter is large. - /// Units are 1/time - float32 angularDamping; - - /// Set this flag to false if this body should never fall asleep. Note that - /// this increases CPU usage. - bool allowSleep; - - /// Is this body initially awake or sleeping? - bool awake; - - /// Should this body be prevented from rotating? Useful for characters. - bool fixedRotation; - - /// Is this a fast moving body that should be prevented from tunneling through - /// other moving bodies? Note that all bodies are prevented from tunneling through - /// kinematic and static bodies. This setting is only considered on dynamic bodies. - /// @warning You should use this flag sparingly since it increases processing time. - bool bullet; - - /// Does this body start out active? - bool active; - - /// Use this to store application specific body data. - void* userData; - - /// Scale the gravity applied to this body. - float32 gravityScale; + /// This constructor sets the body definition default values. + b2BodyDef() + { + userData = nullptr; + position.Set(0.0f, 0.0f); + angle = 0.0f; + linearVelocity.Set(0.0f, 0.0f); + angularVelocity = 0.0f; + linearDamping = 0.0f; + angularDamping = 0.0f; + allowSleep = true; + awake = true; + fixedRotation = false; + bullet = false; + type = b2_staticBody; + active = true; + gravityScale = 1.0f; + } + + /// The body type: static, kinematic, or dynamic. + /// Note: if a dynamic body would have zero mass, the mass is set to one. + b2BodyType type; + + /// The world position of the body. Avoid creating bodies at the origin + /// since this can lead to many overlapping shapes. + b2Vec2 position; + + /// The world angle of the body in radians. + float32 angle; + + /// The linear velocity of the body's origin in world co-ordinates. + b2Vec2 linearVelocity; + + /// The angular velocity of the body. + float32 angularVelocity; + + /// Linear damping is use to reduce the linear velocity. The damping parameter + /// can be larger than 1.0f but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// Units are 1/time + float32 linearDamping; + + /// Angular damping is use to reduce the angular velocity. The damping + /// parameter can be larger than 1.0f but the damping effect becomes sensitive + /// to the time step when the damping parameter is large. Units are 1/time + float32 angularDamping; + + /// Set this flag to false if this body should never fall asleep. Note that + /// this increases CPU usage. + bool allowSleep; + + /// Is this body initially awake or sleeping? + bool awake; + + /// Should this body be prevented from rotating? Useful for characters. + bool fixedRotation; + + /// Is this a fast moving body that should be prevented from tunneling through + /// other moving bodies? Note that all bodies are prevented from tunneling + /// through kinematic and static bodies. This setting is only considered on + /// dynamic bodies. + /// @warning You should use this flag sparingly since it increases processing + /// time. + bool bullet; + + /// Does this body start out active? + bool active; + + /// Use this to store application specific body data. + void * userData; + + /// Scale the gravity applied to this body. + float32 gravityScale; }; /// A rigid body. These are created via b2World::CreateBody. class b2Body { public: - /// Creates a fixture and attach it to this body. Use this function if you need - /// to set some fixture parameters, like friction. Otherwise you can create the - /// fixture directly from a shape. - /// If the density is non-zero, this function automatically updates the mass of the body. - /// Contacts are not created until the next time step. - /// @param def the fixture definition. - /// @warning This function is locked during callbacks. - b2Fixture* CreateFixture(const b2FixtureDef* def); - - /// Creates a fixture from a shape and attach it to this body. - /// This is a convenience function. Use b2FixtureDef if you need to set parameters - /// like friction, restitution, user data, or filtering. - /// If the density is non-zero, this function automatically updates the mass of the body. - /// @param shape the shape to be cloned. - /// @param density the shape density (set to zero for static bodies). - /// @warning This function is locked during callbacks. - b2Fixture* CreateFixture(const b2Shape* shape, float32 density); - - /// Destroy a fixture. This removes the fixture from the broad-phase and - /// destroys all contacts associated with this fixture. This will - /// automatically adjust the mass of the body if the body is dynamic and the - /// fixture has positive density. - /// All fixtures attached to a body are implicitly destroyed when the body is destroyed. - /// @param fixture the fixture to be removed. - /// @warning This function is locked during callbacks. - void DestroyFixture(b2Fixture* fixture); - - /// Set the position of the body's origin and rotation. - /// Manipulating a body's transform may cause non-physical behavior. - /// Note: contacts are updated on the next call to b2World::Step. - /// @param position the world position of the body's local origin. - /// @param angle the world rotation in radians. - void SetTransform(const b2Vec2& position, float32 angle); - - /// Get the body transform for the body's origin. - /// @return the world transform of the body's origin. - const b2Transform& GetTransform() const; - - /// Get the world body origin position. - /// @return the world position of the body's origin. - const b2Vec2& GetPosition() const; - - /// Get the angle in radians. - /// @return the current world rotation angle in radians. - float32 GetAngle() const; - - /// Get the world position of the center of mass. - const b2Vec2& GetWorldCenter() const; - - /// Get the local position of the center of mass. - const b2Vec2& GetLocalCenter() const; - - /// Set the linear velocity of the center of mass. - /// @param v the new linear velocity of the center of mass. - void SetLinearVelocity(const b2Vec2& v); - - /// Get the linear velocity of the center of mass. - /// @return the linear velocity of the center of mass. - const b2Vec2& GetLinearVelocity() const; - - /// Set the angular velocity. - /// @param omega the new angular velocity in radians/second. - void SetAngularVelocity(float32 omega); - - /// Get the angular velocity. - /// @return the angular velocity in radians/second. - float32 GetAngularVelocity() const; - - /// Apply a force at a world point. If the force is not - /// applied at the center of mass, it will generate a torque and - /// affect the angular velocity. This wakes up the body. - /// @param force the world force vector, usually in Newtons (N). - /// @param point the world position of the point of application. - /// @param wake also wake up the body - void ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake); - - /// Apply a force to the center of mass. This wakes up the body. - /// @param force the world force vector, usually in Newtons (N). - /// @param wake also wake up the body - void ApplyForceToCenter(const b2Vec2& force, bool wake); - - /// Apply a torque. This affects the angular velocity - /// without affecting the linear velocity of the center of mass. - /// @param torque about the z-axis (out of the screen), usually in N-m. - /// @param wake also wake up the body - void ApplyTorque(float32 torque, bool wake); - - /// Apply an impulse at a point. This immediately modifies the velocity. - /// It also modifies the angular velocity if the point of application - /// is not at the center of mass. This wakes up the body. - /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. - /// @param point the world position of the point of application. - /// @param wake also wake up the body - void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake); - - /// Apply an impulse to the center of mass. This immediately modifies the velocity. - /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. - /// @param wake also wake up the body - void ApplyLinearImpulseToCenter(const b2Vec2& impulse, bool wake); - - /// Apply an angular impulse. - /// @param impulse the angular impulse in units of kg*m*m/s - /// @param wake also wake up the body - void ApplyAngularImpulse(float32 impulse, bool wake); - - /// Get the total mass of the body. - /// @return the mass, usually in kilograms (kg). - float32 GetMass() const; - - /// Get the rotational inertia of the body about the local origin. - /// @return the rotational inertia, usually in kg-m^2. - float32 GetInertia() const; - - /// Get the mass data of the body. - /// @return a struct containing the mass, inertia and center of the body. - void GetMassData(b2MassData* data) const; - - /// Set the mass properties to override the mass properties of the fixtures. - /// Note that this changes the center of mass position. - /// Note that creating or destroying fixtures can also alter the mass. - /// This function has no effect if the body isn't dynamic. - /// @param massData the mass properties. - void SetMassData(const b2MassData* data); - - /// This resets the mass properties to the sum of the mass properties of the fixtures. - /// This normally does not need to be called unless you called SetMassData to override - /// the mass and you later want to reset the mass. - void ResetMassData(); - - /// Get the world coordinates of a point given the local coordinates. - /// @param localPoint a point on the body measured relative the the body's origin. - /// @return the same point expressed in world coordinates. - b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const; - - /// Get the world coordinates of a vector given the local coordinates. - /// @param localVector a vector fixed in the body. - /// @return the same vector expressed in world coordinates. - b2Vec2 GetWorldVector(const b2Vec2& localVector) const; - - /// Gets a local point relative to the body's origin given a world point. - /// @param a point in world coordinates. - /// @return the corresponding local point relative to the body's origin. - b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const; - - /// Gets a local vector given a world vector. - /// @param a vector in world coordinates. - /// @return the corresponding local vector. - b2Vec2 GetLocalVector(const b2Vec2& worldVector) const; - - /// Get the world linear velocity of a world point attached to this body. - /// @param a point in world coordinates. - /// @return the world velocity of a point. - b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const; - - /// Get the world velocity of a local point. - /// @param a point in local coordinates. - /// @return the world velocity of a point. - b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const; - - /// Get the linear damping of the body. - float32 GetLinearDamping() const; - - /// Set the linear damping of the body. - void SetLinearDamping(float32 linearDamping); - - /// Get the angular damping of the body. - float32 GetAngularDamping() const; - - /// Set the angular damping of the body. - void SetAngularDamping(float32 angularDamping); - - /// Get the gravity scale of the body. - float32 GetGravityScale() const; - - /// Set the gravity scale of the body. - void SetGravityScale(float32 scale); - - /// Set the type of this body. This may alter the mass and velocity. - void SetType(b2BodyType type); - - /// Get the type of this body. - b2BodyType GetType() const; - - /// Should this body be treated like a bullet for continuous collision detection? - void SetBullet(bool flag); - - /// Is this body treated like a bullet for continuous collision detection? - bool IsBullet() const; - - /// You can disable sleeping on this body. If you disable sleeping, the - /// body will be woken. - void SetSleepingAllowed(bool flag); - - /// Is this body allowed to sleep - bool IsSleepingAllowed() const; - - /// Set the sleep state of the body. A sleeping body has very - /// low CPU cost. - /// @param flag set to true to wake the body, false to put it to sleep. - void SetAwake(bool flag); - - /// Get the sleeping state of this body. - /// @return true if the body is awake. - bool IsAwake() const; - - /// Set the active state of the body. An inactive body is not - /// simulated and cannot be collided with or woken up. - /// If you pass a flag of true, all fixtures will be added to the - /// broad-phase. - /// If you pass a flag of false, all fixtures will be removed from - /// the broad-phase and all contacts will be destroyed. - /// Fixtures and joints are otherwise unaffected. You may continue - /// to create/destroy fixtures and joints on inactive bodies. - /// Fixtures on an inactive body are implicitly inactive and will - /// not participate in collisions, ray-casts, or queries. - /// Joints connected to an inactive body are implicitly inactive. - /// An inactive body is still owned by a b2World object and remains - /// in the body list. - void SetActive(bool flag); - - /// Get the active state of the body. - bool IsActive() const; - - /// Set this body to have fixed rotation. This causes the mass - /// to be reset. - void SetFixedRotation(bool flag); - - /// Does this body have fixed rotation? - bool IsFixedRotation() const; - - /// Get the list of all fixtures attached to this body. - b2Fixture* GetFixtureList(); - const b2Fixture* GetFixtureList() const; + /// Creates a fixture and attach it to this body. Use this function if you + /// need to set some fixture parameters, like friction. Otherwise you can + /// create the fixture directly from a shape. If the density is non-zero, this + /// function automatically updates the mass of the body. Contacts are not + /// created until the next time step. + /// @param def the fixture definition. + /// @warning This function is locked during callbacks. + b2Fixture * CreateFixture(const b2FixtureDef * def); + + /// Creates a fixture from a shape and attach it to this body. + /// This is a convenience function. Use b2FixtureDef if you need to set + /// parameters like friction, restitution, user data, or filtering. If the + /// density is non-zero, this function automatically updates the mass of the + /// body. + /// @param shape the shape to be cloned. + /// @param density the shape density (set to zero for static bodies). + /// @warning This function is locked during callbacks. + b2Fixture * CreateFixture(const b2Shape * shape, float32 density); + + /// Destroy a fixture. This removes the fixture from the broad-phase and + /// destroys all contacts associated with this fixture. This will + /// automatically adjust the mass of the body if the body is dynamic and the + /// fixture has positive density. + /// All fixtures attached to a body are implicitly destroyed when the body is + /// destroyed. + /// @param fixture the fixture to be removed. + /// @warning This function is locked during callbacks. + void DestroyFixture(b2Fixture * fixture); + + /// Set the position of the body's origin and rotation. + /// Manipulating a body's transform may cause non-physical behavior. + /// Note: contacts are updated on the next call to b2World::Step. + /// @param position the world position of the body's local origin. + /// @param angle the world rotation in radians. + void SetTransform(const b2Vec2 & position, float32 angle); + + /// Get the body transform for the body's origin. + /// @return the world transform of the body's origin. + const b2Transform & GetTransform() const; + + /// Get the world body origin position. + /// @return the world position of the body's origin. + const b2Vec2 & GetPosition() const; + + /// Get the angle in radians. + /// @return the current world rotation angle in radians. + float32 GetAngle() const; + + /// Get the world position of the center of mass. + const b2Vec2 & GetWorldCenter() const; + + /// Get the local position of the center of mass. + const b2Vec2 & GetLocalCenter() const; + + /// Set the linear velocity of the center of mass. + /// @param v the new linear velocity of the center of mass. + void SetLinearVelocity(const b2Vec2 & v); + + /// Get the linear velocity of the center of mass. + /// @return the linear velocity of the center of mass. + const b2Vec2 & GetLinearVelocity() const; + + /// Set the angular velocity. + /// @param omega the new angular velocity in radians/second. + void SetAngularVelocity(float32 omega); + + /// Get the angular velocity. + /// @return the angular velocity in radians/second. + float32 GetAngularVelocity() const; + + /// Apply a force at a world point. If the force is not + /// applied at the center of mass, it will generate a torque and + /// affect the angular velocity. This wakes up the body. + /// @param force the world force vector, usually in Newtons (N). + /// @param point the world position of the point of application. + /// @param wake also wake up the body + void ApplyForce(const b2Vec2 & force, const b2Vec2 & point, bool wake); + + /// Apply a force to the center of mass. This wakes up the body. + /// @param force the world force vector, usually in Newtons (N). + /// @param wake also wake up the body + void ApplyForceToCenter(const b2Vec2 & force, bool wake); + + /// Apply a torque. This affects the angular velocity + /// without affecting the linear velocity of the center of mass. + /// @param torque about the z-axis (out of the screen), usually in N-m. + /// @param wake also wake up the body + void ApplyTorque(float32 torque, bool wake); + + /// Apply an impulse at a point. This immediately modifies the velocity. + /// It also modifies the angular velocity if the point of application + /// is not at the center of mass. This wakes up the body. + /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. + /// @param point the world position of the point of application. + /// @param wake also wake up the body + void ApplyLinearImpulse(const b2Vec2 & impulse, const b2Vec2 & point, bool wake); + + /// Apply an impulse to the center of mass. This immediately modifies the + /// velocity. + /// @param impulse the world impulse vector, usually in N-seconds or kg-m/s. + /// @param wake also wake up the body + void ApplyLinearImpulseToCenter(const b2Vec2 & impulse, bool wake); + + /// Apply an angular impulse. + /// @param impulse the angular impulse in units of kg*m*m/s + /// @param wake also wake up the body + void ApplyAngularImpulse(float32 impulse, bool wake); + + /// Get the total mass of the body. + /// @return the mass, usually in kilograms (kg). + float32 GetMass() const; + + /// Get the rotational inertia of the body about the local origin. + /// @return the rotational inertia, usually in kg-m^2. + float32 GetInertia() const; + + /// Get the mass data of the body. + /// @return a struct containing the mass, inertia and center of the body. + void GetMassData(b2MassData * data) const; + + /// Set the mass properties to override the mass properties of the fixtures. + /// Note that this changes the center of mass position. + /// Note that creating or destroying fixtures can also alter the mass. + /// This function has no effect if the body isn't dynamic. + /// @param massData the mass properties. + void SetMassData(const b2MassData * data); + + /// This resets the mass properties to the sum of the mass properties of the + /// fixtures. This normally does not need to be called unless you called + /// SetMassData to override the mass and you later want to reset the mass. + void ResetMassData(); + + /// Get the world coordinates of a point given the local coordinates. + /// @param localPoint a point on the body measured relative the the body's + /// origin. + /// @return the same point expressed in world coordinates. + b2Vec2 GetWorldPoint(const b2Vec2 & localPoint) const; + + /// Get the world coordinates of a vector given the local coordinates. + /// @param localVector a vector fixed in the body. + /// @return the same vector expressed in world coordinates. + b2Vec2 GetWorldVector(const b2Vec2 & localVector) const; + + /// Gets a local point relative to the body's origin given a world point. + /// @param a point in world coordinates. + /// @return the corresponding local point relative to the body's origin. + b2Vec2 GetLocalPoint(const b2Vec2 & worldPoint) const; + + /// Gets a local vector given a world vector. + /// @param a vector in world coordinates. + /// @return the corresponding local vector. + b2Vec2 GetLocalVector(const b2Vec2 & worldVector) const; + + /// Get the world linear velocity of a world point attached to this body. + /// @param a point in world coordinates. + /// @return the world velocity of a point. + b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2 & worldPoint) const; + + /// Get the world velocity of a local point. + /// @param a point in local coordinates. + /// @return the world velocity of a point. + b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2 & localPoint) const; + + /// Get the linear damping of the body. + float32 GetLinearDamping() const; + + /// Set the linear damping of the body. + void SetLinearDamping(float32 linearDamping); + + /// Get the angular damping of the body. + float32 GetAngularDamping() const; + + /// Set the angular damping of the body. + void SetAngularDamping(float32 angularDamping); + + /// Get the gravity scale of the body. + float32 GetGravityScale() const; + + /// Set the gravity scale of the body. + void SetGravityScale(float32 scale); + + /// Set the type of this body. This may alter the mass and velocity. + void SetType(b2BodyType type); + + /// Get the type of this body. + b2BodyType GetType() const; + + /// Should this body be treated like a bullet for continuous collision + /// detection? + void SetBullet(bool flag); + + /// Is this body treated like a bullet for continuous collision detection? + bool IsBullet() const; + + /// You can disable sleeping on this body. If you disable sleeping, the + /// body will be woken. + void SetSleepingAllowed(bool flag); + + /// Is this body allowed to sleep + bool IsSleepingAllowed() const; + + /// Set the sleep state of the body. A sleeping body has very + /// low CPU cost. + /// @param flag set to true to wake the body, false to put it to sleep. + void SetAwake(bool flag); + + /// Get the sleeping state of this body. + /// @return true if the body is awake. + bool IsAwake() const; + + /// Set the active state of the body. An inactive body is not + /// simulated and cannot be collided with or woken up. + /// If you pass a flag of true, all fixtures will be added to the + /// broad-phase. + /// If you pass a flag of false, all fixtures will be removed from + /// the broad-phase and all contacts will be destroyed. + /// Fixtures and joints are otherwise unaffected. You may continue + /// to create/destroy fixtures and joints on inactive bodies. + /// Fixtures on an inactive body are implicitly inactive and will + /// not participate in collisions, ray-casts, or queries. + /// Joints connected to an inactive body are implicitly inactive. + /// An inactive body is still owned by a b2World object and remains + /// in the body list. + void SetActive(bool flag); + + /// Get the active state of the body. + bool IsActive() const; + + /// Set this body to have fixed rotation. This causes the mass + /// to be reset. + void SetFixedRotation(bool flag); - /// Get the list of all joints attached to this body. - b2JointEdge* GetJointList(); - const b2JointEdge* GetJointList() const; + /// Does this body have fixed rotation? + bool IsFixedRotation() const; + + /// Get the list of all fixtures attached to this body. + b2Fixture * GetFixtureList(); + const b2Fixture * GetFixtureList() const; + + /// Get the list of all joints attached to this body. + b2JointEdge * GetJointList(); + const b2JointEdge * GetJointList() const; - /// Get the list of all contacts attached to this body. - /// @warning this list changes during the time step and you may - /// miss some collisions if you don't use b2ContactListener. - b2ContactEdge* GetContactList(); - const b2ContactEdge* GetContactList() const; + /// Get the list of all contacts attached to this body. + /// @warning this list changes during the time step and you may + /// miss some collisions if you don't use b2ContactListener. + b2ContactEdge * GetContactList(); + const b2ContactEdge * GetContactList() const; - /// Get the next body in the world's body list. - b2Body* GetNext(); - const b2Body* GetNext() const; + /// Get the next body in the world's body list. + b2Body * GetNext(); + const b2Body * GetNext() const; - /// Get the user data pointer that was provided in the body definition. - void* GetUserData() const; + /// Get the user data pointer that was provided in the body definition. + void * GetUserData() const; - /// Set the user data. Use this to store your application specific data. - void SetUserData(void* data); + /// Set the user data. Use this to store your application specific data. + void SetUserData(void * data); - /// Get the parent world of this body. - b2World* GetWorld(); - const b2World* GetWorld() const; + /// Get the parent world of this body. + b2World * GetWorld(); + const b2World * GetWorld() const; - /// Dump this body to a log file - void Dump(); + /// Dump this body to a log file + void Dump(); private: + friend class b2World; + friend class b2Island; + friend class b2ContactManager; + friend class b2ContactSolver; + friend class b2Contact; - friend class b2World; - friend class b2Island; - friend class b2ContactManager; - friend class b2ContactSolver; - friend class b2Contact; - - friend class b2DistanceJoint; - friend class b2FrictionJoint; - friend class b2GearJoint; - friend class b2MotorJoint; - friend class b2MouseJoint; - friend class b2PrismaticJoint; - friend class b2PulleyJoint; - friend class b2RevoluteJoint; - friend class b2RopeJoint; - friend class b2WeldJoint; - friend class b2WheelJoint; + friend class b2DistanceJoint; + friend class b2FrictionJoint; + friend class b2GearJoint; + friend class b2MotorJoint; + friend class b2MouseJoint; + friend class b2PrismaticJoint; + friend class b2PulleyJoint; + friend class b2RevoluteJoint; + friend class b2RopeJoint; + friend class b2WeldJoint; + friend class b2WheelJoint; - // m_flags - enum - { - e_islandFlag = 0x0001, - e_awakeFlag = 0x0002, - e_autoSleepFlag = 0x0004, - e_bulletFlag = 0x0008, - e_fixedRotationFlag = 0x0010, - e_activeFlag = 0x0020, - e_toiFlag = 0x0040 - }; + // m_flags + enum { + e_islandFlag = 0x0001, + e_awakeFlag = 0x0002, + e_autoSleepFlag = 0x0004, + e_bulletFlag = 0x0008, + e_fixedRotationFlag = 0x0010, + e_activeFlag = 0x0020, + e_toiFlag = 0x0040 + }; - b2Body(const b2BodyDef* bd, b2World* world); - ~b2Body(); + b2Body(const b2BodyDef * bd, b2World * world); + ~b2Body(); - void SynchronizeFixtures(); - void SynchronizeTransform(); + void SynchronizeFixtures(); + void SynchronizeTransform(); - // This is used to prevent connected bodies from colliding. - // It may lie, depending on the collideConnected flag. - bool ShouldCollide(const b2Body* other) const; + // This is used to prevent connected bodies from colliding. + // It may lie, depending on the collideConnected flag. + bool ShouldCollide(const b2Body * other) const; - void Advance(float32 t); + void Advance(float32 t); - b2BodyType m_type; + b2BodyType m_type; - uint16 m_flags; + uint16 m_flags; - int32 m_islandIndex; + int32 m_islandIndex; - b2Transform m_xf; // the body origin transform - b2Sweep m_sweep; // the swept motion for CCD + b2Transform m_xf; // the body origin transform + b2Sweep m_sweep; // the swept motion for CCD - b2Vec2 m_linearVelocity; - float32 m_angularVelocity; + b2Vec2 m_linearVelocity; + float32 m_angularVelocity; - b2Vec2 m_force; - float32 m_torque; + b2Vec2 m_force; + float32 m_torque; - b2World* m_world; - b2Body* m_prev; - b2Body* m_next; + b2World * m_world; + b2Body * m_prev; + b2Body * m_next; - b2Fixture* m_fixtureList; - int32 m_fixtureCount; + b2Fixture * m_fixtureList; + int32 m_fixtureCount; - b2JointEdge* m_jointList; - b2ContactEdge* m_contactList; + b2JointEdge * m_jointList; + b2ContactEdge * m_contactList; - float32 m_mass, m_invMass; + float32 m_mass, m_invMass; - // Rotational inertia about the center of mass. - float32 m_I, m_invI; + // Rotational inertia about the center of mass. + float32 m_I, m_invI; - float32 m_linearDamping; - float32 m_angularDamping; - float32 m_gravityScale; + float32 m_linearDamping; + float32 m_angularDamping; + float32 m_gravityScale; - float32 m_sleepTime; + float32 m_sleepTime; - void* m_userData; + void * m_userData; }; -inline b2BodyType b2Body::GetType() const -{ - return m_type; -} +inline b2BodyType b2Body::GetType() const { return m_type; } -inline const b2Transform& b2Body::GetTransform() const -{ - return m_xf; -} +inline const b2Transform & b2Body::GetTransform() const { return m_xf; } -inline const b2Vec2& b2Body::GetPosition() const -{ - return m_xf.p; -} +inline const b2Vec2 & b2Body::GetPosition() const { return m_xf.p; } -inline float32 b2Body::GetAngle() const -{ - return m_sweep.a; -} +inline float32 b2Body::GetAngle() const { return m_sweep.a; } -inline const b2Vec2& b2Body::GetWorldCenter() const -{ - return m_sweep.c; -} +inline const b2Vec2 & b2Body::GetWorldCenter() const { return m_sweep.c; } -inline const b2Vec2& b2Body::GetLocalCenter() const -{ - return m_sweep.localCenter; -} +inline const b2Vec2 & b2Body::GetLocalCenter() const { return m_sweep.localCenter; } -inline void b2Body::SetLinearVelocity(const b2Vec2& v) +inline void b2Body::SetLinearVelocity(const b2Vec2 & v) { - if (m_type == b2_staticBody) - { - return; - } + if (m_type == b2_staticBody) { + return; + } - if (b2Dot(v,v) > 0.0f) - { - SetAwake(true); - } + if (b2Dot(v, v) > 0.0f) { + SetAwake(true); + } - m_linearVelocity = v; + m_linearVelocity = v; } -inline const b2Vec2& b2Body::GetLinearVelocity() const -{ - return m_linearVelocity; -} +inline const b2Vec2 & b2Body::GetLinearVelocity() const { return m_linearVelocity; } inline void b2Body::SetAngularVelocity(float32 w) { - if (m_type == b2_staticBody) - { - return; - } + if (m_type == b2_staticBody) { + return; + } - if (w * w > 0.0f) - { - SetAwake(true); - } + if (w * w > 0.0f) { + SetAwake(true); + } - m_angularVelocity = w; + m_angularVelocity = w; } -inline float32 b2Body::GetAngularVelocity() const -{ - return m_angularVelocity; -} +inline float32 b2Body::GetAngularVelocity() const { return m_angularVelocity; } -inline float32 b2Body::GetMass() const -{ - return m_mass; -} +inline float32 b2Body::GetMass() const { return m_mass; } inline float32 b2Body::GetInertia() const { - return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); + return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); } -inline void b2Body::GetMassData(b2MassData* data) const +inline void b2Body::GetMassData(b2MassData * data) const { - data->mass = m_mass; - data->I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); - data->center = m_sweep.localCenter; + data->mass = m_mass; + data->I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter); + data->center = m_sweep.localCenter; } -inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const +inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2 & localPoint) const { - return b2Mul(m_xf, localPoint); + return b2Mul(m_xf, localPoint); } -inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const +inline b2Vec2 b2Body::GetWorldVector(const b2Vec2 & localVector) const { - return b2Mul(m_xf.q, localVector); + return b2Mul(m_xf.q, localVector); } -inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const +inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2 & worldPoint) const { - return b2MulT(m_xf, worldPoint); + return b2MulT(m_xf, worldPoint); } -inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const +inline b2Vec2 b2Body::GetLocalVector(const b2Vec2 & worldVector) const { - return b2MulT(m_xf.q, worldVector); + return b2MulT(m_xf.q, worldVector); } -inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const +inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2 & worldPoint) const { - return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c); + return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c); } -inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const +inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2 & localPoint) const { - return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint)); + return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint)); } -inline float32 b2Body::GetLinearDamping() const -{ - return m_linearDamping; -} +inline float32 b2Body::GetLinearDamping() const { return m_linearDamping; } -inline void b2Body::SetLinearDamping(float32 linearDamping) -{ - m_linearDamping = linearDamping; -} +inline void b2Body::SetLinearDamping(float32 linearDamping) { m_linearDamping = linearDamping; } -inline float32 b2Body::GetAngularDamping() const -{ - return m_angularDamping; -} +inline float32 b2Body::GetAngularDamping() const { return m_angularDamping; } -inline void b2Body::SetAngularDamping(float32 angularDamping) -{ - m_angularDamping = angularDamping; -} +inline void b2Body::SetAngularDamping(float32 angularDamping) { m_angularDamping = angularDamping; } -inline float32 b2Body::GetGravityScale() const -{ - return m_gravityScale; -} +inline float32 b2Body::GetGravityScale() const { return m_gravityScale; } -inline void b2Body::SetGravityScale(float32 scale) -{ - m_gravityScale = scale; -} +inline void b2Body::SetGravityScale(float32 scale) { m_gravityScale = scale; } inline void b2Body::SetBullet(bool flag) { - if (flag) - { - m_flags |= e_bulletFlag; - } - else - { - m_flags &= ~e_bulletFlag; - } + if (flag) { + m_flags |= e_bulletFlag; + } else { + m_flags &= ~e_bulletFlag; + } } -inline bool b2Body::IsBullet() const -{ - return (m_flags & e_bulletFlag) == e_bulletFlag; -} +inline bool b2Body::IsBullet() const { return (m_flags & e_bulletFlag) == e_bulletFlag; } inline void b2Body::SetAwake(bool flag) { - if (flag) - { - m_flags |= e_awakeFlag; - m_sleepTime = 0.0f; - } - else - { - m_flags &= ~e_awakeFlag; - m_sleepTime = 0.0f; - m_linearVelocity.SetZero(); - m_angularVelocity = 0.0f; - m_force.SetZero(); - m_torque = 0.0f; - } + if (flag) { + m_flags |= e_awakeFlag; + m_sleepTime = 0.0f; + } else { + m_flags &= ~e_awakeFlag; + m_sleepTime = 0.0f; + m_linearVelocity.SetZero(); + m_angularVelocity = 0.0f; + m_force.SetZero(); + m_torque = 0.0f; + } } -inline bool b2Body::IsAwake() const -{ - return (m_flags & e_awakeFlag) == e_awakeFlag; -} +inline bool b2Body::IsAwake() const { return (m_flags & e_awakeFlag) == e_awakeFlag; } -inline bool b2Body::IsActive() const -{ - return (m_flags & e_activeFlag) == e_activeFlag; -} +inline bool b2Body::IsActive() const { return (m_flags & e_activeFlag) == e_activeFlag; } inline bool b2Body::IsFixedRotation() const { - return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; + return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; } inline void b2Body::SetSleepingAllowed(bool flag) { - if (flag) - { - m_flags |= e_autoSleepFlag; - } - else - { - m_flags &= ~e_autoSleepFlag; - SetAwake(true); - } + if (flag) { + m_flags |= e_autoSleepFlag; + } else { + m_flags &= ~e_autoSleepFlag; + SetAwake(true); + } } inline bool b2Body::IsSleepingAllowed() const { - return (m_flags & e_autoSleepFlag) == e_autoSleepFlag; + return (m_flags & e_autoSleepFlag) == e_autoSleepFlag; } -inline b2Fixture* b2Body::GetFixtureList() -{ - return m_fixtureList; -} +inline b2Fixture * b2Body::GetFixtureList() { return m_fixtureList; } -inline const b2Fixture* b2Body::GetFixtureList() const -{ - return m_fixtureList; -} +inline const b2Fixture * b2Body::GetFixtureList() const { return m_fixtureList; } -inline b2JointEdge* b2Body::GetJointList() -{ - return m_jointList; -} +inline b2JointEdge * b2Body::GetJointList() { return m_jointList; } -inline const b2JointEdge* b2Body::GetJointList() const -{ - return m_jointList; -} +inline const b2JointEdge * b2Body::GetJointList() const { return m_jointList; } -inline b2ContactEdge* b2Body::GetContactList() -{ - return m_contactList; -} +inline b2ContactEdge * b2Body::GetContactList() { return m_contactList; } -inline const b2ContactEdge* b2Body::GetContactList() const -{ - return m_contactList; -} +inline const b2ContactEdge * b2Body::GetContactList() const { return m_contactList; } -inline b2Body* b2Body::GetNext() -{ - return m_next; -} +inline b2Body * b2Body::GetNext() { return m_next; } -inline const b2Body* b2Body::GetNext() const -{ - return m_next; -} +inline const b2Body * b2Body::GetNext() const { return m_next; } -inline void b2Body::SetUserData(void* data) -{ - m_userData = data; -} +inline void b2Body::SetUserData(void * data) { m_userData = data; } -inline void* b2Body::GetUserData() const -{ - return m_userData; -} +inline void * b2Body::GetUserData() const { return m_userData; } -inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake) +inline void b2Body::ApplyForce(const b2Vec2 & force, const b2Vec2 & point, bool wake) { - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping. - if (m_flags & e_awakeFlag) - { - m_force += force; - m_torque += b2Cross(point - m_sweep.c, force); - } + if (m_type != b2_dynamicBody) { + return; + } + + if (wake && (m_flags & e_awakeFlag) == 0) { + SetAwake(true); + } + + // Don't accumulate a force if the body is sleeping. + if (m_flags & e_awakeFlag) { + m_force += force; + m_torque += b2Cross(point - m_sweep.c, force); + } } -inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake) +inline void b2Body::ApplyForceToCenter(const b2Vec2 & force, bool wake) { - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_force += force; - } + if (m_type != b2_dynamicBody) { + return; + } + + if (wake && (m_flags & e_awakeFlag) == 0) { + SetAwake(true); + } + + // Don't accumulate a force if the body is sleeping + if (m_flags & e_awakeFlag) { + m_force += force; + } } inline void b2Body::ApplyTorque(float32 torque, bool wake) { - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate a force if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_torque += torque; - } + if (m_type != b2_dynamicBody) { + return; + } + + if (wake && (m_flags & e_awakeFlag) == 0) { + SetAwake(true); + } + + // Don't accumulate a force if the body is sleeping + if (m_flags & e_awakeFlag) { + m_torque += torque; + } } -inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake) +inline void b2Body::ApplyLinearImpulse(const b2Vec2 & impulse, const b2Vec2 & point, bool wake) { - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_linearVelocity += m_invMass * impulse; - m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse); - } + if (m_type != b2_dynamicBody) { + return; + } + + if (wake && (m_flags & e_awakeFlag) == 0) { + SetAwake(true); + } + + // Don't accumulate velocity if the body is sleeping + if (m_flags & e_awakeFlag) { + m_linearVelocity += m_invMass * impulse; + m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse); + } } -inline void b2Body::ApplyLinearImpulseToCenter(const b2Vec2& impulse, bool wake) +inline void b2Body::ApplyLinearImpulseToCenter(const b2Vec2 & impulse, bool wake) { - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_linearVelocity += m_invMass * impulse; - } + if (m_type != b2_dynamicBody) { + return; + } + + if (wake && (m_flags & e_awakeFlag) == 0) { + SetAwake(true); + } + + // Don't accumulate velocity if the body is sleeping + if (m_flags & e_awakeFlag) { + m_linearVelocity += m_invMass * impulse; + } } inline void b2Body::ApplyAngularImpulse(float32 impulse, bool wake) { - if (m_type != b2_dynamicBody) - { - return; - } - - if (wake && (m_flags & e_awakeFlag) == 0) - { - SetAwake(true); - } - - // Don't accumulate velocity if the body is sleeping - if (m_flags & e_awakeFlag) - { - m_angularVelocity += m_invI * impulse; - } + if (m_type != b2_dynamicBody) { + return; + } + + if (wake && (m_flags & e_awakeFlag) == 0) { + SetAwake(true); + } + + // Don't accumulate velocity if the body is sleeping + if (m_flags & e_awakeFlag) { + m_angularVelocity += m_invI * impulse; + } } inline void b2Body::SynchronizeTransform() { - m_xf.q.Set(m_sweep.a); - m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); + m_xf.q.Set(m_sweep.a); + m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); } inline void b2Body::Advance(float32 alpha) { - // Advance to the new safe time. This doesn't sync the broad-phase. - m_sweep.Advance(alpha); - m_sweep.c = m_sweep.c0; - m_sweep.a = m_sweep.a0; - m_xf.q.Set(m_sweep.a); - m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); + // Advance to the new safe time. This doesn't sync the broad-phase. + m_sweep.Advance(alpha); + m_sweep.c = m_sweep.c0; + m_sweep.a = m_sweep.a0; + m_xf.q.Set(m_sweep.a); + m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter); } -inline b2World* b2Body::GetWorld() -{ - return m_world; -} +inline b2World * b2Body::GetWorld() { return m_world; } -inline const b2World* b2Body::GetWorld() const -{ - return m_world; -} +inline const b2World * b2Body::GetWorld() const { return m_world; } #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/b2ContactManager.h b/flatland_box2d/include/Box2D/Dynamics/b2ContactManager.h index 63ec3a09..d3a98f71 100644 --- a/flatland_box2d/include/Box2D/Dynamics/b2ContactManager.h +++ b/flatland_box2d/include/Box2D/Dynamics/b2ContactManager.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_CONTACT_MANAGER_H #define B2_CONTACT_MANAGER_H @@ -30,23 +30,23 @@ class b2BlockAllocator; class b2ContactManager { public: - b2ContactManager(); + b2ContactManager(); - // Broad-phase callback. - void AddPair(void* proxyUserDataA, void* proxyUserDataB); + // Broad-phase callback. + void AddPair(void * proxyUserDataA, void * proxyUserDataB); - void FindNewContacts(); + void FindNewContacts(); - void Destroy(b2Contact* c); + void Destroy(b2Contact * c); - void Collide(); - - b2BroadPhase m_broadPhase; - b2Contact* m_contactList; - int32 m_contactCount; - b2ContactFilter* m_contactFilter; - b2ContactListener* m_contactListener; - b2BlockAllocator* m_allocator; + void Collide(); + + b2BroadPhase m_broadPhase; + b2Contact * m_contactList; + int32 m_contactCount; + b2ContactFilter * m_contactFilter; + b2ContactListener * m_contactListener; + b2BlockAllocator * m_allocator; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/b2Fixture.h b/flatland_box2d/include/Box2D/Dynamics/b2Fixture.h index ac12b32d..f61ecf39 100644 --- a/flatland_box2d/include/Box2D/Dynamics/b2Fixture.h +++ b/flatland_box2d/include/Box2D/Dynamics/b2Fixture.h @@ -1,27 +1,27 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_FIXTURE_H #define B2_FIXTURE_H -#include "Box2D/Dynamics/b2Body.h" -#include "Box2D/Collision/b2Collision.h" #include "Box2D/Collision/Shapes/b2Shape.h" +#include "Box2D/Collision/b2Collision.h" +#include "Box2D/Dynamics/b2Body.h" class b2BlockAllocator; class b2Body; @@ -31,315 +31,273 @@ class b2Fixture; /// This holds contact filtering data. struct b2Filter { - b2Filter() - { - categoryBits = 0x0001; - maskBits = 0xFFFF; - groupIndex = 0; - } - - /// The collision category bits. Normally you would just set one bit. - uint16 categoryBits; - - /// The collision mask bits. This states the categories that this - /// shape would accept for collision. - uint16 maskBits; - - /// Collision groups allow a certain group of objects to never collide (negative) - /// or always collide (positive). Zero means no collision group. Non-zero group - /// filtering always wins against the mask bits. - int16 groupIndex; + b2Filter() + { + categoryBits = 0x0001; + maskBits = 0xFFFF; + groupIndex = 0; + } + + /// The collision category bits. Normally you would just set one bit. + uint16 categoryBits; + + /// The collision mask bits. This states the categories that this + /// shape would accept for collision. + uint16 maskBits; + + /// Collision groups allow a certain group of objects to never collide + /// (negative) or always collide (positive). Zero means no collision group. + /// Non-zero group filtering always wins against the mask bits. + int16 groupIndex; }; /// A fixture definition is used to create a fixture. This class defines an /// abstract fixture definition. You can reuse fixture definitions safely. struct b2FixtureDef { - /// The constructor sets the default fixture definition values. - b2FixtureDef() - { - shape = nullptr; - userData = nullptr; - friction = 0.2f; - restitution = 0.0f; - density = 0.0f; - isSensor = false; - } - - /// The shape, this must be set. The shape will be cloned, so you - /// can create the shape on the stack. - const b2Shape* shape; - - /// Use this to store application specific fixture data. - void* userData; - - /// The friction coefficient, usually in the range [0,1]. - float32 friction; - - /// The restitution (elasticity) usually in the range [0,1]. - float32 restitution; - - /// The density, usually in kg/m^2. - float32 density; - - /// A sensor shape collects contact information but never generates a collision - /// response. - bool isSensor; - - /// Contact filtering data. - b2Filter filter; + /// The constructor sets the default fixture definition values. + b2FixtureDef() + { + shape = nullptr; + userData = nullptr; + friction = 0.2f; + restitution = 0.0f; + density = 0.0f; + isSensor = false; + } + + /// The shape, this must be set. The shape will be cloned, so you + /// can create the shape on the stack. + const b2Shape * shape; + + /// Use this to store application specific fixture data. + void * userData; + + /// The friction coefficient, usually in the range [0,1]. + float32 friction; + + /// The restitution (elasticity) usually in the range [0,1]. + float32 restitution; + + /// The density, usually in kg/m^2. + float32 density; + + /// A sensor shape collects contact information but never generates a + /// collision response. + bool isSensor; + + /// Contact filtering data. + b2Filter filter; }; /// This proxy is used internally to connect fixtures to the broad-phase. struct b2FixtureProxy { - b2AABB aabb; - b2Fixture* fixture; - int32 childIndex; - int32 proxyId; + b2AABB aabb; + b2Fixture * fixture; + int32 childIndex; + int32 proxyId; }; -/// A fixture is used to attach a shape to a body for collision detection. A fixture -/// inherits its transform from its parent. Fixtures hold additional non-geometric data -/// such as friction, collision filters, etc. -/// Fixtures are created via b2Body::CreateFixture. +/// A fixture is used to attach a shape to a body for collision detection. A +/// fixture inherits its transform from its parent. Fixtures hold additional +/// non-geometric data such as friction, collision filters, etc. Fixtures are +/// created via b2Body::CreateFixture. /// @warning you cannot reuse fixtures. class b2Fixture { public: - /// Get the type of the child shape. You can use this to down cast to the concrete shape. - /// @return the shape type. - b2Shape::Type GetType() const; - - /// Get the child shape. You can modify the child shape, however you should not change the - /// number of vertices because this will crash some collision caching mechanisms. - /// Manipulating the shape may lead to non-physical behavior. - b2Shape* GetShape(); - const b2Shape* GetShape() const; - - /// Set if this fixture is a sensor. - void SetSensor(bool sensor); - - /// Is this fixture a sensor (non-solid)? - /// @return the true if the shape is a sensor. - bool IsSensor() const; - - /// Set the contact filtering data. This will not update contacts until the next time - /// step when either parent body is active and awake. - /// This automatically calls Refilter. - void SetFilterData(const b2Filter& filter); - - /// Get the contact filtering data. - const b2Filter& GetFilterData() const; - - /// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide. - void Refilter(); - - /// Get the parent body of this fixture. This is nullptr if the fixture is not attached. - /// @return the parent body. - b2Body* GetBody(); - const b2Body* GetBody() const; - - /// Get the next fixture in the parent body's fixture list. - /// @return the next shape. - b2Fixture* GetNext(); - const b2Fixture* GetNext() const; - - /// Get the user data that was assigned in the fixture definition. Use this to - /// store your application specific data. - void* GetUserData() const; - - /// Set the user data. Use this to store your application specific data. - void SetUserData(void* data); - - /// Test a point for containment in this fixture. - /// @param p a point in world coordinates. - bool TestPoint(const b2Vec2& p) const; - - /// Cast a ray against this shape. - /// @param output the ray-cast results. - /// @param input the ray-cast input parameters. - bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const; - - /// Get the mass data for this fixture. The mass data is based on the density and - /// the shape. The rotational inertia is about the shape's origin. This operation - /// may be expensive. - void GetMassData(b2MassData* massData) const; - - /// Set the density of this fixture. This will _not_ automatically adjust the mass - /// of the body. You must call b2Body::ResetMassData to update the body's mass. - void SetDensity(float32 density); - - /// Get the density of this fixture. - float32 GetDensity() const; - - /// Get the coefficient of friction. - float32 GetFriction() const; - - /// Set the coefficient of friction. This will _not_ change the friction of - /// existing contacts. - void SetFriction(float32 friction); - - /// Get the coefficient of restitution. - float32 GetRestitution() const; - - /// Set the coefficient of restitution. This will _not_ change the restitution of - /// existing contacts. - void SetRestitution(float32 restitution); - - /// Get the fixture's AABB. This AABB may be enlarge and/or stale. - /// If you need a more accurate AABB, compute it using the shape and - /// the body transform. - const b2AABB& GetAABB(int32 childIndex) const; - - /// Dump this fixture to the log file. - void Dump(int32 bodyIndex); + /// Get the type of the child shape. You can use this to down cast to the + /// concrete shape. + /// @return the shape type. + b2Shape::Type GetType() const; + + /// Get the child shape. You can modify the child shape, however you should + /// not change the number of vertices because this will crash some collision + /// caching mechanisms. Manipulating the shape may lead to non-physical + /// behavior. + b2Shape * GetShape(); + const b2Shape * GetShape() const; + + /// Set if this fixture is a sensor. + void SetSensor(bool sensor); + + /// Is this fixture a sensor (non-solid)? + /// @return the true if the shape is a sensor. + bool IsSensor() const; + + /// Set the contact filtering data. This will not update contacts until the + /// next time step when either parent body is active and awake. This + /// automatically calls Refilter. + void SetFilterData(const b2Filter & filter); + + /// Get the contact filtering data. + const b2Filter & GetFilterData() const; + + /// Call this if you want to establish collision that was previously disabled + /// by b2ContactFilter::ShouldCollide. + void Refilter(); + + /// Get the parent body of this fixture. This is nullptr if the fixture is not + /// attached. + /// @return the parent body. + b2Body * GetBody(); + const b2Body * GetBody() const; + + /// Get the next fixture in the parent body's fixture list. + /// @return the next shape. + b2Fixture * GetNext(); + const b2Fixture * GetNext() const; + + /// Get the user data that was assigned in the fixture definition. Use this to + /// store your application specific data. + void * GetUserData() const; + + /// Set the user data. Use this to store your application specific data. + void SetUserData(void * data); + + /// Test a point for containment in this fixture. + /// @param p a point in world coordinates. + bool TestPoint(const b2Vec2 & p) const; + + /// Cast a ray against this shape. + /// @param output the ray-cast results. + /// @param input the ray-cast input parameters. + bool RayCast(b2RayCastOutput * output, const b2RayCastInput & input, int32 childIndex) const; + + /// Get the mass data for this fixture. The mass data is based on the density + /// and the shape. The rotational inertia is about the shape's origin. This + /// operation may be expensive. + void GetMassData(b2MassData * massData) const; + + /// Set the density of this fixture. This will _not_ automatically adjust the + /// mass of the body. You must call b2Body::ResetMassData to update the body's + /// mass. + void SetDensity(float32 density); + + /// Get the density of this fixture. + float32 GetDensity() const; + + /// Get the coefficient of friction. + float32 GetFriction() const; + + /// Set the coefficient of friction. This will _not_ change the friction of + /// existing contacts. + void SetFriction(float32 friction); + + /// Get the coefficient of restitution. + float32 GetRestitution() const; + + /// Set the coefficient of restitution. This will _not_ change the restitution + /// of existing contacts. + void SetRestitution(float32 restitution); + + /// Get the fixture's AABB. This AABB may be enlarge and/or stale. + /// If you need a more accurate AABB, compute it using the shape and + /// the body transform. + const b2AABB & GetAABB(int32 childIndex) const; + + /// Dump this fixture to the log file. + void Dump(int32 bodyIndex); protected: + friend class b2Body; + friend class b2World; + friend class b2Contact; + friend class b2ContactManager; - friend class b2Body; - friend class b2World; - friend class b2Contact; - friend class b2ContactManager; - - b2Fixture(); + b2Fixture(); - // We need separation create/destroy functions from the constructor/destructor because - // the destructor cannot access the allocator (no destructor arguments allowed by C++). - void Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def); - void Destroy(b2BlockAllocator* allocator); + // We need separation create/destroy functions from the constructor/destructor + // because the destructor cannot access the allocator (no destructor arguments + // allowed by C++). + void Create(b2BlockAllocator * allocator, b2Body * body, const b2FixtureDef * def); + void Destroy(b2BlockAllocator * allocator); - // These support body activation/deactivation. - void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf); - void DestroyProxies(b2BroadPhase* broadPhase); + // These support body activation/deactivation. + void CreateProxies(b2BroadPhase * broadPhase, const b2Transform & xf); + void DestroyProxies(b2BroadPhase * broadPhase); - void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2); + void Synchronize(b2BroadPhase * broadPhase, const b2Transform & xf1, const b2Transform & xf2); - float32 m_density; + float32 m_density; - b2Fixture* m_next; - b2Body* m_body; + b2Fixture * m_next; + b2Body * m_body; - b2Shape* m_shape; + b2Shape * m_shape; - float32 m_friction; - float32 m_restitution; + float32 m_friction; + float32 m_restitution; - b2FixtureProxy* m_proxies; - int32 m_proxyCount; + b2FixtureProxy * m_proxies; + int32 m_proxyCount; - b2Filter m_filter; + b2Filter m_filter; - bool m_isSensor; + bool m_isSensor; - void* m_userData; + void * m_userData; }; -inline b2Shape::Type b2Fixture::GetType() const -{ - return m_shape->GetType(); -} +inline b2Shape::Type b2Fixture::GetType() const { return m_shape->GetType(); } -inline b2Shape* b2Fixture::GetShape() -{ - return m_shape; -} +inline b2Shape * b2Fixture::GetShape() { return m_shape; } -inline const b2Shape* b2Fixture::GetShape() const -{ - return m_shape; -} +inline const b2Shape * b2Fixture::GetShape() const { return m_shape; } -inline bool b2Fixture::IsSensor() const -{ - return m_isSensor; -} +inline bool b2Fixture::IsSensor() const { return m_isSensor; } -inline const b2Filter& b2Fixture::GetFilterData() const -{ - return m_filter; -} +inline const b2Filter & b2Fixture::GetFilterData() const { return m_filter; } -inline void* b2Fixture::GetUserData() const -{ - return m_userData; -} +inline void * b2Fixture::GetUserData() const { return m_userData; } -inline void b2Fixture::SetUserData(void* data) -{ - m_userData = data; -} +inline void b2Fixture::SetUserData(void * data) { m_userData = data; } -inline b2Body* b2Fixture::GetBody() -{ - return m_body; -} +inline b2Body * b2Fixture::GetBody() { return m_body; } -inline const b2Body* b2Fixture::GetBody() const -{ - return m_body; -} +inline const b2Body * b2Fixture::GetBody() const { return m_body; } -inline b2Fixture* b2Fixture::GetNext() -{ - return m_next; -} +inline b2Fixture * b2Fixture::GetNext() { return m_next; } -inline const b2Fixture* b2Fixture::GetNext() const -{ - return m_next; -} +inline const b2Fixture * b2Fixture::GetNext() const { return m_next; } inline void b2Fixture::SetDensity(float32 density) { - b2Assert(b2IsValid(density) && density >= 0.0f); - m_density = density; + b2Assert(b2IsValid(density) && density >= 0.0f); + m_density = density; } -inline float32 b2Fixture::GetDensity() const -{ - return m_density; -} +inline float32 b2Fixture::GetDensity() const { return m_density; } -inline float32 b2Fixture::GetFriction() const -{ - return m_friction; -} +inline float32 b2Fixture::GetFriction() const { return m_friction; } -inline void b2Fixture::SetFriction(float32 friction) -{ - m_friction = friction; -} +inline void b2Fixture::SetFriction(float32 friction) { m_friction = friction; } -inline float32 b2Fixture::GetRestitution() const -{ - return m_restitution; -} +inline float32 b2Fixture::GetRestitution() const { return m_restitution; } -inline void b2Fixture::SetRestitution(float32 restitution) -{ - m_restitution = restitution; -} +inline void b2Fixture::SetRestitution(float32 restitution) { m_restitution = restitution; } -inline bool b2Fixture::TestPoint(const b2Vec2& p) const +inline bool b2Fixture::TestPoint(const b2Vec2 & p) const { - return m_shape->TestPoint(m_body->GetTransform(), p); + return m_shape->TestPoint(m_body->GetTransform(), p); } -inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const +inline bool b2Fixture::RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, int32 childIndex) const { - return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex); + return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex); } -inline void b2Fixture::GetMassData(b2MassData* massData) const +inline void b2Fixture::GetMassData(b2MassData * massData) const { - m_shape->ComputeMass(massData, m_density); + m_shape->ComputeMass(massData, m_density); } -inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const +inline const b2AABB & b2Fixture::GetAABB(int32 childIndex) const { - b2Assert(0 <= childIndex && childIndex < m_proxyCount); - return m_proxies[childIndex].aabb; + b2Assert(0 <= childIndex && childIndex < m_proxyCount); + return m_proxies[childIndex].aabb; } #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/b2Island.h b/flatland_box2d/include/Box2D/Dynamics/b2Island.h index 32a25f8a..84c2d82d 100644 --- a/flatland_box2d/include/Box2D/Dynamics/b2Island.h +++ b/flatland_box2d/include/Box2D/Dynamics/b2Island.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_ISLAND_H #define B2_ISLAND_H @@ -34,60 +34,61 @@ struct b2Profile; class b2Island { public: - b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity, - b2StackAllocator* allocator, b2ContactListener* listener); - ~b2Island(); - - void Clear() - { - m_bodyCount = 0; - m_contactCount = 0; - m_jointCount = 0; - } - - void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep); - - void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB); - - void Add(b2Body* body) - { - b2Assert(m_bodyCount < m_bodyCapacity); - body->m_islandIndex = m_bodyCount; - m_bodies[m_bodyCount] = body; - ++m_bodyCount; - } - - void Add(b2Contact* contact) - { - b2Assert(m_contactCount < m_contactCapacity); - m_contacts[m_contactCount++] = contact; - } - - void Add(b2Joint* joint) - { - b2Assert(m_jointCount < m_jointCapacity); - m_joints[m_jointCount++] = joint; - } - - void Report(const b2ContactVelocityConstraint* constraints); - - b2StackAllocator* m_allocator; - b2ContactListener* m_listener; - - b2Body** m_bodies; - b2Contact** m_contacts; - b2Joint** m_joints; - - b2Position* m_positions; - b2Velocity* m_velocities; - - int32 m_bodyCount; - int32 m_jointCount; - int32 m_contactCount; - - int32 m_bodyCapacity; - int32 m_contactCapacity; - int32 m_jointCapacity; + b2Island( + int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity, b2StackAllocator * allocator, + b2ContactListener * listener); + ~b2Island(); + + void Clear() + { + m_bodyCount = 0; + m_contactCount = 0; + m_jointCount = 0; + } + + void Solve(b2Profile * profile, const b2TimeStep & step, const b2Vec2 & gravity, bool allowSleep); + + void SolveTOI(const b2TimeStep & subStep, int32 toiIndexA, int32 toiIndexB); + + void Add(b2Body * body) + { + b2Assert(m_bodyCount < m_bodyCapacity); + body->m_islandIndex = m_bodyCount; + m_bodies[m_bodyCount] = body; + ++m_bodyCount; + } + + void Add(b2Contact * contact) + { + b2Assert(m_contactCount < m_contactCapacity); + m_contacts[m_contactCount++] = contact; + } + + void Add(b2Joint * joint) + { + b2Assert(m_jointCount < m_jointCapacity); + m_joints[m_jointCount++] = joint; + } + + void Report(const b2ContactVelocityConstraint * constraints); + + b2StackAllocator * m_allocator; + b2ContactListener * m_listener; + + b2Body ** m_bodies; + b2Contact ** m_contacts; + b2Joint ** m_joints; + + b2Position * m_positions; + b2Velocity * m_velocities; + + int32 m_bodyCount; + int32 m_jointCount; + int32 m_contactCount; + + int32 m_bodyCapacity; + int32 m_contactCapacity; + int32 m_jointCapacity; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/b2TimeStep.h b/flatland_box2d/include/Box2D/Dynamics/b2TimeStep.h index 642643eb..6d75ae18 100644 --- a/flatland_box2d/include/Box2D/Dynamics/b2TimeStep.h +++ b/flatland_box2d/include/Box2D/Dynamics/b2TimeStep.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_TIME_STEP_H #define B2_TIME_STEP_H @@ -24,47 +24,47 @@ /// Profiling data. Times are in milliseconds. struct b2Profile { - float32 step; - float32 collide; - float32 solve; - float32 solveInit; - float32 solveVelocity; - float32 solvePosition; - float32 broadphase; - float32 solveTOI; + float32 step; + float32 collide; + float32 solve; + float32 solveInit; + float32 solveVelocity; + float32 solvePosition; + float32 broadphase; + float32 solveTOI; }; /// This is an internal structure. struct b2TimeStep { - float32 dt; // time step - float32 inv_dt; // inverse time step (0 if dt == 0). - float32 dtRatio; // dt * inv_dt0 - int32 velocityIterations; - int32 positionIterations; - bool warmStarting; + float32 dt; // time step + float32 inv_dt; // inverse time step (0 if dt == 0). + float32 dtRatio; // dt * inv_dt0 + int32 velocityIterations; + int32 positionIterations; + bool warmStarting; }; /// This is an internal structure. struct b2Position { - b2Vec2 c; - float32 a; + b2Vec2 c; + float32 a; }; /// This is an internal structure. struct b2Velocity { - b2Vec2 v; - float32 w; + b2Vec2 v; + float32 w; }; /// Solver Data struct b2SolverData { - b2TimeStep step; - b2Position* positions; - b2Velocity* velocities; + b2TimeStep step; + b2Position * positions; + b2Velocity * velocities; }; #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/b2World.h b/flatland_box2d/include/Box2D/Dynamics/b2World.h index fa0ba0b8..335acdf7 100644 --- a/flatland_box2d/include/Box2D/Dynamics/b2World.h +++ b/flatland_box2d/include/Box2D/Dynamics/b2World.h @@ -1,30 +1,30 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_WORLD_H #define B2_WORLD_H -#include "Box2D/Common/b2Math.h" #include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Common/b2Math.h" #include "Box2D/Common/b2StackAllocator.h" #include "Box2D/Dynamics/b2ContactManager.h" -#include "Box2D/Dynamics/b2WorldCallbacks.h" #include "Box2D/Dynamics/b2TimeStep.h" +#include "Box2D/Dynamics/b2WorldCallbacks.h" struct b2AABB; struct b2BodyDef; @@ -41,314 +41,270 @@ class b2Joint; class b2World { public: - /// Construct a world object. - /// @param gravity the world gravity vector. - b2World(const b2Vec2& gravity); - - /// Destruct the world. All physics entities are destroyed and all heap memory is released. - ~b2World(); - - /// Register a destruction listener. The listener is owned by you and must - /// remain in scope. - void SetDestructionListener(b2DestructionListener* listener); - - /// Register a contact filter to provide specific control over collision. - /// Otherwise the default filter is used (b2_defaultFilter). The listener is - /// owned by you and must remain in scope. - void SetContactFilter(b2ContactFilter* filter); - - /// Register a contact event listener. The listener is owned by you and must - /// remain in scope. - void SetContactListener(b2ContactListener* listener); - - /// Register a routine for debug drawing. The debug draw functions are called - /// inside with b2World::DrawDebugData method. The debug draw object is owned - /// by you and must remain in scope. - void SetDebugDraw(b2Draw* debugDraw); - - /// Create a rigid body given a definition. No reference to the definition - /// is retained. - /// @warning This function is locked during callbacks. - b2Body* CreateBody(const b2BodyDef* def); - - /// Destroy a rigid body given a definition. No reference to the definition - /// is retained. This function is locked during callbacks. - /// @warning This automatically deletes all associated shapes and joints. - /// @warning This function is locked during callbacks. - void DestroyBody(b2Body* body); - - /// Create a joint to constrain bodies together. No reference to the definition - /// is retained. This may cause the connected bodies to cease colliding. - /// @warning This function is locked during callbacks. - b2Joint* CreateJoint(const b2JointDef* def); - - /// Destroy a joint. This may cause the connected bodies to begin colliding. - /// @warning This function is locked during callbacks. - void DestroyJoint(b2Joint* joint); - - /// Take a time step. This performs collision detection, integration, - /// and constraint solution. - /// @param timeStep the amount of time to simulate, this should not vary. - /// @param velocityIterations for the velocity constraint solver. - /// @param positionIterations for the position constraint solver. - void Step( float32 timeStep, - int32 velocityIterations, - int32 positionIterations); - - /// Manually clear the force buffer on all bodies. By default, forces are cleared automatically - /// after each call to Step. The default behavior is modified by calling SetAutoClearForces. - /// The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain - /// a fixed sized time step under a variable frame-rate. - /// When you perform sub-stepping you will disable auto clearing of forces and instead call - /// ClearForces after all sub-steps are complete in one pass of your game loop. - /// @see SetAutoClearForces - void ClearForces(); - - /// Call this to draw shapes and other debug draw data. This is intentionally non-const. - void DrawDebugData(); - - /// Query the world for all fixtures that potentially overlap the - /// provided AABB. - /// @param callback a user implemented callback class. - /// @param aabb the query box. - void QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const; - - /// Ray-cast the world for all fixtures in the path of the ray. Your callback - /// controls whether you get the closest point, any point, or n-points. - /// The ray-cast ignores shapes that contain the starting point. - /// @param callback a user implemented callback class. - /// @param point1 the ray starting point - /// @param point2 the ray ending point - void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const; - - /// Get the world body list. With the returned body, use b2Body::GetNext to get - /// the next body in the world list. A nullptr body indicates the end of the list. - /// @return the head of the world body list. - b2Body* GetBodyList(); - const b2Body* GetBodyList() const; - - /// Get the world joint list. With the returned joint, use b2Joint::GetNext to get - /// the next joint in the world list. A nullptr joint indicates the end of the list. - /// @return the head of the world joint list. - b2Joint* GetJointList(); - const b2Joint* GetJointList() const; - - /// Get the world contact list. With the returned contact, use b2Contact::GetNext to get - /// the next contact in the world list. A nullptr contact indicates the end of the list. - /// @return the head of the world contact list. - /// @warning contacts are created and destroyed in the middle of a time step. - /// Use b2ContactListener to avoid missing contacts. - b2Contact* GetContactList(); - const b2Contact* GetContactList() const; - - /// Enable/disable sleep. - void SetAllowSleeping(bool flag); - bool GetAllowSleeping() const { return m_allowSleep; } - - /// Enable/disable warm starting. For testing. - void SetWarmStarting(bool flag) { m_warmStarting = flag; } - bool GetWarmStarting() const { return m_warmStarting; } - - /// Enable/disable continuous physics. For testing. - void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; } - bool GetContinuousPhysics() const { return m_continuousPhysics; } - - /// Enable/disable single stepped continuous physics. For testing. - void SetSubStepping(bool flag) { m_subStepping = flag; } - bool GetSubStepping() const { return m_subStepping; } - - /// Get the number of broad-phase proxies. - int32 GetProxyCount() const; - - /// Get the number of bodies. - int32 GetBodyCount() const; - - /// Get the number of joints. - int32 GetJointCount() const; - - /// Get the number of contacts (each may have 0 or more contact points). - int32 GetContactCount() const; - - /// Get the height of the dynamic tree. - int32 GetTreeHeight() const; - - /// Get the balance of the dynamic tree. - int32 GetTreeBalance() const; - - /// Get the quality metric of the dynamic tree. The smaller the better. - /// The minimum is 1. - float32 GetTreeQuality() const; - - /// Change the global gravity vector. - void SetGravity(const b2Vec2& gravity); - - /// Get the global gravity vector. - b2Vec2 GetGravity() const; - - /// Is the world locked (in the middle of a time step). - bool IsLocked() const; - - /// Set flag to control automatic clearing of forces after each time step. - void SetAutoClearForces(bool flag); - - /// Get the flag that controls automatic clearing of forces after each time step. - bool GetAutoClearForces() const; - - /// Shift the world origin. Useful for large worlds. - /// The body shift formula is: position -= newOrigin - /// @param newOrigin the new origin with respect to the old origin - void ShiftOrigin(const b2Vec2& newOrigin); - - /// Get the contact manager for testing. - const b2ContactManager& GetContactManager() const; - - /// Get the current profile. - const b2Profile& GetProfile() const; - - /// Dump the world into the log file. - /// @warning this should be called outside of a time step. - void Dump(); + /// Construct a world object. + /// @param gravity the world gravity vector. + b2World(const b2Vec2 & gravity); + + /// Destruct the world. All physics entities are destroyed and all heap memory + /// is released. + ~b2World(); + + /// Register a destruction listener. The listener is owned by you and must + /// remain in scope. + void SetDestructionListener(b2DestructionListener * listener); + + /// Register a contact filter to provide specific control over collision. + /// Otherwise the default filter is used (b2_defaultFilter). The listener is + /// owned by you and must remain in scope. + void SetContactFilter(b2ContactFilter * filter); + + /// Register a contact event listener. The listener is owned by you and must + /// remain in scope. + void SetContactListener(b2ContactListener * listener); + + /// Register a routine for debug drawing. The debug draw functions are called + /// inside with b2World::DrawDebugData method. The debug draw object is owned + /// by you and must remain in scope. + void SetDebugDraw(b2Draw * debugDraw); + + /// Create a rigid body given a definition. No reference to the definition + /// is retained. + /// @warning This function is locked during callbacks. + b2Body * CreateBody(const b2BodyDef * def); + + /// Destroy a rigid body given a definition. No reference to the definition + /// is retained. This function is locked during callbacks. + /// @warning This automatically deletes all associated shapes and joints. + /// @warning This function is locked during callbacks. + void DestroyBody(b2Body * body); + + /// Create a joint to constrain bodies together. No reference to the + /// definition is retained. This may cause the connected bodies to cease + /// colliding. + /// @warning This function is locked during callbacks. + b2Joint * CreateJoint(const b2JointDef * def); + + /// Destroy a joint. This may cause the connected bodies to begin colliding. + /// @warning This function is locked during callbacks. + void DestroyJoint(b2Joint * joint); + + /// Take a time step. This performs collision detection, integration, + /// and constraint solution. + /// @param timeStep the amount of time to simulate, this should not vary. + /// @param velocityIterations for the velocity constraint solver. + /// @param positionIterations for the position constraint solver. + void Step(float32 timeStep, int32 velocityIterations, int32 positionIterations); + + /// Manually clear the force buffer on all bodies. By default, forces are + /// cleared automatically after each call to Step. The default behavior is + /// modified by calling SetAutoClearForces. The purpose of this function is to + /// support sub-stepping. Sub-stepping is often used to maintain a fixed sized + /// time step under a variable frame-rate. When you perform sub-stepping you + /// will disable auto clearing of forces and instead call ClearForces after + /// all sub-steps are complete in one pass of your game loop. + /// @see SetAutoClearForces + void ClearForces(); + + /// Call this to draw shapes and other debug draw data. This is intentionally + /// non-const. + void DrawDebugData(); + + /// Query the world for all fixtures that potentially overlap the + /// provided AABB. + /// @param callback a user implemented callback class. + /// @param aabb the query box. + void QueryAABB(b2QueryCallback * callback, const b2AABB & aabb) const; + + /// Ray-cast the world for all fixtures in the path of the ray. Your callback + /// controls whether you get the closest point, any point, or n-points. + /// The ray-cast ignores shapes that contain the starting point. + /// @param callback a user implemented callback class. + /// @param point1 the ray starting point + /// @param point2 the ray ending point + void RayCast(b2RayCastCallback * callback, const b2Vec2 & point1, const b2Vec2 & point2) const; + + /// Get the world body list. With the returned body, use b2Body::GetNext to + /// get the next body in the world list. A nullptr body indicates the end of + /// the list. + /// @return the head of the world body list. + b2Body * GetBodyList(); + const b2Body * GetBodyList() const; + + /// Get the world joint list. With the returned joint, use b2Joint::GetNext to + /// get the next joint in the world list. A nullptr joint indicates the end of + /// the list. + /// @return the head of the world joint list. + b2Joint * GetJointList(); + const b2Joint * GetJointList() const; + + /// Get the world contact list. With the returned contact, use + /// b2Contact::GetNext to get the next contact in the world list. A nullptr + /// contact indicates the end of the list. + /// @return the head of the world contact list. + /// @warning contacts are created and destroyed in the middle of a time step. + /// Use b2ContactListener to avoid missing contacts. + b2Contact * GetContactList(); + const b2Contact * GetContactList() const; + + /// Enable/disable sleep. + void SetAllowSleeping(bool flag); + bool GetAllowSleeping() const { return m_allowSleep; } + + /// Enable/disable warm starting. For testing. + void SetWarmStarting(bool flag) { m_warmStarting = flag; } + bool GetWarmStarting() const { return m_warmStarting; } + + /// Enable/disable continuous physics. For testing. + void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; } + bool GetContinuousPhysics() const { return m_continuousPhysics; } + + /// Enable/disable single stepped continuous physics. For testing. + void SetSubStepping(bool flag) { m_subStepping = flag; } + bool GetSubStepping() const { return m_subStepping; } + + /// Get the number of broad-phase proxies. + int32 GetProxyCount() const; + + /// Get the number of bodies. + int32 GetBodyCount() const; + + /// Get the number of joints. + int32 GetJointCount() const; + + /// Get the number of contacts (each may have 0 or more contact points). + int32 GetContactCount() const; + + /// Get the height of the dynamic tree. + int32 GetTreeHeight() const; + + /// Get the balance of the dynamic tree. + int32 GetTreeBalance() const; + + /// Get the quality metric of the dynamic tree. The smaller the better. + /// The minimum is 1. + float32 GetTreeQuality() const; + + /// Change the global gravity vector. + void SetGravity(const b2Vec2 & gravity); + + /// Get the global gravity vector. + b2Vec2 GetGravity() const; + + /// Is the world locked (in the middle of a time step). + bool IsLocked() const; + + /// Set flag to control automatic clearing of forces after each time step. + void SetAutoClearForces(bool flag); + + /// Get the flag that controls automatic clearing of forces after each time + /// step. + bool GetAutoClearForces() const; + + /// Shift the world origin. Useful for large worlds. + /// The body shift formula is: position -= newOrigin + /// @param newOrigin the new origin with respect to the old origin + void ShiftOrigin(const b2Vec2 & newOrigin); -private: + /// Get the contact manager for testing. + const b2ContactManager & GetContactManager() const; + + /// Get the current profile. + const b2Profile & GetProfile() const; + + /// Dump the world into the log file. + /// @warning this should be called outside of a time step. + void Dump(); - // m_flags - enum - { - e_newFixture = 0x0001, - e_locked = 0x0002, - e_clearForces = 0x0004 - }; +private: + // m_flags + enum { e_newFixture = 0x0001, e_locked = 0x0002, e_clearForces = 0x0004 }; - friend class b2Body; - friend class b2Fixture; - friend class b2ContactManager; - friend class b2Controller; + friend class b2Body; + friend class b2Fixture; + friend class b2ContactManager; + friend class b2Controller; - void Solve(const b2TimeStep& step); - void SolveTOI(const b2TimeStep& step); + void Solve(const b2TimeStep & step); + void SolveTOI(const b2TimeStep & step); - void DrawJoint(b2Joint* joint); - void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color); + void DrawJoint(b2Joint * joint); + void DrawShape(b2Fixture * shape, const b2Transform & xf, const b2Color & color); - b2BlockAllocator m_blockAllocator; - b2StackAllocator m_stackAllocator; + b2BlockAllocator m_blockAllocator; + b2StackAllocator m_stackAllocator; - int32 m_flags; + int32 m_flags; - b2ContactManager m_contactManager; + b2ContactManager m_contactManager; - b2Body* m_bodyList; - b2Joint* m_jointList; + b2Body * m_bodyList; + b2Joint * m_jointList; - int32 m_bodyCount; - int32 m_jointCount; + int32 m_bodyCount; + int32 m_jointCount; - b2Vec2 m_gravity; - bool m_allowSleep; + b2Vec2 m_gravity; + bool m_allowSleep; - b2DestructionListener* m_destructionListener; - b2Draw* g_debugDraw; + b2DestructionListener * m_destructionListener; + b2Draw * g_debugDraw; - // This is used to compute the time step ratio to - // support a variable time step. - float32 m_inv_dt0; + // This is used to compute the time step ratio to + // support a variable time step. + float32 m_inv_dt0; - // These are for debugging the solver. - bool m_warmStarting; - bool m_continuousPhysics; - bool m_subStepping; + // These are for debugging the solver. + bool m_warmStarting; + bool m_continuousPhysics; + bool m_subStepping; - bool m_stepComplete; + bool m_stepComplete; - b2Profile m_profile; + b2Profile m_profile; }; -inline b2Body* b2World::GetBodyList() -{ - return m_bodyList; -} +inline b2Body * b2World::GetBodyList() { return m_bodyList; } -inline const b2Body* b2World::GetBodyList() const -{ - return m_bodyList; -} +inline const b2Body * b2World::GetBodyList() const { return m_bodyList; } -inline b2Joint* b2World::GetJointList() -{ - return m_jointList; -} +inline b2Joint * b2World::GetJointList() { return m_jointList; } -inline const b2Joint* b2World::GetJointList() const -{ - return m_jointList; -} +inline const b2Joint * b2World::GetJointList() const { return m_jointList; } -inline b2Contact* b2World::GetContactList() -{ - return m_contactManager.m_contactList; -} +inline b2Contact * b2World::GetContactList() { return m_contactManager.m_contactList; } -inline const b2Contact* b2World::GetContactList() const -{ - return m_contactManager.m_contactList; -} +inline const b2Contact * b2World::GetContactList() const { return m_contactManager.m_contactList; } -inline int32 b2World::GetBodyCount() const -{ - return m_bodyCount; -} +inline int32 b2World::GetBodyCount() const { return m_bodyCount; } -inline int32 b2World::GetJointCount() const -{ - return m_jointCount; -} +inline int32 b2World::GetJointCount() const { return m_jointCount; } -inline int32 b2World::GetContactCount() const -{ - return m_contactManager.m_contactCount; -} +inline int32 b2World::GetContactCount() const { return m_contactManager.m_contactCount; } -inline void b2World::SetGravity(const b2Vec2& gravity) -{ - m_gravity = gravity; -} +inline void b2World::SetGravity(const b2Vec2 & gravity) { m_gravity = gravity; } -inline b2Vec2 b2World::GetGravity() const -{ - return m_gravity; -} +inline b2Vec2 b2World::GetGravity() const { return m_gravity; } -inline bool b2World::IsLocked() const -{ - return (m_flags & e_locked) == e_locked; -} +inline bool b2World::IsLocked() const { return (m_flags & e_locked) == e_locked; } inline void b2World::SetAutoClearForces(bool flag) { - if (flag) - { - m_flags |= e_clearForces; - } - else - { - m_flags &= ~e_clearForces; - } + if (flag) { + m_flags |= e_clearForces; + } else { + m_flags &= ~e_clearForces; + } } -/// Get the flag that controls automatic clearing of forces after each time step. +/// Get the flag that controls automatic clearing of forces after each time +/// step. inline bool b2World::GetAutoClearForces() const { - return (m_flags & e_clearForces) == e_clearForces; + return (m_flags & e_clearForces) == e_clearForces; } -inline const b2ContactManager& b2World::GetContactManager() const -{ - return m_contactManager; -} +inline const b2ContactManager & b2World::GetContactManager() const { return m_contactManager; } -inline const b2Profile& b2World::GetProfile() const -{ - return m_profile; -} +inline const b2Profile & b2World::GetProfile() const { return m_profile; } #endif diff --git a/flatland_box2d/include/Box2D/Dynamics/b2WorldCallbacks.h b/flatland_box2d/include/Box2D/Dynamics/b2WorldCallbacks.h index 539b9320..9bc745d5 100644 --- a/flatland_box2d/include/Box2D/Dynamics/b2WorldCallbacks.h +++ b/flatland_box2d/include/Box2D/Dynamics/b2WorldCallbacks.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_WORLD_CALLBACKS_H #define B2_WORLD_CALLBACKS_H @@ -36,27 +36,29 @@ struct b2Manifold; class b2DestructionListener { public: - virtual ~b2DestructionListener() {} + virtual ~b2DestructionListener() {} - /// Called when any joint is about to be destroyed due - /// to the destruction of one of its attached bodies. - virtual void SayGoodbye(b2Joint* joint) = 0; + /// Called when any joint is about to be destroyed due + /// to the destruction of one of its attached bodies. + virtual void SayGoodbye(b2Joint * joint) = 0; - /// Called when any fixture is about to be destroyed due - /// to the destruction of its parent body. - virtual void SayGoodbye(b2Fixture* fixture) = 0; + /// Called when any fixture is about to be destroyed due + /// to the destruction of its parent body. + virtual void SayGoodbye(b2Fixture * fixture) = 0; }; -/// Implement this class to provide collision filtering. In other words, you can implement -/// this class if you want finer control over contact creation. +/// Implement this class to provide collision filtering. In other words, you can +/// implement this class if you want finer control over contact creation. class b2ContactFilter { public: - virtual ~b2ContactFilter() {} + virtual ~b2ContactFilter() {} - /// Return true if contact calculations should be performed between these two shapes. - /// @warning for performance reasons this is only called when the AABBs begin to overlap. - virtual bool ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB); + /// Return true if contact calculations should be performed between these two + /// shapes. + /// @warning for performance reasons this is only called when the AABBs begin + /// to overlap. + virtual bool ShouldCollide(b2Fixture * fixtureA, b2Fixture * fixtureB); }; /// Contact impulses for reporting. Impulses are used instead of forces because @@ -64,13 +66,13 @@ class b2ContactFilter /// match up one-to-one with the contact points in b2Manifold. struct b2ContactImpulse { - float32 normalImpulses[b2_maxManifoldPoints]; - float32 tangentImpulses[b2_maxManifoldPoints]; - int32 count; + float32 normalImpulses[b2_maxManifoldPoints]; + float32 tangentImpulses[b2_maxManifoldPoints]; + int32 count; }; -/// Implement this class to get contact information. You can use these results for -/// things like sounds and game logic. You can also get contact results by +/// Implement this class to get contact information. You can use these results +/// for things like sounds and game logic. You can also get contact results by /// traversing the contact lists after the time step. However, you might miss /// some contacts because continuous physics leads to sub-stepping. /// Additionally you may receive multiple callbacks for the same contact in a @@ -81,41 +83,39 @@ struct b2ContactImpulse class b2ContactListener { public: - virtual ~b2ContactListener() {} - - /// Called when two fixtures begin to touch. - virtual void BeginContact(b2Contact* contact) { B2_NOT_USED(contact); } - - /// Called when two fixtures cease to touch. - virtual void EndContact(b2Contact* contact) { B2_NOT_USED(contact); } - - /// This is called after a contact is updated. This allows you to inspect a - /// contact before it goes to the solver. If you are careful, you can modify the - /// contact manifold (e.g. disable contact). - /// A copy of the old manifold is provided so that you can detect changes. - /// Note: this is called only for awake bodies. - /// Note: this is called even when the number of contact points is zero. - /// Note: this is not called for sensors. - /// Note: if you set the number of contact points to zero, you will not - /// get an EndContact callback. However, you may get a BeginContact callback - /// the next step. - virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold) - { - B2_NOT_USED(contact); - B2_NOT_USED(oldManifold); - } - - /// This lets you inspect a contact after the solver is finished. This is useful - /// for inspecting impulses. - /// Note: the contact manifold does not include time of impact impulses, which can be - /// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly - /// in a separate data structure. - /// Note: this is only called for contacts that are touching, solid, and awake. - virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse) - { - B2_NOT_USED(contact); - B2_NOT_USED(impulse); - } + virtual ~b2ContactListener() {} + + /// Called when two fixtures begin to touch. + virtual void BeginContact(b2Contact * contact) { B2_NOT_USED(contact); } + + /// Called when two fixtures cease to touch. + virtual void EndContact(b2Contact * contact) { B2_NOT_USED(contact); } + + /// This is called after a contact is updated. This allows you to inspect a + /// contact before it goes to the solver. If you are careful, you can modify + /// the contact manifold (e.g. disable contact). A copy of the old manifold is + /// provided so that you can detect changes. Note: this is called only for + /// awake bodies. Note: this is called even when the number of contact points + /// is zero. Note: this is not called for sensors. Note: if you set the number + /// of contact points to zero, you will not get an EndContact callback. + /// However, you may get a BeginContact callback the next step. + virtual void PreSolve(b2Contact * contact, const b2Manifold * oldManifold) + { + B2_NOT_USED(contact); + B2_NOT_USED(oldManifold); + } + + /// This lets you inspect a contact after the solver is finished. This is + /// useful for inspecting impulses. Note: the contact manifold does not + /// include time of impact impulses, which can be arbitrarily large if the + /// sub-step is small. Hence the impulse is provided explicitly in a separate + /// data structure. Note: this is only called for contacts that are touching, + /// solid, and awake. + virtual void PostSolve(b2Contact * contact, const b2ContactImpulse * impulse) + { + B2_NOT_USED(contact); + B2_NOT_USED(impulse); + } }; /// Callback class for AABB queries. @@ -123,11 +123,11 @@ class b2ContactListener class b2QueryCallback { public: - virtual ~b2QueryCallback() {} + virtual ~b2QueryCallback() {} - /// Called for each fixture found in the query AABB. - /// @return false to terminate the query. - virtual bool ReportFixture(b2Fixture* fixture) = 0; + /// Called for each fixture found in the query AABB. + /// @return false to terminate the query. + virtual bool ReportFixture(b2Fixture * fixture) = 0; }; /// Callback class for ray casts. @@ -135,21 +135,21 @@ class b2QueryCallback class b2RayCastCallback { public: - virtual ~b2RayCastCallback() {} - - /// Called for each fixture found in the query. You control how the ray cast - /// proceeds by returning a float: - /// return -1: ignore this fixture and continue - /// return 0: terminate the ray cast - /// return fraction: clip the ray to this point - /// return 1: don't clip the ray and continue - /// @param fixture the fixture hit by the ray - /// @param point the point of initial intersection - /// @param normal the normal vector at the point of intersection - /// @return -1 to filter, 0 to terminate, fraction to clip the ray for - /// closest hit, 1 to continue - virtual float32 ReportFixture( b2Fixture* fixture, const b2Vec2& point, - const b2Vec2& normal, float32 fraction) = 0; + virtual ~b2RayCastCallback() {} + + /// Called for each fixture found in the query. You control how the ray cast + /// proceeds by returning a float: + /// return -1: ignore this fixture and continue + /// return 0: terminate the ray cast + /// return fraction: clip the ray to this point + /// return 1: don't clip the ray and continue + /// @param fixture the fixture hit by the ray + /// @param point the point of initial intersection + /// @param normal the normal vector at the point of intersection + /// @return -1 to filter, 0 to terminate, fraction to clip the ray for + /// closest hit, 1 to continue + virtual float32 ReportFixture( + b2Fixture * fixture, const b2Vec2 & point, const b2Vec2 & normal, float32 fraction) = 0; }; #endif diff --git a/flatland_box2d/include/Box2D/Rope/b2Rope.h b/flatland_box2d/include/Box2D/Rope/b2Rope.h index becbaf7c..7d8c76c6 100644 --- a/flatland_box2d/include/Box2D/Rope/b2Rope.h +++ b/flatland_box2d/include/Box2D/Rope/b2Rope.h @@ -1,20 +1,20 @@ /* -* Copyright (c) 2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_ROPE_H #define B2_ROPE_H @@ -23,93 +23,86 @@ class b2Draw; -/// +/// struct b2RopeDef { - b2RopeDef() - { - vertices = nullptr; - count = 0; - masses = nullptr; - gravity.SetZero(); - damping = 0.1f; - k2 = 0.9f; - k3 = 0.1f; - } + b2RopeDef() + { + vertices = nullptr; + count = 0; + masses = nullptr; + gravity.SetZero(); + damping = 0.1f; + k2 = 0.9f; + k3 = 0.1f; + } - /// - b2Vec2* vertices; + /// + b2Vec2 * vertices; - /// - int32 count; + /// + int32 count; - /// - float32* masses; + /// + float32 * masses; - /// - b2Vec2 gravity; + /// + b2Vec2 gravity; - /// - float32 damping; + /// + float32 damping; - /// Stretching stiffness - float32 k2; + /// Stretching stiffness + float32 k2; - /// Bending stiffness. Values above 0.5 can make the simulation blow up. - float32 k3; + /// Bending stiffness. Values above 0.5 can make the simulation blow up. + float32 k3; }; -/// +/// class b2Rope { public: - b2Rope(); - ~b2Rope(); + b2Rope(); + ~b2Rope(); - /// - void Initialize(const b2RopeDef* def); + /// + void Initialize(const b2RopeDef * def); - /// - void Step(float32 timeStep, int32 iterations); + /// + void Step(float32 timeStep, int32 iterations); - /// - int32 GetVertexCount() const - { - return m_count; - } + /// + int32 GetVertexCount() const { return m_count; } - /// - const b2Vec2* GetVertices() const - { - return m_ps; - } + /// + const b2Vec2 * GetVertices() const { return m_ps; } - /// - void Draw(b2Draw* draw) const; + /// + void Draw(b2Draw * draw) const; - /// - void SetAngle(float32 angle); + /// + void SetAngle(float32 angle); private: + void SolveC2(); + void SolveC3(); - void SolveC2(); - void SolveC3(); + int32 m_count; + b2Vec2 * m_ps; + b2Vec2 * m_p0s; + b2Vec2 * m_vs; - int32 m_count; - b2Vec2* m_ps; - b2Vec2* m_p0s; - b2Vec2* m_vs; + float32 * m_ims; - float32* m_ims; + float32 * m_Ls; + float32 * m_as; - float32* m_Ls; - float32* m_as; + b2Vec2 m_gravity; + float32 m_damping; - b2Vec2 m_gravity; - float32 m_damping; - - float32 m_k2; - float32 m_k3; + float32 m_k2; + float32 m_k3; }; #endif diff --git a/flatland_box2d/src/Collision/Shapes/b2ChainShape.cpp b/flatland_box2d/src/Collision/Shapes/b2ChainShape.cpp index 2e510438..d857ce9f 100644 --- a/flatland_box2d/src/Collision/Shapes/b2ChainShape.cpp +++ b/flatland_box2d/src/Collision/Shapes/b2ChainShape.cpp @@ -1,198 +1,188 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Collision/Shapes/b2ChainShape.h" -#include "Box2D/Collision/Shapes/b2EdgeShape.h" -#include + #include -b2ChainShape::~b2ChainShape() -{ - Clear(); -} +#include + +#include "Box2D/Collision/Shapes/b2EdgeShape.h" + +b2ChainShape::~b2ChainShape() { Clear(); } void b2ChainShape::Clear() { - b2Free(m_vertices); - m_vertices = nullptr; - m_count = 0; + b2Free(m_vertices); + m_vertices = nullptr; + m_count = 0; } -void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count) +void b2ChainShape::CreateLoop(const b2Vec2 * vertices, int32 count) { - b2Assert(m_vertices == nullptr && m_count == 0); - b2Assert(count >= 3); - if (count < 3) - { - return; - } - - for (int32 i = 1; i < count; ++i) - { - b2Vec2 v1 = vertices[i-1]; - b2Vec2 v2 = vertices[i]; - // If the code crashes here, it means your vertices are too close together. - b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop); - } - - m_count = count + 1; - m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - memcpy(m_vertices, vertices, count * sizeof(b2Vec2)); - m_vertices[count] = m_vertices[0]; - m_prevVertex = m_vertices[m_count - 2]; - m_nextVertex = m_vertices[1]; - m_hasPrevVertex = true; - m_hasNextVertex = true; + b2Assert(m_vertices == nullptr && m_count == 0); + b2Assert(count >= 3); + if (count < 3) { + return; + } + + for (int32 i = 1; i < count; ++i) { + b2Vec2 v1 = vertices[i - 1]; + b2Vec2 v2 = vertices[i]; + // If the code crashes here, it means your vertices are too close together. + b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop); + } + + m_count = count + 1; + m_vertices = (b2Vec2 *)b2Alloc(m_count * sizeof(b2Vec2)); + memcpy(m_vertices, vertices, count * sizeof(b2Vec2)); + m_vertices[count] = m_vertices[0]; + m_prevVertex = m_vertices[m_count - 2]; + m_nextVertex = m_vertices[1]; + m_hasPrevVertex = true; + m_hasNextVertex = true; } -void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count) +void b2ChainShape::CreateChain(const b2Vec2 * vertices, int32 count) { - b2Assert(m_vertices == nullptr && m_count == 0); - b2Assert(count >= 2); - for (int32 i = 1; i < count; ++i) - { - // If the code crashes here, it means your vertices are too close together. - b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop); - } - - m_count = count; - m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2)); - memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); - - m_hasPrevVertex = false; - m_hasNextVertex = false; - - m_prevVertex.SetZero(); - m_nextVertex.SetZero(); + b2Assert(m_vertices == nullptr && m_count == 0); + b2Assert(count >= 2); + for (int32 i = 1; i < count; ++i) { + // If the code crashes here, it means your vertices are too close together. + b2Assert(b2DistanceSquared(vertices[i - 1], vertices[i]) > b2_linearSlop * b2_linearSlop); + } + + m_count = count; + m_vertices = (b2Vec2 *)b2Alloc(count * sizeof(b2Vec2)); + memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2)); + + m_hasPrevVertex = false; + m_hasNextVertex = false; + + m_prevVertex.SetZero(); + m_nextVertex.SetZero(); } -void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex) +void b2ChainShape::SetPrevVertex(const b2Vec2 & prevVertex) { - m_prevVertex = prevVertex; - m_hasPrevVertex = true; + m_prevVertex = prevVertex; + m_hasPrevVertex = true; } -void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex) +void b2ChainShape::SetNextVertex(const b2Vec2 & nextVertex) { - m_nextVertex = nextVertex; - m_hasNextVertex = true; + m_nextVertex = nextVertex; + m_hasNextVertex = true; } -b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const +b2Shape * b2ChainShape::Clone(b2BlockAllocator * allocator) const { - void* mem = allocator->Allocate(sizeof(b2ChainShape)); - b2ChainShape* clone = new (mem) b2ChainShape; - clone->CreateChain(m_vertices, m_count); - clone->m_prevVertex = m_prevVertex; - clone->m_nextVertex = m_nextVertex; - clone->m_hasPrevVertex = m_hasPrevVertex; - clone->m_hasNextVertex = m_hasNextVertex; - return clone; + void * mem = allocator->Allocate(sizeof(b2ChainShape)); + b2ChainShape * clone = new (mem) b2ChainShape; + clone->CreateChain(m_vertices, m_count); + clone->m_prevVertex = m_prevVertex; + clone->m_nextVertex = m_nextVertex; + clone->m_hasPrevVertex = m_hasPrevVertex; + clone->m_hasNextVertex = m_hasNextVertex; + return clone; } int32 b2ChainShape::GetChildCount() const { - // edge count = vertex count - 1 - return m_count - 1; + // edge count = vertex count - 1 + return m_count - 1; } -void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const +void b2ChainShape::GetChildEdge(b2EdgeShape * edge, int32 index) const { - b2Assert(0 <= index && index < m_count - 1); - edge->m_type = b2Shape::e_edge; - edge->m_radius = m_radius; - - edge->m_vertex1 = m_vertices[index + 0]; - edge->m_vertex2 = m_vertices[index + 1]; - - if (index > 0) - { - edge->m_vertex0 = m_vertices[index - 1]; - edge->m_hasVertex0 = true; - } - else - { - edge->m_vertex0 = m_prevVertex; - edge->m_hasVertex0 = m_hasPrevVertex; - } - - if (index < m_count - 2) - { - edge->m_vertex3 = m_vertices[index + 2]; - edge->m_hasVertex3 = true; - } - else - { - edge->m_vertex3 = m_nextVertex; - edge->m_hasVertex3 = m_hasNextVertex; - } + b2Assert(0 <= index && index < m_count - 1); + edge->m_type = b2Shape::e_edge; + edge->m_radius = m_radius; + + edge->m_vertex1 = m_vertices[index + 0]; + edge->m_vertex2 = m_vertices[index + 1]; + + if (index > 0) { + edge->m_vertex0 = m_vertices[index - 1]; + edge->m_hasVertex0 = true; + } else { + edge->m_vertex0 = m_prevVertex; + edge->m_hasVertex0 = m_hasPrevVertex; + } + + if (index < m_count - 2) { + edge->m_vertex3 = m_vertices[index + 2]; + edge->m_hasVertex3 = true; + } else { + edge->m_vertex3 = m_nextVertex; + edge->m_hasVertex3 = m_hasNextVertex; + } } -bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +bool b2ChainShape::TestPoint(const b2Transform & xf, const b2Vec2 & p) const { - B2_NOT_USED(xf); - B2_NOT_USED(p); - return false; + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; } -bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& xf, int32 childIndex) const +bool b2ChainShape::RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, const b2Transform & xf, + int32 childIndex) const { - b2Assert(childIndex < m_count); + b2Assert(childIndex < m_count); - b2EdgeShape edgeShape; + b2EdgeShape edgeShape; - int32 i1 = childIndex; - int32 i2 = childIndex + 1; - if (i2 == m_count) - { - i2 = 0; - } + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) { + i2 = 0; + } - edgeShape.m_vertex1 = m_vertices[i1]; - edgeShape.m_vertex2 = m_vertices[i2]; + edgeShape.m_vertex1 = m_vertices[i1]; + edgeShape.m_vertex2 = m_vertices[i2]; - return edgeShape.RayCast(output, input, xf, 0); + return edgeShape.RayCast(output, input, xf, 0); } -void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +void b2ChainShape::ComputeAABB(b2AABB * aabb, const b2Transform & xf, int32 childIndex) const { - b2Assert(childIndex < m_count); + b2Assert(childIndex < m_count); - int32 i1 = childIndex; - int32 i2 = childIndex + 1; - if (i2 == m_count) - { - i2 = 0; - } + int32 i1 = childIndex; + int32 i2 = childIndex + 1; + if (i2 == m_count) { + i2 = 0; + } - b2Vec2 v1 = b2Mul(xf, m_vertices[i1]); - b2Vec2 v2 = b2Mul(xf, m_vertices[i2]); + b2Vec2 v1 = b2Mul(xf, m_vertices[i1]); + b2Vec2 v2 = b2Mul(xf, m_vertices[i2]); - aabb->lowerBound = b2Min(v1, v2); - aabb->upperBound = b2Max(v1, v2); + aabb->lowerBound = b2Min(v1, v2); + aabb->upperBound = b2Max(v1, v2); } -void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const +void b2ChainShape::ComputeMass(b2MassData * massData, float32 density) const { - B2_NOT_USED(density); + B2_NOT_USED(density); - massData->mass = 0.0f; - massData->center.SetZero(); - massData->I = 0.0f; + massData->mass = 0.0f; + massData->center.SetZero(); + massData->I = 0.0f; } diff --git a/flatland_box2d/src/Collision/Shapes/b2CircleShape.cpp b/flatland_box2d/src/Collision/Shapes/b2CircleShape.cpp index ef76e9a9..b20bd2dd 100644 --- a/flatland_box2d/src/Collision/Shapes/b2CircleShape.cpp +++ b/flatland_box2d/src/Collision/Shapes/b2CircleShape.cpp @@ -1,99 +1,97 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Collision/Shapes/b2CircleShape.h" + #include -b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const +b2Shape * b2CircleShape::Clone(b2BlockAllocator * allocator) const { - void* mem = allocator->Allocate(sizeof(b2CircleShape)); - b2CircleShape* clone = new (mem) b2CircleShape; - *clone = *this; - return clone; + void * mem = allocator->Allocate(sizeof(b2CircleShape)); + b2CircleShape * clone = new (mem) b2CircleShape; + *clone = *this; + return clone; } -int32 b2CircleShape::GetChildCount() const -{ - return 1; -} +int32 b2CircleShape::GetChildCount() const { return 1; } -bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const +bool b2CircleShape::TestPoint(const b2Transform & transform, const b2Vec2 & p) const { - b2Vec2 center = transform.p + b2Mul(transform.q, m_p); - b2Vec2 d = p - center; - return b2Dot(d, d) <= m_radius * m_radius; + b2Vec2 center = transform.p + b2Mul(transform.q, m_p); + b2Vec2 d = p - center; + return b2Dot(d, d) <= m_radius * m_radius; } // Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius -bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& transform, int32 childIndex) const +bool b2CircleShape::RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, const b2Transform & transform, + int32 childIndex) const { - B2_NOT_USED(childIndex); - - b2Vec2 position = transform.p + b2Mul(transform.q, m_p); - b2Vec2 s = input.p1 - position; - float32 b = b2Dot(s, s) - m_radius * m_radius; - - // Solve quadratic equation. - b2Vec2 r = input.p2 - input.p1; - float32 c = b2Dot(s, r); - float32 rr = b2Dot(r, r); - float32 sigma = c * c - rr * b; - - // Check for negative discriminant and short segment. - if (sigma < 0.0f || rr < b2_epsilon) - { - return false; - } - - // Find the point of intersection of the line with the circle. - float32 a = -(c + b2Sqrt(sigma)); - - // Is the intersection point on the segment? - if (0.0f <= a && a <= input.maxFraction * rr) - { - a /= rr; - output->fraction = a; - output->normal = s + a * r; - output->normal.Normalize(); - return true; - } - - return false; + B2_NOT_USED(childIndex); + + b2Vec2 position = transform.p + b2Mul(transform.q, m_p); + b2Vec2 s = input.p1 - position; + float32 b = b2Dot(s, s) - m_radius * m_radius; + + // Solve quadratic equation. + b2Vec2 r = input.p2 - input.p1; + float32 c = b2Dot(s, r); + float32 rr = b2Dot(r, r); + float32 sigma = c * c - rr * b; + + // Check for negative discriminant and short segment. + if (sigma < 0.0f || rr < b2_epsilon) { + return false; + } + + // Find the point of intersection of the line with the circle. + float32 a = -(c + b2Sqrt(sigma)); + + // Is the intersection point on the segment? + if (0.0f <= a && a <= input.maxFraction * rr) { + a /= rr; + output->fraction = a; + output->normal = s + a * r; + output->normal.Normalize(); + return true; + } + + return false; } -void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const +void b2CircleShape::ComputeAABB( + b2AABB * aabb, const b2Transform & transform, int32 childIndex) const { - B2_NOT_USED(childIndex); + B2_NOT_USED(childIndex); - b2Vec2 p = transform.p + b2Mul(transform.q, m_p); - aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius); - aabb->upperBound.Set(p.x + m_radius, p.y + m_radius); + b2Vec2 p = transform.p + b2Mul(transform.q, m_p); + aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius); + aabb->upperBound.Set(p.x + m_radius, p.y + m_radius); } -void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const +void b2CircleShape::ComputeMass(b2MassData * massData, float32 density) const { - massData->mass = density * b2_pi * m_radius * m_radius; - massData->center = m_p; + massData->mass = density * b2_pi * m_radius * m_radius; + massData->center = m_p; - // inertia about the local origin - massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p)); + // inertia about the local origin + massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p)); } diff --git a/flatland_box2d/src/Collision/Shapes/b2EdgeShape.cpp b/flatland_box2d/src/Collision/Shapes/b2EdgeShape.cpp index d106d1cc..f926b8f8 100644 --- a/flatland_box2d/src/Collision/Shapes/b2EdgeShape.cpp +++ b/flatland_box2d/src/Collision/Shapes/b2EdgeShape.cpp @@ -1,138 +1,130 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Collision/Shapes/b2EdgeShape.h" + #include -void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2) +void b2EdgeShape::Set(const b2Vec2 & v1, const b2Vec2 & v2) { - m_vertex1 = v1; - m_vertex2 = v2; - m_hasVertex0 = false; - m_hasVertex3 = false; + m_vertex1 = v1; + m_vertex2 = v2; + m_hasVertex0 = false; + m_hasVertex3 = false; } -b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const +b2Shape * b2EdgeShape::Clone(b2BlockAllocator * allocator) const { - void* mem = allocator->Allocate(sizeof(b2EdgeShape)); - b2EdgeShape* clone = new (mem) b2EdgeShape; - *clone = *this; - return clone; + void * mem = allocator->Allocate(sizeof(b2EdgeShape)); + b2EdgeShape * clone = new (mem) b2EdgeShape; + *clone = *this; + return clone; } -int32 b2EdgeShape::GetChildCount() const -{ - return 1; -} +int32 b2EdgeShape::GetChildCount() const { return 1; } -bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +bool b2EdgeShape::TestPoint(const b2Transform & xf, const b2Vec2 & p) const { - B2_NOT_USED(xf); - B2_NOT_USED(p); - return false; + B2_NOT_USED(xf); + B2_NOT_USED(p); + return false; } // p = p1 + t * d // v = v1 + s * e // p1 + t * d = v1 + s * e // s * e - t * d = p1 - v1 -bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& xf, int32 childIndex) const +bool b2EdgeShape::RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, const b2Transform & xf, + int32 childIndex) const { - B2_NOT_USED(childIndex); - - // Put the ray into the edge's frame of reference. - b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); - b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); - b2Vec2 d = p2 - p1; - - b2Vec2 v1 = m_vertex1; - b2Vec2 v2 = m_vertex2; - b2Vec2 e = v2 - v1; - b2Vec2 normal(e.y, -e.x); - normal.Normalize(); - - // q = p1 + t * d - // dot(normal, q - v1) = 0 - // dot(normal, p1 - v1) + t * dot(normal, d) = 0 - float32 numerator = b2Dot(normal, v1 - p1); - float32 denominator = b2Dot(normal, d); - - if (denominator == 0.0f) - { - return false; - } - - float32 t = numerator / denominator; - if (t < 0.0f || input.maxFraction < t) - { - return false; - } - - b2Vec2 q = p1 + t * d; - - // q = v1 + s * r - // s = dot(q - v1, r) / dot(r, r) - b2Vec2 r = v2 - v1; - float32 rr = b2Dot(r, r); - if (rr == 0.0f) - { - return false; - } - - float32 s = b2Dot(q - v1, r) / rr; - if (s < 0.0f || 1.0f < s) - { - return false; - } - - output->fraction = t; - if (numerator > 0.0f) - { - output->normal = -b2Mul(xf.q, normal); - } - else - { - output->normal = b2Mul(xf.q, normal); - } - return true; + B2_NOT_USED(childIndex); + + // Put the ray into the edge's frame of reference. + b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); + b2Vec2 d = p2 - p1; + + b2Vec2 v1 = m_vertex1; + b2Vec2 v2 = m_vertex2; + b2Vec2 e = v2 - v1; + b2Vec2 normal(e.y, -e.x); + normal.Normalize(); + + // q = p1 + t * d + // dot(normal, q - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float32 numerator = b2Dot(normal, v1 - p1); + float32 denominator = b2Dot(normal, d); + + if (denominator == 0.0f) { + return false; + } + + float32 t = numerator / denominator; + if (t < 0.0f || input.maxFraction < t) { + return false; + } + + b2Vec2 q = p1 + t * d; + + // q = v1 + s * r + // s = dot(q - v1, r) / dot(r, r) + b2Vec2 r = v2 - v1; + float32 rr = b2Dot(r, r); + if (rr == 0.0f) { + return false; + } + + float32 s = b2Dot(q - v1, r) / rr; + if (s < 0.0f || 1.0f < s) { + return false; + } + + output->fraction = t; + if (numerator > 0.0f) { + output->normal = -b2Mul(xf.q, normal); + } else { + output->normal = b2Mul(xf.q, normal); + } + return true; } -void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +void b2EdgeShape::ComputeAABB(b2AABB * aabb, const b2Transform & xf, int32 childIndex) const { - B2_NOT_USED(childIndex); + B2_NOT_USED(childIndex); - b2Vec2 v1 = b2Mul(xf, m_vertex1); - b2Vec2 v2 = b2Mul(xf, m_vertex2); + b2Vec2 v1 = b2Mul(xf, m_vertex1); + b2Vec2 v2 = b2Mul(xf, m_vertex2); - b2Vec2 lower = b2Min(v1, v2); - b2Vec2 upper = b2Max(v1, v2); + b2Vec2 lower = b2Min(v1, v2); + b2Vec2 upper = b2Max(v1, v2); - b2Vec2 r(m_radius, m_radius); - aabb->lowerBound = lower - r; - aabb->upperBound = upper + r; + b2Vec2 r(m_radius, m_radius); + aabb->lowerBound = lower - r; + aabb->upperBound = upper + r; } -void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const +void b2EdgeShape::ComputeMass(b2MassData * massData, float32 density) const { - B2_NOT_USED(density); + B2_NOT_USED(density); - massData->mass = 0.0f; - massData->center = 0.5f * (m_vertex1 + m_vertex2); - massData->I = 0.0f; + massData->mass = 0.0f; + massData->center = 0.5f * (m_vertex1 + m_vertex2); + massData->I = 0.0f; } diff --git a/flatland_box2d/src/Collision/Shapes/b2PolygonShape.cpp b/flatland_box2d/src/Collision/Shapes/b2PolygonShape.cpp index 68a76af4..a262963a 100644 --- a/flatland_box2d/src/Collision/Shapes/b2PolygonShape.cpp +++ b/flatland_box2d/src/Collision/Shapes/b2PolygonShape.cpp @@ -1,86 +1,84 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Collision/Shapes/b2PolygonShape.h" + #include -b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const +b2Shape * b2PolygonShape::Clone(b2BlockAllocator * allocator) const { - void* mem = allocator->Allocate(sizeof(b2PolygonShape)); - b2PolygonShape* clone = new (mem) b2PolygonShape; - *clone = *this; - return clone; + void * mem = allocator->Allocate(sizeof(b2PolygonShape)); + b2PolygonShape * clone = new (mem) b2PolygonShape; + *clone = *this; + return clone; } void b2PolygonShape::SetAsBox(float32 hx, float32 hy) { - m_count = 4; - m_vertices[0].Set(-hx, -hy); - m_vertices[1].Set( hx, -hy); - m_vertices[2].Set( hx, hy); - m_vertices[3].Set(-hx, hy); - m_normals[0].Set(0.0f, -1.0f); - m_normals[1].Set(1.0f, 0.0f); - m_normals[2].Set(0.0f, 1.0f); - m_normals[3].Set(-1.0f, 0.0f); - m_centroid.SetZero(); + m_count = 4; + m_vertices[0].Set(-hx, -hy); + m_vertices[1].Set(hx, -hy); + m_vertices[2].Set(hx, hy); + m_vertices[3].Set(-hx, hy); + m_normals[0].Set(0.0f, -1.0f); + m_normals[1].Set(1.0f, 0.0f); + m_normals[2].Set(0.0f, 1.0f); + m_normals[3].Set(-1.0f, 0.0f); + m_centroid.SetZero(); } -void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle) +void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2 & center, float32 angle) { - m_count = 4; - m_vertices[0].Set(-hx, -hy); - m_vertices[1].Set( hx, -hy); - m_vertices[2].Set( hx, hy); - m_vertices[3].Set(-hx, hy); - m_normals[0].Set(0.0f, -1.0f); - m_normals[1].Set(1.0f, 0.0f); - m_normals[2].Set(0.0f, 1.0f); - m_normals[3].Set(-1.0f, 0.0f); - m_centroid = center; - - b2Transform xf; - xf.p = center; - xf.q.Set(angle); - - // Transform vertices and normals. - for (int32 i = 0; i < m_count; ++i) - { - m_vertices[i] = b2Mul(xf, m_vertices[i]); - m_normals[i] = b2Mul(xf.q, m_normals[i]); - } + m_count = 4; + m_vertices[0].Set(-hx, -hy); + m_vertices[1].Set(hx, -hy); + m_vertices[2].Set(hx, hy); + m_vertices[3].Set(-hx, hy); + m_normals[0].Set(0.0f, -1.0f); + m_normals[1].Set(1.0f, 0.0f); + m_normals[2].Set(0.0f, 1.0f); + m_normals[3].Set(-1.0f, 0.0f); + m_centroid = center; + + b2Transform xf; + xf.p = center; + xf.q.Set(angle); + + // Transform vertices and normals. + for (int32 i = 0; i < m_count; ++i) { + m_vertices[i] = b2Mul(xf, m_vertices[i]); + m_normals[i] = b2Mul(xf.q, m_normals[i]); + } } -int32 b2PolygonShape::GetChildCount() const -{ - return 1; -} +int32 b2PolygonShape::GetChildCount() const { return 1; } -static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) +static b2Vec2 ComputeCentroid(const b2Vec2 * vs, int32 count) { - b2Assert(count >= 3); + b2Assert(count >= 3); - b2Vec2 c; c.Set(0.0f, 0.0f); - float32 area = 0.0f; + b2Vec2 c; + c.Set(0.0f, 0.0f); + float32 area = 0.0f; - // pRef is the reference point for forming triangles. - // It's location doesn't change the result (except for rounding error). - b2Vec2 pRef(0.0f, 0.0f); + // pRef is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + b2Vec2 pRef(0.0f, 0.0f); #if 0 // This code would put the reference point inside the polygon. for (int32 i = 0; i < count; ++i) @@ -90,379 +88,345 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count) pRef *= 1.0f / count; #endif - const float32 inv3 = 1.0f / 3.0f; + const float32 inv3 = 1.0f / 3.0f; - for (int32 i = 0; i < count; ++i) - { - // Triangle vertices. - b2Vec2 p1 = pRef; - b2Vec2 p2 = vs[i]; - b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0]; + for (int32 i = 0; i < count; ++i) { + // Triangle vertices. + b2Vec2 p1 = pRef; + b2Vec2 p2 = vs[i]; + b2Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0]; - b2Vec2 e1 = p2 - p1; - b2Vec2 e2 = p3 - p1; + b2Vec2 e1 = p2 - p1; + b2Vec2 e2 = p3 - p1; - float32 D = b2Cross(e1, e2); + float32 D = b2Cross(e1, e2); - float32 triangleArea = 0.5f * D; - area += triangleArea; + float32 triangleArea = 0.5f * D; + area += triangleArea; - // Area weighted centroid - c += triangleArea * inv3 * (p1 + p2 + p3); - } + // Area weighted centroid + c += triangleArea * inv3 * (p1 + p2 + p3); + } - // Centroid - b2Assert(area > b2_epsilon); - c *= 1.0f / area; - return c; + // Centroid + b2Assert(area > b2_epsilon); + c *= 1.0f / area; + return c; } -void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) +void b2PolygonShape::Set(const b2Vec2 * vertices, int32 count) { - b2Assert(3 <= count && count <= b2_maxPolygonVertices); - if (count < 3) - { - SetAsBox(1.0f, 1.0f); - return; - } - - int32 n = b2Min(count, b2_maxPolygonVertices); - - // Perform welding and copy vertices into local buffer. - b2Vec2 ps[b2_maxPolygonVertices]; - int32 tempCount = 0; - for (int32 i = 0; i < n; ++i) - { - b2Vec2 v = vertices[i]; - - bool unique = true; - for (int32 j = 0; j < tempCount; ++j) - { - if (b2DistanceSquared(v, ps[j]) < ((0.5f * b2_linearSlop) * (0.5f * b2_linearSlop))) - { - unique = false; - break; - } - } - - if (unique) - { - ps[tempCount++] = v; - } - } - - n = tempCount; - if (n < 3) - { - // Polygon is degenerate. - b2Assert(false); - SetAsBox(1.0f, 1.0f); - return; - } - - // Create the convex hull using the Gift wrapping algorithm - // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm - - // Find the right most point on the hull - int32 i0 = 0; - float32 x0 = ps[0].x; - for (int32 i = 1; i < n; ++i) - { - float32 x = ps[i].x; - if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) - { - i0 = i; - x0 = x; - } - } - - int32 hull[b2_maxPolygonVertices]; - int32 m = 0; - int32 ih = i0; - - for (;;) - { - b2Assert(m < b2_maxPolygonVertices); - hull[m] = ih; - - int32 ie = 0; - for (int32 j = 1; j < n; ++j) - { - if (ie == ih) - { - ie = j; - continue; - } - - b2Vec2 r = ps[ie] - ps[hull[m]]; - b2Vec2 v = ps[j] - ps[hull[m]]; - float32 c = b2Cross(r, v); - if (c < 0.0f) - { - ie = j; - } - - // Collinearity check - if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) - { - ie = j; - } - } - - ++m; - ih = ie; - - if (ie == i0) - { - break; - } - } - - if (m < 3) - { - // Polygon is degenerate. - b2Assert(false); - SetAsBox(1.0f, 1.0f); - return; - } - - m_count = m; - - // Copy vertices. - for (int32 i = 0; i < m; ++i) - { - m_vertices[i] = ps[hull[i]]; - } - - // Compute normals. Ensure the edges have non-zero length. - for (int32 i = 0; i < m; ++i) - { - int32 i1 = i; - int32 i2 = i + 1 < m ? i + 1 : 0; - b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; - b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); - m_normals[i] = b2Cross(edge, 1.0f); - m_normals[i].Normalize(); - } - - // Compute the polygon centroid. - m_centroid = ComputeCentroid(m_vertices, m); + b2Assert(3 <= count && count <= b2_maxPolygonVertices); + if (count < 3) { + SetAsBox(1.0f, 1.0f); + return; + } + + int32 n = b2Min(count, b2_maxPolygonVertices); + + // Perform welding and copy vertices into local buffer. + b2Vec2 ps[b2_maxPolygonVertices]; + int32 tempCount = 0; + for (int32 i = 0; i < n; ++i) { + b2Vec2 v = vertices[i]; + + bool unique = true; + for (int32 j = 0; j < tempCount; ++j) { + if (b2DistanceSquared(v, ps[j]) < ((0.5f * b2_linearSlop) * (0.5f * b2_linearSlop))) { + unique = false; + break; + } + } + + if (unique) { + ps[tempCount++] = v; + } + } + + n = tempCount; + if (n < 3) { + // Polygon is degenerate. + b2Assert(false); + SetAsBox(1.0f, 1.0f); + return; + } + + // Create the convex hull using the Gift wrapping algorithm + // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm + + // Find the right most point on the hull + int32 i0 = 0; + float32 x0 = ps[0].x; + for (int32 i = 1; i < n; ++i) { + float32 x = ps[i].x; + if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) { + i0 = i; + x0 = x; + } + } + + int32 hull[b2_maxPolygonVertices]; + int32 m = 0; + int32 ih = i0; + + for (;;) { + b2Assert(m < b2_maxPolygonVertices); + hull[m] = ih; + + int32 ie = 0; + for (int32 j = 1; j < n; ++j) { + if (ie == ih) { + ie = j; + continue; + } + + b2Vec2 r = ps[ie] - ps[hull[m]]; + b2Vec2 v = ps[j] - ps[hull[m]]; + float32 c = b2Cross(r, v); + if (c < 0.0f) { + ie = j; + } + + // Collinearity check + if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) { + ie = j; + } + } + + ++m; + ih = ie; + + if (ie == i0) { + break; + } + } + + if (m < 3) { + // Polygon is degenerate. + b2Assert(false); + SetAsBox(1.0f, 1.0f); + return; + } + + m_count = m; + + // Copy vertices. + for (int32 i = 0; i < m; ++i) { + m_vertices[i] = ps[hull[i]]; + } + + // Compute normals. Ensure the edges have non-zero length. + for (int32 i = 0; i < m; ++i) { + int32 i1 = i; + int32 i2 = i + 1 < m ? i + 1 : 0; + b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; + b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); + m_normals[i] = b2Cross(edge, 1.0f); + m_normals[i].Normalize(); + } + + // Compute the polygon centroid. + m_centroid = ComputeCentroid(m_vertices, m); } -bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const +bool b2PolygonShape::TestPoint(const b2Transform & xf, const b2Vec2 & p) const { - b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); + b2Vec2 pLocal = b2MulT(xf.q, p - xf.p); - for (int32 i = 0; i < m_count; ++i) - { - float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); - if (dot > 0.0f) - { - return false; - } - } + for (int32 i = 0; i < m_count; ++i) { + float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]); + if (dot > 0.0f) { + return false; + } + } - return true; + return true; } -bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, - const b2Transform& xf, int32 childIndex) const +bool b2PolygonShape::RayCast( + b2RayCastOutput * output, const b2RayCastInput & input, const b2Transform & xf, + int32 childIndex) const { - B2_NOT_USED(childIndex); - - // Put the ray into the polygon's frame of reference. - b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); - b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); - b2Vec2 d = p2 - p1; - - float32 lower = 0.0f, upper = input.maxFraction; - - int32 index = -1; - - for (int32 i = 0; i < m_count; ++i) - { - // p = p1 + a * d - // dot(normal, p - v) = 0 - // dot(normal, p1 - v) + a * dot(normal, d) = 0 - float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); - float32 denominator = b2Dot(m_normals[i], d); - - if (denominator == 0.0f) - { - if (numerator < 0.0f) - { - return false; - } - } - else - { - // Note: we want this predicate without division: - // lower < numerator / denominator, where denominator < 0 - // Since denominator < 0, we have to flip the inequality: - // lower < numerator / denominator <==> denominator * lower > numerator. - if (denominator < 0.0f && numerator < lower * denominator) - { - // Increase lower. - // The segment enters this half-space. - lower = numerator / denominator; - index = i; - } - else if (denominator > 0.0f && numerator < upper * denominator) - { - // Decrease upper. - // The segment exits this half-space. - upper = numerator / denominator; - } - } - - // The use of epsilon here causes the assert on lower to trip - // in some cases. Apparently the use of epsilon was to make edge - // shapes work, but now those are handled separately. - //if (upper < lower - b2_epsilon) - if (upper < lower) - { - return false; - } - } - - b2Assert(0.0f <= lower && lower <= input.maxFraction); - - if (index >= 0) - { - output->fraction = lower; - output->normal = b2Mul(xf.q, m_normals[index]); - return true; - } - - return false; + B2_NOT_USED(childIndex); + + // Put the ray into the polygon's frame of reference. + b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p); + b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p); + b2Vec2 d = p2 - p1; + + float32 lower = 0.0f, upper = input.maxFraction; + + int32 index = -1; + + for (int32 i = 0; i < m_count; ++i) { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1); + float32 denominator = b2Dot(m_normals[i], d); + + if (denominator == 0.0f) { + if (numerator < 0.0f) { + return false; + } + } else { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if (denominator < 0.0f && numerator < lower * denominator) { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } else if (denominator > 0.0f && numerator < upper * denominator) { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + // The use of epsilon here causes the assert on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + // if (upper < lower - b2_epsilon) + if (upper < lower) { + return false; + } + } + + b2Assert(0.0f <= lower && lower <= input.maxFraction); + + if (index >= 0) { + output->fraction = lower; + output->normal = b2Mul(xf.q, m_normals[index]); + return true; + } + + return false; } -void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const +void b2PolygonShape::ComputeAABB(b2AABB * aabb, const b2Transform & xf, int32 childIndex) const { - B2_NOT_USED(childIndex); + B2_NOT_USED(childIndex); - b2Vec2 lower = b2Mul(xf, m_vertices[0]); - b2Vec2 upper = lower; + b2Vec2 lower = b2Mul(xf, m_vertices[0]); + b2Vec2 upper = lower; - for (int32 i = 1; i < m_count; ++i) - { - b2Vec2 v = b2Mul(xf, m_vertices[i]); - lower = b2Min(lower, v); - upper = b2Max(upper, v); - } + for (int32 i = 1; i < m_count; ++i) { + b2Vec2 v = b2Mul(xf, m_vertices[i]); + lower = b2Min(lower, v); + upper = b2Max(upper, v); + } - b2Vec2 r(m_radius, m_radius); - aabb->lowerBound = lower - r; - aabb->upperBound = upper + r; + b2Vec2 r(m_radius, m_radius); + aabb->lowerBound = lower - r; + aabb->upperBound = upper + r; } -void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const +void b2PolygonShape::ComputeMass(b2MassData * massData, float32 density) const { - // Polygon mass, centroid, and inertia. - // Let rho be the polygon density in mass per unit area. - // Then: - // mass = rho * int(dA) - // centroid.x = (1/mass) * rho * int(x * dA) - // centroid.y = (1/mass) * rho * int(y * dA) - // I = rho * int((x*x + y*y) * dA) - // - // We can compute these integrals by summing all the integrals - // for each triangle of the polygon. To evaluate the integral - // for a single triangle, we make a change of variables to - // the (u,v) coordinates of the triangle: - // x = x0 + e1x * u + e2x * v - // y = y0 + e1y * u + e2y * v - // where 0 <= u && 0 <= v && u + v <= 1. - // - // We integrate u from [0,1-v] and then v from [0,1]. - // We also need to use the Jacobian of the transformation: - // D = cross(e1, e2) - // - // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) - // - // The rest of the derivation is handled by computer algebra. - - b2Assert(m_count >= 3); - - b2Vec2 center; center.Set(0.0f, 0.0f); - float32 area = 0.0f; - float32 I = 0.0f; - - // s is the reference point for forming triangles. - // It's location doesn't change the result (except for rounding error). - b2Vec2 s(0.0f, 0.0f); - - // This code would put the reference point inside the polygon. - for (int32 i = 0; i < m_count; ++i) - { - s += m_vertices[i]; - } - s *= 1.0f / m_count; - - const float32 k_inv3 = 1.0f / 3.0f; - - for (int32 i = 0; i < m_count; ++i) - { - // Triangle vertices. - b2Vec2 e1 = m_vertices[i] - s; - b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s; - - float32 D = b2Cross(e1, e2); - - float32 triangleArea = 0.5f * D; - area += triangleArea; - - // Area weighted centroid - center += triangleArea * k_inv3 * (e1 + e2); - - float32 ex1 = e1.x, ey1 = e1.y; - float32 ex2 = e2.x, ey2 = e2.y; - - float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2; - float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2; - - I += (0.25f * k_inv3 * D) * (intx2 + inty2); - } - - // Total mass - massData->mass = density * area; - - // Center of mass - b2Assert(area > b2_epsilon); - center *= 1.0f / area; - massData->center = center + s; - - // Inertia tensor relative to the local origin (point s). - massData->I = density * I; - - // Shift to center of mass then to original body origin. - massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.x = (1/mass) * rho * int(x * dA) + // centroid.y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + b2Assert(m_count >= 3); + + b2Vec2 center; + center.Set(0.0f, 0.0f); + float32 area = 0.0f; + float32 I = 0.0f; + + // s is the reference point for forming triangles. + // It's location doesn't change the result (except for rounding error). + b2Vec2 s(0.0f, 0.0f); + + // This code would put the reference point inside the polygon. + for (int32 i = 0; i < m_count; ++i) { + s += m_vertices[i]; + } + s *= 1.0f / m_count; + + const float32 k_inv3 = 1.0f / 3.0f; + + for (int32 i = 0; i < m_count; ++i) { + // Triangle vertices. + b2Vec2 e1 = m_vertices[i] - s; + b2Vec2 e2 = i + 1 < m_count ? m_vertices[i + 1] - s : m_vertices[0] - s; + + float32 D = b2Cross(e1, e2); + + float32 triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid + center += triangleArea * k_inv3 * (e1 + e2); + + float32 ex1 = e1.x, ey1 = e1.y; + float32 ex2 = e2.x, ey2 = e2.y; + + float32 intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; + float32 inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; + + I += (0.25f * k_inv3 * D) * (intx2 + inty2); + } + + // Total mass + massData->mass = density * area; + + // Center of mass + b2Assert(area > b2_epsilon); + center *= 1.0f / area; + massData->center = center + s; + + // Inertia tensor relative to the local origin (point s). + massData->I = density * I; + + // Shift to center of mass then to original body origin. + massData->I += + massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center)); } bool b2PolygonShape::Validate() const { - for (int32 i = 0; i < m_count; ++i) - { - int32 i1 = i; - int32 i2 = i < m_count - 1 ? i1 + 1 : 0; - b2Vec2 p = m_vertices[i1]; - b2Vec2 e = m_vertices[i2] - p; - - for (int32 j = 0; j < m_count; ++j) - { - if (j == i1 || j == i2) - { - continue; - } - - b2Vec2 v = m_vertices[j] - p; - float32 c = b2Cross(e, v); - if (c < 0.0f) - { - return false; - } - } - } - - return true; + for (int32 i = 0; i < m_count; ++i) { + int32 i1 = i; + int32 i2 = i < m_count - 1 ? i1 + 1 : 0; + b2Vec2 p = m_vertices[i1]; + b2Vec2 e = m_vertices[i2] - p; + + for (int32 j = 0; j < m_count; ++j) { + if (j == i1 || j == i2) { + continue; + } + + b2Vec2 v = m_vertices[j] - p; + float32 c = b2Cross(e, v); + if (c < 0.0f) { + return false; + } + } + } + + return true; } diff --git a/flatland_box2d/src/Collision/b2BroadPhase.cpp b/flatland_box2d/src/Collision/b2BroadPhase.cpp index 2a7f816e..3ef90da6 100644 --- a/flatland_box2d/src/Collision/b2BroadPhase.cpp +++ b/flatland_box2d/src/Collision/b2BroadPhase.cpp @@ -1,119 +1,110 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Collision/b2BroadPhase.h" b2BroadPhase::b2BroadPhase() { - m_proxyCount = 0; + m_proxyCount = 0; - m_pairCapacity = 16; - m_pairCount = 0; - m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); + m_pairCapacity = 16; + m_pairCount = 0; + m_pairBuffer = (b2Pair *)b2Alloc(m_pairCapacity * sizeof(b2Pair)); - m_moveCapacity = 16; - m_moveCount = 0; - m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); + m_moveCapacity = 16; + m_moveCount = 0; + m_moveBuffer = (int32 *)b2Alloc(m_moveCapacity * sizeof(int32)); } b2BroadPhase::~b2BroadPhase() { - b2Free(m_moveBuffer); - b2Free(m_pairBuffer); + b2Free(m_moveBuffer); + b2Free(m_pairBuffer); } -int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData) +int32 b2BroadPhase::CreateProxy(const b2AABB & aabb, void * userData) { - int32 proxyId = m_tree.CreateProxy(aabb, userData); - ++m_proxyCount; - BufferMove(proxyId); - return proxyId; + int32 proxyId = m_tree.CreateProxy(aabb, userData); + ++m_proxyCount; + BufferMove(proxyId); + return proxyId; } void b2BroadPhase::DestroyProxy(int32 proxyId) { - UnBufferMove(proxyId); - --m_proxyCount; - m_tree.DestroyProxy(proxyId); + UnBufferMove(proxyId); + --m_proxyCount; + m_tree.DestroyProxy(proxyId); } -void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) +void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB & aabb, const b2Vec2 & displacement) { - bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement); - if (buffer) - { - BufferMove(proxyId); - } + bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement); + if (buffer) { + BufferMove(proxyId); + } } -void b2BroadPhase::TouchProxy(int32 proxyId) -{ - BufferMove(proxyId); -} +void b2BroadPhase::TouchProxy(int32 proxyId) { BufferMove(proxyId); } void b2BroadPhase::BufferMove(int32 proxyId) { - if (m_moveCount == m_moveCapacity) - { - int32* oldBuffer = m_moveBuffer; - m_moveCapacity *= 2; - m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32)); - memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32)); - b2Free(oldBuffer); - } - - m_moveBuffer[m_moveCount] = proxyId; - ++m_moveCount; + if (m_moveCount == m_moveCapacity) { + int32 * oldBuffer = m_moveBuffer; + m_moveCapacity *= 2; + m_moveBuffer = (int32 *)b2Alloc(m_moveCapacity * sizeof(int32)); + memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32)); + b2Free(oldBuffer); + } + + m_moveBuffer[m_moveCount] = proxyId; + ++m_moveCount; } void b2BroadPhase::UnBufferMove(int32 proxyId) { - for (int32 i = 0; i < m_moveCount; ++i) - { - if (m_moveBuffer[i] == proxyId) - { - m_moveBuffer[i] = e_nullProxy; - } - } + for (int32 i = 0; i < m_moveCount; ++i) { + if (m_moveBuffer[i] == proxyId) { + m_moveBuffer[i] = e_nullProxy; + } + } } // This is called from b2DynamicTree::Query when we are gathering pairs. bool b2BroadPhase::QueryCallback(int32 proxyId) { - // A proxy cannot form a pair with itself. - if (proxyId == m_queryProxyId) - { - return true; - } - - // Grow the pair buffer as needed. - if (m_pairCount == m_pairCapacity) - { - b2Pair* oldBuffer = m_pairBuffer; - m_pairCapacity *= 2; - m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair)); - memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair)); - b2Free(oldBuffer); - } - - m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId); - m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId); - ++m_pairCount; - - return true; + // A proxy cannot form a pair with itself. + if (proxyId == m_queryProxyId) { + return true; + } + + // Grow the pair buffer as needed. + if (m_pairCount == m_pairCapacity) { + b2Pair * oldBuffer = m_pairBuffer; + m_pairCapacity *= 2; + m_pairBuffer = (b2Pair *)b2Alloc(m_pairCapacity * sizeof(b2Pair)); + memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair)); + b2Free(oldBuffer); + } + + m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId); + m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId); + ++m_pairCount; + + return true; } diff --git a/flatland_box2d/src/Collision/b2CollideCircle.cpp b/flatland_box2d/src/Collision/b2CollideCircle.cpp index ce26fb49..ef0912ac 100644 --- a/flatland_box2d/src/Collision/b2CollideCircle.cpp +++ b/flatland_box2d/src/Collision/b2CollideCircle.cpp @@ -1,154 +1,139 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ -#include "Box2D/Collision/b2Collision.h" #include "Box2D/Collision/Shapes/b2CircleShape.h" #include "Box2D/Collision/Shapes/b2PolygonShape.h" +#include "Box2D/Collision/b2Collision.h" void b2CollideCircles( - b2Manifold* manifold, - const b2CircleShape* circleA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) + b2Manifold * manifold, const b2CircleShape * circleA, const b2Transform & xfA, + const b2CircleShape * circleB, const b2Transform & xfB) { - manifold->pointCount = 0; - - b2Vec2 pA = b2Mul(xfA, circleA->m_p); - b2Vec2 pB = b2Mul(xfB, circleB->m_p); - - b2Vec2 d = pB - pA; - float32 distSqr = b2Dot(d, d); - float32 rA = circleA->m_radius, rB = circleB->m_radius; - float32 radius = rA + rB; - if (distSqr > radius * radius) - { - return; - } - - manifold->type = b2Manifold::e_circles; - manifold->localPoint = circleA->m_p; - manifold->localNormal.SetZero(); - manifold->pointCount = 1; - - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; + manifold->pointCount = 0; + + b2Vec2 pA = b2Mul(xfA, circleA->m_p); + b2Vec2 pB = b2Mul(xfB, circleB->m_p); + + b2Vec2 d = pB - pA; + float32 distSqr = b2Dot(d, d); + float32 rA = circleA->m_radius, rB = circleB->m_radius; + float32 radius = rA + rB; + if (distSqr > radius * radius) { + return; + } + + manifold->type = b2Manifold::e_circles; + manifold->localPoint = circleA->m_p; + manifold->localNormal.SetZero(); + manifold->pointCount = 1; + + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; } void b2CollidePolygonAndCircle( - b2Manifold* manifold, - const b2PolygonShape* polygonA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) + b2Manifold * manifold, const b2PolygonShape * polygonA, const b2Transform & xfA, + const b2CircleShape * circleB, const b2Transform & xfB) { - manifold->pointCount = 0; - - // Compute circle position in the frame of the polygon. - b2Vec2 c = b2Mul(xfB, circleB->m_p); - b2Vec2 cLocal = b2MulT(xfA, c); - - // Find the min separating edge. - int32 normalIndex = 0; - float32 separation = -b2_maxFloat; - float32 radius = polygonA->m_radius + circleB->m_radius; - int32 vertexCount = polygonA->m_count; - const b2Vec2* vertices = polygonA->m_vertices; - const b2Vec2* normals = polygonA->m_normals; - - for (int32 i = 0; i < vertexCount; ++i) - { - float32 s = b2Dot(normals[i], cLocal - vertices[i]); - - if (s > radius) - { - // Early out. - return; - } - - if (s > separation) - { - separation = s; - normalIndex = i; - } - } - - // Vertices that subtend the incident face. - int32 vertIndex1 = normalIndex; - int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; - b2Vec2 v1 = vertices[vertIndex1]; - b2Vec2 v2 = vertices[vertIndex2]; - - // If the center is inside the polygon ... - if (separation < b2_epsilon) - { - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = normals[normalIndex]; - manifold->localPoint = 0.5f * (v1 + v2); - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - return; - } - - // Compute barycentric coordinates - float32 u1 = b2Dot(cLocal - v1, v2 - v1); - float32 u2 = b2Dot(cLocal - v2, v1 - v2); - if (u1 <= 0.0f) - { - if (b2DistanceSquared(cLocal, v1) > radius * radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = cLocal - v1; - manifold->localNormal.Normalize(); - manifold->localPoint = v1; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } - else if (u2 <= 0.0f) - { - if (b2DistanceSquared(cLocal, v2) > radius * radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = cLocal - v2; - manifold->localNormal.Normalize(); - manifold->localPoint = v2; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } - else - { - b2Vec2 faceCenter = 0.5f * (v1 + v2); - float32 s = b2Dot(cLocal - faceCenter, normals[vertIndex1]); - if (s > radius) - { - return; - } - - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = normals[vertIndex1]; - manifold->localPoint = faceCenter; - manifold->points[0].localPoint = circleB->m_p; - manifold->points[0].id.key = 0; - } + manifold->pointCount = 0; + + // Compute circle position in the frame of the polygon. + b2Vec2 c = b2Mul(xfB, circleB->m_p); + b2Vec2 cLocal = b2MulT(xfA, c); + + // Find the min separating edge. + int32 normalIndex = 0; + float32 separation = -b2_maxFloat; + float32 radius = polygonA->m_radius + circleB->m_radius; + int32 vertexCount = polygonA->m_count; + const b2Vec2 * vertices = polygonA->m_vertices; + const b2Vec2 * normals = polygonA->m_normals; + + for (int32 i = 0; i < vertexCount; ++i) { + float32 s = b2Dot(normals[i], cLocal - vertices[i]); + + if (s > radius) { + // Early out. + return; + } + + if (s > separation) { + separation = s; + normalIndex = i; + } + } + + // Vertices that subtend the incident face. + int32 vertIndex1 = normalIndex; + int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; + b2Vec2 v1 = vertices[vertIndex1]; + b2Vec2 v2 = vertices[vertIndex2]; + + // If the center is inside the polygon ... + if (separation < b2_epsilon) { + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = normals[normalIndex]; + manifold->localPoint = 0.5f * (v1 + v2); + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + return; + } + + // Compute barycentric coordinates + float32 u1 = b2Dot(cLocal - v1, v2 - v1); + float32 u2 = b2Dot(cLocal - v2, v1 - v2); + if (u1 <= 0.0f) { + if (b2DistanceSquared(cLocal, v1) > radius * radius) { + return; + } + + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = cLocal - v1; + manifold->localNormal.Normalize(); + manifold->localPoint = v1; + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + } else if (u2 <= 0.0f) { + if (b2DistanceSquared(cLocal, v2) > radius * radius) { + return; + } + + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = cLocal - v2; + manifold->localNormal.Normalize(); + manifold->localPoint = v2; + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + } else { + b2Vec2 faceCenter = 0.5f * (v1 + v2); + float32 s = b2Dot(cLocal - faceCenter, normals[vertIndex1]); + if (s > radius) { + return; + } + + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = normals[vertIndex1]; + manifold->localPoint = faceCenter; + manifold->points[0].localPoint = circleB->m_p; + manifold->points[0].id.key = 0; + } } diff --git a/flatland_box2d/src/Collision/b2CollideEdge.cpp b/flatland_box2d/src/Collision/b2CollideEdge.cpp index ba060c02..faadedb1 100644 --- a/flatland_box2d/src/Collision/b2CollideEdge.cpp +++ b/flatland_box2d/src/Collision/b2CollideEdge.cpp @@ -16,206 +16,187 @@ * 3. This notice may not be removed or altered from any source distribution. */ -#include "Box2D/Collision/b2Collision.h" #include "Box2D/Collision/Shapes/b2CircleShape.h" #include "Box2D/Collision/Shapes/b2EdgeShape.h" #include "Box2D/Collision/Shapes/b2PolygonShape.h" - +#include "Box2D/Collision/b2Collision.h" // Compute contact points for edge versus circle. // This accounts for edge connectivity. -void b2CollideEdgeAndCircle(b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2CircleShape* circleB, const b2Transform& xfB) +void b2CollideEdgeAndCircle( + b2Manifold * manifold, const b2EdgeShape * edgeA, const b2Transform & xfA, + const b2CircleShape * circleB, const b2Transform & xfB) { - manifold->pointCount = 0; - - // Compute circle in frame of edge - b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); - - b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; - b2Vec2 e = B - A; - - // Barycentric coordinates - float32 u = b2Dot(e, B - Q); - float32 v = b2Dot(e, Q - A); - - float32 radius = edgeA->m_radius + circleB->m_radius; - - b2ContactFeature cf; - cf.indexB = 0; - cf.typeB = b2ContactFeature::e_vertex; - - // Region A - if (v <= 0.0f) - { - b2Vec2 P = A; - b2Vec2 d = Q - P; - float32 dd = b2Dot(d, d); - if (dd > radius * radius) - { - return; - } - - // Is there an edge connected to A? - if (edgeA->m_hasVertex0) - { - b2Vec2 A1 = edgeA->m_vertex0; - b2Vec2 B1 = A; - b2Vec2 e1 = B1 - A1; - float32 u1 = b2Dot(e1, B1 - Q); - - // Is the circle in Region AB of the previous edge? - if (u1 > 0.0f) - { - return; - } - } - - cf.indexA = 0; - cf.typeA = b2ContactFeature::e_vertex; - manifold->pointCount = 1; - manifold->type = b2Manifold::e_circles; - manifold->localNormal.SetZero(); - manifold->localPoint = P; - manifold->points[0].id.key = 0; - manifold->points[0].id.cf = cf; - manifold->points[0].localPoint = circleB->m_p; - return; - } - - // Region B - if (u <= 0.0f) - { - b2Vec2 P = B; - b2Vec2 d = Q - P; - float32 dd = b2Dot(d, d); - if (dd > radius * radius) - { - return; - } - - // Is there an edge connected to B? - if (edgeA->m_hasVertex3) - { - b2Vec2 B2 = edgeA->m_vertex3; - b2Vec2 A2 = B; - b2Vec2 e2 = B2 - A2; - float32 v2 = b2Dot(e2, Q - A2); - - // Is the circle in Region AB of the next edge? - if (v2 > 0.0f) - { - return; - } - } - - cf.indexA = 1; - cf.typeA = b2ContactFeature::e_vertex; - manifold->pointCount = 1; - manifold->type = b2Manifold::e_circles; - manifold->localNormal.SetZero(); - manifold->localPoint = P; - manifold->points[0].id.key = 0; - manifold->points[0].id.cf = cf; - manifold->points[0].localPoint = circleB->m_p; - return; - } - - // Region AB - float32 den = b2Dot(e, e); - b2Assert(den > 0.0f); - b2Vec2 P = (1.0f / den) * (u * A + v * B); - b2Vec2 d = Q - P; - float32 dd = b2Dot(d, d); - if (dd > radius * radius) - { - return; - } - - b2Vec2 n(-e.y, e.x); - if (b2Dot(n, Q - A) < 0.0f) - { - n.Set(-n.x, -n.y); - } - n.Normalize(); - - cf.indexA = 0; - cf.typeA = b2ContactFeature::e_face; - manifold->pointCount = 1; - manifold->type = b2Manifold::e_faceA; - manifold->localNormal = n; - manifold->localPoint = A; - manifold->points[0].id.key = 0; - manifold->points[0].id.cf = cf; - manifold->points[0].localPoint = circleB->m_p; + manifold->pointCount = 0; + + // Compute circle in frame of edge + b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p)); + + b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2; + b2Vec2 e = B - A; + + // Barycentric coordinates + float32 u = b2Dot(e, B - Q); + float32 v = b2Dot(e, Q - A); + + float32 radius = edgeA->m_radius + circleB->m_radius; + + b2ContactFeature cf; + cf.indexB = 0; + cf.typeB = b2ContactFeature::e_vertex; + + // Region A + if (v <= 0.0f) { + b2Vec2 P = A; + b2Vec2 d = Q - P; + float32 dd = b2Dot(d, d); + if (dd > radius * radius) { + return; + } + + // Is there an edge connected to A? + if (edgeA->m_hasVertex0) { + b2Vec2 A1 = edgeA->m_vertex0; + b2Vec2 B1 = A; + b2Vec2 e1 = B1 - A1; + float32 u1 = b2Dot(e1, B1 - Q); + + // Is the circle in Region AB of the previous edge? + if (u1 > 0.0f) { + return; + } + } + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region B + if (u <= 0.0f) { + b2Vec2 P = B; + b2Vec2 d = Q - P; + float32 dd = b2Dot(d, d); + if (dd > radius * radius) { + return; + } + + // Is there an edge connected to B? + if (edgeA->m_hasVertex3) { + b2Vec2 B2 = edgeA->m_vertex3; + b2Vec2 A2 = B; + b2Vec2 e2 = B2 - A2; + float32 v2 = b2Dot(e2, Q - A2); + + // Is the circle in Region AB of the next edge? + if (v2 > 0.0f) { + return; + } + } + + cf.indexA = 1; + cf.typeA = b2ContactFeature::e_vertex; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_circles; + manifold->localNormal.SetZero(); + manifold->localPoint = P; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; + return; + } + + // Region AB + float32 den = b2Dot(e, e); + b2Assert(den > 0.0f); + b2Vec2 P = (1.0f / den) * (u * A + v * B); + b2Vec2 d = Q - P; + float32 dd = b2Dot(d, d); + if (dd > radius * radius) { + return; + } + + b2Vec2 n(-e.y, e.x); + if (b2Dot(n, Q - A) < 0.0f) { + n.Set(-n.x, -n.y); + } + n.Normalize(); + + cf.indexA = 0; + cf.typeA = b2ContactFeature::e_face; + manifold->pointCount = 1; + manifold->type = b2Manifold::e_faceA; + manifold->localNormal = n; + manifold->localPoint = A; + manifold->points[0].id.key = 0; + manifold->points[0].id.cf = cf; + manifold->points[0].localPoint = circleB->m_p; } // This structure is used to keep track of the best separating axis. struct b2EPAxis { - enum Type - { - e_unknown, - e_edgeA, - e_edgeB - }; - - Type type; - int32 index; - float32 separation; + enum Type { e_unknown, e_edgeA, e_edgeB }; + + Type type; + int32 index; + float32 separation; }; // This holds polygon B expressed in frame A. struct b2TempPolygon { - b2Vec2 vertices[b2_maxPolygonVertices]; - b2Vec2 normals[b2_maxPolygonVertices]; - int32 count; + b2Vec2 vertices[b2_maxPolygonVertices]; + b2Vec2 normals[b2_maxPolygonVertices]; + int32 count; }; // Reference face used for clipping struct b2ReferenceFace { - int32 i1, i2; - - b2Vec2 v1, v2; - - b2Vec2 normal; - - b2Vec2 sideNormal1; - float32 sideOffset1; - - b2Vec2 sideNormal2; - float32 sideOffset2; + int32 i1, i2; + + b2Vec2 v1, v2; + + b2Vec2 normal; + + b2Vec2 sideNormal1; + float32 sideOffset1; + + b2Vec2 sideNormal2; + float32 sideOffset2; }; -// This class collides and edge and a polygon, taking into account edge adjacency. +// This class collides and edge and a polygon, taking into account edge +// adjacency. struct b2EPCollider { - void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB); - b2EPAxis ComputeEdgeSeparation(); - b2EPAxis ComputePolygonSeparation(); - - enum VertexType - { - e_isolated, - e_concave, - e_convex - }; - - b2TempPolygon m_polygonB; - - b2Transform m_xf; - b2Vec2 m_centroidB; - b2Vec2 m_v0, m_v1, m_v2, m_v3; - b2Vec2 m_normal0, m_normal1, m_normal2; - b2Vec2 m_normal; - VertexType m_type1, m_type2; - b2Vec2 m_lowerLimit, m_upperLimit; - float32 m_radius; - bool m_front; + void Collide( + b2Manifold * manifold, const b2EdgeShape * edgeA, const b2Transform & xfA, + const b2PolygonShape * polygonB, const b2Transform & xfB); + b2EPAxis ComputeEdgeSeparation(); + b2EPAxis ComputePolygonSeparation(); + + enum VertexType { e_isolated, e_concave, e_convex }; + + b2TempPolygon m_polygonB; + + b2Transform m_xf; + b2Vec2 m_centroidB; + b2Vec2 m_v0, m_v1, m_v2, m_v3; + b2Vec2 m_normal0, m_normal1, m_normal2; + b2Vec2 m_normal; + VertexType m_type1, m_type2; + b2Vec2 m_lowerLimit, m_upperLimit; + float32 m_radius; + bool m_front; }; // Algorithm: @@ -227,472 +208,388 @@ struct b2EPCollider // 6. Visit each separating axes, only accept axes within the range // 7. Return if _any_ axis indicates separation // 8. Clip -void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB) +void b2EPCollider::Collide( + b2Manifold * manifold, const b2EdgeShape * edgeA, const b2Transform & xfA, + const b2PolygonShape * polygonB, const b2Transform & xfB) { - m_xf = b2MulT(xfA, xfB); - - m_centroidB = b2Mul(m_xf, polygonB->m_centroid); - - m_v0 = edgeA->m_vertex0; - m_v1 = edgeA->m_vertex1; - m_v2 = edgeA->m_vertex2; - m_v3 = edgeA->m_vertex3; - - bool hasVertex0 = edgeA->m_hasVertex0; - bool hasVertex3 = edgeA->m_hasVertex3; - - b2Vec2 edge1 = m_v2 - m_v1; - edge1.Normalize(); - m_normal1.Set(edge1.y, -edge1.x); - float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1); - float32 offset0 = 0.0f, offset2 = 0.0f; - bool convex1 = false, convex2 = false; - - // Is there a preceding edge? - if (hasVertex0) - { - b2Vec2 edge0 = m_v1 - m_v0; - edge0.Normalize(); - m_normal0.Set(edge0.y, -edge0.x); - convex1 = b2Cross(edge0, edge1) >= 0.0f; - offset0 = b2Dot(m_normal0, m_centroidB - m_v0); - } - - // Is there a following edge? - if (hasVertex3) - { - b2Vec2 edge2 = m_v3 - m_v2; - edge2.Normalize(); - m_normal2.Set(edge2.y, -edge2.x); - convex2 = b2Cross(edge1, edge2) > 0.0f; - offset2 = b2Dot(m_normal2, m_centroidB - m_v2); - } - - // Determine front or back collision. Determine collision normal limits. - if (hasVertex0 && hasVertex3) - { - if (convex1 && convex2) - { - m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal0; - m_upperLimit = m_normal2; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = -m_normal1; - } - } - else if (convex1) - { - m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal0; - m_upperLimit = m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal2; - m_upperLimit = -m_normal1; - } - } - else if (convex2) - { - m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = m_normal2; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = -m_normal0; - } - } - else - { - m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal2; - m_upperLimit = -m_normal0; - } - } - } - else if (hasVertex0) - { - if (convex1) - { - m_front = offset0 >= 0.0f || offset1 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal0; - m_upperLimit = -m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = -m_normal1; - } - } - else - { - m_front = offset0 >= 0.0f && offset1 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = -m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = -m_normal0; - } - } - } - else if (hasVertex3) - { - if (convex2) - { - m_front = offset1 >= 0.0f || offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = m_normal2; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = m_normal1; - } - } - else - { - m_front = offset1 >= 0.0f && offset2 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = -m_normal2; - m_upperLimit = m_normal1; - } - } - } - else - { - m_front = offset1 >= 0.0f; - if (m_front) - { - m_normal = m_normal1; - m_lowerLimit = -m_normal1; - m_upperLimit = -m_normal1; - } - else - { - m_normal = -m_normal1; - m_lowerLimit = m_normal1; - m_upperLimit = m_normal1; - } - } - - // Get polygonB in frameA - m_polygonB.count = polygonB->m_count; - for (int32 i = 0; i < polygonB->m_count; ++i) - { - m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); - m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]); - } - - m_radius = polygonB->m_radius + edgeA->m_radius; - - manifold->pointCount = 0; - - b2EPAxis edgeAxis = ComputeEdgeSeparation(); - - // If no valid normal can be found than this edge should not collide. - if (edgeAxis.type == b2EPAxis::e_unknown) - { - return; - } - - if (edgeAxis.separation > m_radius) - { - return; - } - - b2EPAxis polygonAxis = ComputePolygonSeparation(); - if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius) - { - return; - } - - // Use hysteresis for jitter reduction. - const float32 k_relativeTol = 0.98f; - const float32 k_absoluteTol = 0.001f; - - b2EPAxis primaryAxis; - if (polygonAxis.type == b2EPAxis::e_unknown) - { - primaryAxis = edgeAxis; - } - else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) - { - primaryAxis = polygonAxis; - } - else - { - primaryAxis = edgeAxis; - } - - b2ClipVertex ie[2]; - b2ReferenceFace rf; - if (primaryAxis.type == b2EPAxis::e_edgeA) - { - manifold->type = b2Manifold::e_faceA; - - // Search for the polygon normal that is most anti-parallel to the edge normal. - int32 bestIndex = 0; - float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]); - for (int32 i = 1; i < m_polygonB.count; ++i) - { - float32 value = b2Dot(m_normal, m_polygonB.normals[i]); - if (value < bestValue) - { - bestValue = value; - bestIndex = i; - } - } - - int32 i1 = bestIndex; - int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; - - ie[0].v = m_polygonB.vertices[i1]; - ie[0].id.cf.indexA = 0; - ie[0].id.cf.indexB = static_cast(i1); - ie[0].id.cf.typeA = b2ContactFeature::e_face; - ie[0].id.cf.typeB = b2ContactFeature::e_vertex; - - ie[1].v = m_polygonB.vertices[i2]; - ie[1].id.cf.indexA = 0; - ie[1].id.cf.indexB = static_cast(i2); - ie[1].id.cf.typeA = b2ContactFeature::e_face; - ie[1].id.cf.typeB = b2ContactFeature::e_vertex; - - if (m_front) - { - rf.i1 = 0; - rf.i2 = 1; - rf.v1 = m_v1; - rf.v2 = m_v2; - rf.normal = m_normal1; - } - else - { - rf.i1 = 1; - rf.i2 = 0; - rf.v1 = m_v2; - rf.v2 = m_v1; - rf.normal = -m_normal1; - } - } - else - { - manifold->type = b2Manifold::e_faceB; - - ie[0].v = m_v1; - ie[0].id.cf.indexA = 0; - ie[0].id.cf.indexB = static_cast(primaryAxis.index); - ie[0].id.cf.typeA = b2ContactFeature::e_vertex; - ie[0].id.cf.typeB = b2ContactFeature::e_face; - - ie[1].v = m_v2; - ie[1].id.cf.indexA = 0; - ie[1].id.cf.indexB = static_cast(primaryAxis.index); - ie[1].id.cf.typeA = b2ContactFeature::e_vertex; - ie[1].id.cf.typeB = b2ContactFeature::e_face; - - rf.i1 = primaryAxis.index; - rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; - rf.v1 = m_polygonB.vertices[rf.i1]; - rf.v2 = m_polygonB.vertices[rf.i2]; - rf.normal = m_polygonB.normals[rf.i1]; - } - - rf.sideNormal1.Set(rf.normal.y, -rf.normal.x); - rf.sideNormal2 = -rf.sideNormal1; - rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1); - rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2); - - // Clip incident edge against extruded edge1 side edges. - b2ClipVertex clipPoints1[2]; - b2ClipVertex clipPoints2[2]; - int32 np; - - // Clip to box side 1 - np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); - - if (np < b2_maxManifoldPoints) - { - return; - } - - // Clip to negative box side 1 - np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); - - if (np < b2_maxManifoldPoints) - { - return; - } - - // Now clipPoints2 contains the clipped points. - if (primaryAxis.type == b2EPAxis::e_edgeA) - { - manifold->localNormal = rf.normal; - manifold->localPoint = rf.v1; - } - else - { - manifold->localNormal = polygonB->m_normals[rf.i1]; - manifold->localPoint = polygonB->m_vertices[rf.i1]; - } - - int32 pointCount = 0; - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - float32 separation; - - separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1); - - if (separation <= m_radius) - { - b2ManifoldPoint* cp = manifold->points + pointCount; - - if (primaryAxis.type == b2EPAxis::e_edgeA) - { - cp->localPoint = b2MulT(m_xf, clipPoints2[i].v); - cp->id = clipPoints2[i].id; - } - else - { - cp->localPoint = clipPoints2[i].v; - cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; - cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; - cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; - cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; - } - - ++pointCount; - } - } - - manifold->pointCount = pointCount; + m_xf = b2MulT(xfA, xfB); + + m_centroidB = b2Mul(m_xf, polygonB->m_centroid); + + m_v0 = edgeA->m_vertex0; + m_v1 = edgeA->m_vertex1; + m_v2 = edgeA->m_vertex2; + m_v3 = edgeA->m_vertex3; + + bool hasVertex0 = edgeA->m_hasVertex0; + bool hasVertex3 = edgeA->m_hasVertex3; + + b2Vec2 edge1 = m_v2 - m_v1; + edge1.Normalize(); + m_normal1.Set(edge1.y, -edge1.x); + float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1); + float32 offset0 = 0.0f, offset2 = 0.0f; + bool convex1 = false, convex2 = false; + + // Is there a preceding edge? + if (hasVertex0) { + b2Vec2 edge0 = m_v1 - m_v0; + edge0.Normalize(); + m_normal0.Set(edge0.y, -edge0.x); + convex1 = b2Cross(edge0, edge1) >= 0.0f; + offset0 = b2Dot(m_normal0, m_centroidB - m_v0); + } + + // Is there a following edge? + if (hasVertex3) { + b2Vec2 edge2 = m_v3 - m_v2; + edge2.Normalize(); + m_normal2.Set(edge2.y, -edge2.x); + convex2 = b2Cross(edge1, edge2) > 0.0f; + offset2 = b2Dot(m_normal2, m_centroidB - m_v2); + } + + // Determine front or back collision. Determine collision normal limits. + if (hasVertex0 && hasVertex3) { + if (convex1 && convex2) { + m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f; + if (m_front) { + m_normal = m_normal1; + m_lowerLimit = m_normal0; + m_upperLimit = m_normal2; + } else { + m_normal = -m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = -m_normal1; + } + } else if (convex1) { + m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); + if (m_front) { + m_normal = m_normal1; + m_lowerLimit = m_normal0; + m_upperLimit = m_normal1; + } else { + m_normal = -m_normal1; + m_lowerLimit = -m_normal2; + m_upperLimit = -m_normal1; + } + } else if (convex2) { + m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); + if (m_front) { + m_normal = m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = m_normal2; + } else { + m_normal = -m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = -m_normal0; + } + } else { + m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; + if (m_front) { + m_normal = m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = m_normal1; + } else { + m_normal = -m_normal1; + m_lowerLimit = -m_normal2; + m_upperLimit = -m_normal0; + } + } + } else if (hasVertex0) { + if (convex1) { + m_front = offset0 >= 0.0f || offset1 >= 0.0f; + if (m_front) { + m_normal = m_normal1; + m_lowerLimit = m_normal0; + m_upperLimit = -m_normal1; + } else { + m_normal = -m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = -m_normal1; + } + } else { + m_front = offset0 >= 0.0f && offset1 >= 0.0f; + if (m_front) { + m_normal = m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = -m_normal1; + } else { + m_normal = -m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = -m_normal0; + } + } + } else if (hasVertex3) { + if (convex2) { + m_front = offset1 >= 0.0f || offset2 >= 0.0f; + if (m_front) { + m_normal = m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = m_normal2; + } else { + m_normal = -m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = m_normal1; + } + } else { + m_front = offset1 >= 0.0f && offset2 >= 0.0f; + if (m_front) { + m_normal = m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = m_normal1; + } else { + m_normal = -m_normal1; + m_lowerLimit = -m_normal2; + m_upperLimit = m_normal1; + } + } + } else { + m_front = offset1 >= 0.0f; + if (m_front) { + m_normal = m_normal1; + m_lowerLimit = -m_normal1; + m_upperLimit = -m_normal1; + } else { + m_normal = -m_normal1; + m_lowerLimit = m_normal1; + m_upperLimit = m_normal1; + } + } + + // Get polygonB in frameA + m_polygonB.count = polygonB->m_count; + for (int32 i = 0; i < polygonB->m_count; ++i) { + m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]); + m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]); + } + + m_radius = polygonB->m_radius + edgeA->m_radius; + + manifold->pointCount = 0; + + b2EPAxis edgeAxis = ComputeEdgeSeparation(); + + // If no valid normal can be found than this edge should not collide. + if (edgeAxis.type == b2EPAxis::e_unknown) { + return; + } + + if (edgeAxis.separation > m_radius) { + return; + } + + b2EPAxis polygonAxis = ComputePolygonSeparation(); + if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius) { + return; + } + + // Use hysteresis for jitter reduction. + const float32 k_relativeTol = 0.98f; + const float32 k_absoluteTol = 0.001f; + + b2EPAxis primaryAxis; + if (polygonAxis.type == b2EPAxis::e_unknown) { + primaryAxis = edgeAxis; + } else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) { + primaryAxis = polygonAxis; + } else { + primaryAxis = edgeAxis; + } + + b2ClipVertex ie[2]; + b2ReferenceFace rf; + if (primaryAxis.type == b2EPAxis::e_edgeA) { + manifold->type = b2Manifold::e_faceA; + + // Search for the polygon normal that is most anti-parallel to the edge + // normal. + int32 bestIndex = 0; + float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]); + for (int32 i = 1; i < m_polygonB.count; ++i) { + float32 value = b2Dot(m_normal, m_polygonB.normals[i]); + if (value < bestValue) { + bestValue = value; + bestIndex = i; + } + } + + int32 i1 = bestIndex; + int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; + + ie[0].v = m_polygonB.vertices[i1]; + ie[0].id.cf.indexA = 0; + ie[0].id.cf.indexB = static_cast(i1); + ie[0].id.cf.typeA = b2ContactFeature::e_face; + ie[0].id.cf.typeB = b2ContactFeature::e_vertex; + + ie[1].v = m_polygonB.vertices[i2]; + ie[1].id.cf.indexA = 0; + ie[1].id.cf.indexB = static_cast(i2); + ie[1].id.cf.typeA = b2ContactFeature::e_face; + ie[1].id.cf.typeB = b2ContactFeature::e_vertex; + + if (m_front) { + rf.i1 = 0; + rf.i2 = 1; + rf.v1 = m_v1; + rf.v2 = m_v2; + rf.normal = m_normal1; + } else { + rf.i1 = 1; + rf.i2 = 0; + rf.v1 = m_v2; + rf.v2 = m_v1; + rf.normal = -m_normal1; + } + } else { + manifold->type = b2Manifold::e_faceB; + + ie[0].v = m_v1; + ie[0].id.cf.indexA = 0; + ie[0].id.cf.indexB = static_cast(primaryAxis.index); + ie[0].id.cf.typeA = b2ContactFeature::e_vertex; + ie[0].id.cf.typeB = b2ContactFeature::e_face; + + ie[1].v = m_v2; + ie[1].id.cf.indexA = 0; + ie[1].id.cf.indexB = static_cast(primaryAxis.index); + ie[1].id.cf.typeA = b2ContactFeature::e_vertex; + ie[1].id.cf.typeB = b2ContactFeature::e_face; + + rf.i1 = primaryAxis.index; + rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; + rf.v1 = m_polygonB.vertices[rf.i1]; + rf.v2 = m_polygonB.vertices[rf.i2]; + rf.normal = m_polygonB.normals[rf.i1]; + } + + rf.sideNormal1.Set(rf.normal.y, -rf.normal.x); + rf.sideNormal2 = -rf.sideNormal1; + rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1); + rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2); + + // Clip incident edge against extruded edge1 side edges. + b2ClipVertex clipPoints1[2]; + b2ClipVertex clipPoints2[2]; + int32 np; + + // Clip to box side 1 + np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1); + + if (np < b2_maxManifoldPoints) { + return; + } + + // Clip to negative box side 1 + np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2); + + if (np < b2_maxManifoldPoints) { + return; + } + + // Now clipPoints2 contains the clipped points. + if (primaryAxis.type == b2EPAxis::e_edgeA) { + manifold->localNormal = rf.normal; + manifold->localPoint = rf.v1; + } else { + manifold->localNormal = polygonB->m_normals[rf.i1]; + manifold->localPoint = polygonB->m_vertices[rf.i1]; + } + + int32 pointCount = 0; + for (int32 i = 0; i < b2_maxManifoldPoints; ++i) { + float32 separation; + + separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1); + + if (separation <= m_radius) { + b2ManifoldPoint * cp = manifold->points + pointCount; + + if (primaryAxis.type == b2EPAxis::e_edgeA) { + cp->localPoint = b2MulT(m_xf, clipPoints2[i].v); + cp->id = clipPoints2[i].id; + } else { + cp->localPoint = clipPoints2[i].v; + cp->id.cf.typeA = clipPoints2[i].id.cf.typeB; + cp->id.cf.typeB = clipPoints2[i].id.cf.typeA; + cp->id.cf.indexA = clipPoints2[i].id.cf.indexB; + cp->id.cf.indexB = clipPoints2[i].id.cf.indexA; + } + + ++pointCount; + } + } + + manifold->pointCount = pointCount; } b2EPAxis b2EPCollider::ComputeEdgeSeparation() { - b2EPAxis axis; - axis.type = b2EPAxis::e_edgeA; - axis.index = m_front ? 0 : 1; - axis.separation = FLT_MAX; - - for (int32 i = 0; i < m_polygonB.count; ++i) - { - float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1); - if (s < axis.separation) - { - axis.separation = s; - } - } - - return axis; + b2EPAxis axis; + axis.type = b2EPAxis::e_edgeA; + axis.index = m_front ? 0 : 1; + axis.separation = FLT_MAX; + + for (int32 i = 0; i < m_polygonB.count; ++i) { + float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1); + if (s < axis.separation) { + axis.separation = s; + } + } + + return axis; } b2EPAxis b2EPCollider::ComputePolygonSeparation() { - b2EPAxis axis; - axis.type = b2EPAxis::e_unknown; - axis.index = -1; - axis.separation = -FLT_MAX; - - b2Vec2 perp(-m_normal.y, m_normal.x); - - for (int32 i = 0; i < m_polygonB.count; ++i) - { - b2Vec2 n = -m_polygonB.normals[i]; - - float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1); - float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2); - float32 s = b2Min(s1, s2); - - if (s > m_radius) - { - // No collision - axis.type = b2EPAxis::e_edgeB; - axis.index = i; - axis.separation = s; - return axis; - } - - // Adjacency - if (b2Dot(n, perp) >= 0.0f) - { - if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop) - { - continue; - } - } - else - { - if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop) - { - continue; - } - } - - if (s > axis.separation) - { - axis.type = b2EPAxis::e_edgeB; - axis.index = i; - axis.separation = s; - } - } - - return axis; + b2EPAxis axis; + axis.type = b2EPAxis::e_unknown; + axis.index = -1; + axis.separation = -FLT_MAX; + + b2Vec2 perp(-m_normal.y, m_normal.x); + + for (int32 i = 0; i < m_polygonB.count; ++i) { + b2Vec2 n = -m_polygonB.normals[i]; + + float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1); + float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2); + float32 s = b2Min(s1, s2); + + if (s > m_radius) { + // No collision + axis.type = b2EPAxis::e_edgeB; + axis.index = i; + axis.separation = s; + return axis; + } + + // Adjacency + if (b2Dot(n, perp) >= 0.0f) { + if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop) { + continue; + } + } else { + if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop) { + continue; + } + } + + if (s > axis.separation) { + axis.type = b2EPAxis::e_edgeB; + axis.index = i; + axis.separation = s; + } + } + + return axis; } -void b2CollideEdgeAndPolygon( b2Manifold* manifold, - const b2EdgeShape* edgeA, const b2Transform& xfA, - const b2PolygonShape* polygonB, const b2Transform& xfB) +void b2CollideEdgeAndPolygon( + b2Manifold * manifold, const b2EdgeShape * edgeA, const b2Transform & xfA, + const b2PolygonShape * polygonB, const b2Transform & xfB) { - b2EPCollider collider; - collider.Collide(manifold, edgeA, xfA, polygonB, xfB); + b2EPCollider collider; + collider.Collide(manifold, edgeA, xfA, polygonB, xfB); } diff --git a/flatland_box2d/src/Collision/b2CollidePolygon.cpp b/flatland_box2d/src/Collision/b2CollidePolygon.cpp index c588f6ec..67fac415 100644 --- a/flatland_box2d/src/Collision/b2CollidePolygon.cpp +++ b/flatland_box2d/src/Collision/b2CollidePolygon.cpp @@ -1,109 +1,104 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ -#include "Box2D/Collision/b2Collision.h" #include "Box2D/Collision/Shapes/b2PolygonShape.h" +#include "Box2D/Collision/b2Collision.h" -// Find the max separation between poly1 and poly2 using edge normals from poly1. -static float32 b2FindMaxSeparation(int32* edgeIndex, - const b2PolygonShape* poly1, const b2Transform& xf1, - const b2PolygonShape* poly2, const b2Transform& xf2) +// Find the max separation between poly1 and poly2 using edge normals from +// poly1. +static float32 b2FindMaxSeparation( + int32 * edgeIndex, const b2PolygonShape * poly1, const b2Transform & xf1, + const b2PolygonShape * poly2, const b2Transform & xf2) { - int32 count1 = poly1->m_count; - int32 count2 = poly2->m_count; - const b2Vec2* n1s = poly1->m_normals; - const b2Vec2* v1s = poly1->m_vertices; - const b2Vec2* v2s = poly2->m_vertices; - b2Transform xf = b2MulT(xf2, xf1); - - int32 bestIndex = 0; - float32 maxSeparation = -b2_maxFloat; - for (int32 i = 0; i < count1; ++i) - { - // Get poly1 normal in frame2. - b2Vec2 n = b2Mul(xf.q, n1s[i]); - b2Vec2 v1 = b2Mul(xf, v1s[i]); - - // Find deepest point for normal i. - float32 si = b2_maxFloat; - for (int32 j = 0; j < count2; ++j) - { - float32 sij = b2Dot(n, v2s[j] - v1); - if (sij < si) - { - si = sij; - } - } - - if (si > maxSeparation) - { - maxSeparation = si; - bestIndex = i; - } - } - - *edgeIndex = bestIndex; - return maxSeparation; + int32 count1 = poly1->m_count; + int32 count2 = poly2->m_count; + const b2Vec2 * n1s = poly1->m_normals; + const b2Vec2 * v1s = poly1->m_vertices; + const b2Vec2 * v2s = poly2->m_vertices; + b2Transform xf = b2MulT(xf2, xf1); + + int32 bestIndex = 0; + float32 maxSeparation = -b2_maxFloat; + for (int32 i = 0; i < count1; ++i) { + // Get poly1 normal in frame2. + b2Vec2 n = b2Mul(xf.q, n1s[i]); + b2Vec2 v1 = b2Mul(xf, v1s[i]); + + // Find deepest point for normal i. + float32 si = b2_maxFloat; + for (int32 j = 0; j < count2; ++j) { + float32 sij = b2Dot(n, v2s[j] - v1); + if (sij < si) { + si = sij; + } + } + + if (si > maxSeparation) { + maxSeparation = si; + bestIndex = i; + } + } + + *edgeIndex = bestIndex; + return maxSeparation; } -static void b2FindIncidentEdge(b2ClipVertex c[2], - const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1, - const b2PolygonShape* poly2, const b2Transform& xf2) +static void b2FindIncidentEdge( + b2ClipVertex c[2], const b2PolygonShape * poly1, const b2Transform & xf1, int32 edge1, + const b2PolygonShape * poly2, const b2Transform & xf2) { - const b2Vec2* normals1 = poly1->m_normals; - - int32 count2 = poly2->m_count; - const b2Vec2* vertices2 = poly2->m_vertices; - const b2Vec2* normals2 = poly2->m_normals; - - b2Assert(0 <= edge1 && edge1 < poly1->m_count); - - // Get the normal of the reference edge in poly2's frame. - b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); - - // Find the incident edge on poly2. - int32 index = 0; - float32 minDot = b2_maxFloat; - for (int32 i = 0; i < count2; ++i) - { - float32 dot = b2Dot(normal1, normals2[i]); - if (dot < minDot) - { - minDot = dot; - index = i; - } - } - - // Build the clip vertices for the incident edge. - int32 i1 = index; - int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; - - c[0].v = b2Mul(xf2, vertices2[i1]); - c[0].id.cf.indexA = (uint8)edge1; - c[0].id.cf.indexB = (uint8)i1; - c[0].id.cf.typeA = b2ContactFeature::e_face; - c[0].id.cf.typeB = b2ContactFeature::e_vertex; - - c[1].v = b2Mul(xf2, vertices2[i2]); - c[1].id.cf.indexA = (uint8)edge1; - c[1].id.cf.indexB = (uint8)i2; - c[1].id.cf.typeA = b2ContactFeature::e_face; - c[1].id.cf.typeB = b2ContactFeature::e_vertex; + const b2Vec2 * normals1 = poly1->m_normals; + + int32 count2 = poly2->m_count; + const b2Vec2 * vertices2 = poly2->m_vertices; + const b2Vec2 * normals2 = poly2->m_normals; + + b2Assert(0 <= edge1 && edge1 < poly1->m_count); + + // Get the normal of the reference edge in poly2's frame. + b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1])); + + // Find the incident edge on poly2. + int32 index = 0; + float32 minDot = b2_maxFloat; + for (int32 i = 0; i < count2; ++i) { + float32 dot = b2Dot(normal1, normals2[i]); + if (dot < minDot) { + minDot = dot; + index = i; + } + } + + // Build the clip vertices for the incident edge. + int32 i1 = index; + int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0; + + c[0].v = b2Mul(xf2, vertices2[i1]); + c[0].id.cf.indexA = (uint8)edge1; + c[0].id.cf.indexB = (uint8)i1; + c[0].id.cf.typeA = b2ContactFeature::e_face; + c[0].id.cf.typeB = b2ContactFeature::e_vertex; + + c[1].v = b2Mul(xf2, vertices2[i2]); + c[1].id.cf.indexA = (uint8)edge1; + c[1].id.cf.indexB = (uint8)i2; + c[1].id.cf.typeA = b2ContactFeature::e_face; + c[1].id.cf.typeB = b2ContactFeature::e_vertex; } // Find edge normal of max separation on A - return if separating axis is found @@ -113,127 +108,117 @@ static void b2FindIncidentEdge(b2ClipVertex c[2], // Clip // The normal points from 1 to 2 -void b2CollidePolygons(b2Manifold* manifold, - const b2PolygonShape* polyA, const b2Transform& xfA, - const b2PolygonShape* polyB, const b2Transform& xfB) +void b2CollidePolygons( + b2Manifold * manifold, const b2PolygonShape * polyA, const b2Transform & xfA, + const b2PolygonShape * polyB, const b2Transform & xfB) { - manifold->pointCount = 0; - float32 totalRadius = polyA->m_radius + polyB->m_radius; - - int32 edgeA = 0; - float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); - if (separationA > totalRadius) - return; - - int32 edgeB = 0; - float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); - if (separationB > totalRadius) - return; - - const b2PolygonShape* poly1; // reference polygon - const b2PolygonShape* poly2; // incident polygon - b2Transform xf1, xf2; - int32 edge1; // reference edge - uint8 flip; - const float32 k_tol = 0.1f * b2_linearSlop; - - if (separationB > separationA + k_tol) - { - poly1 = polyB; - poly2 = polyA; - xf1 = xfB; - xf2 = xfA; - edge1 = edgeB; - manifold->type = b2Manifold::e_faceB; - flip = 1; - } - else - { - poly1 = polyA; - poly2 = polyB; - xf1 = xfA; - xf2 = xfB; - edge1 = edgeA; - manifold->type = b2Manifold::e_faceA; - flip = 0; - } - - b2ClipVertex incidentEdge[2]; - b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); - - int32 count1 = poly1->m_count; - const b2Vec2* vertices1 = poly1->m_vertices; - - int32 iv1 = edge1; - int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; - - b2Vec2 v11 = vertices1[iv1]; - b2Vec2 v12 = vertices1[iv2]; - - b2Vec2 localTangent = v12 - v11; - localTangent.Normalize(); - - b2Vec2 localNormal = b2Cross(localTangent, 1.0f); - b2Vec2 planePoint = 0.5f * (v11 + v12); - - b2Vec2 tangent = b2Mul(xf1.q, localTangent); - b2Vec2 normal = b2Cross(tangent, 1.0f); - - v11 = b2Mul(xf1, v11); - v12 = b2Mul(xf1, v12); - - // Face offset. - float32 frontOffset = b2Dot(normal, v11); - - // Side offsets, extended by polytope skin thickness. - float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius; - float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius; - - // Clip incident edge against extruded edge1 side edges. - b2ClipVertex clipPoints1[2]; - b2ClipVertex clipPoints2[2]; - int np; - - // Clip to box side 1 - np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); - - if (np < 2) - return; - - // Clip to negative box side 1 - np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); - - if (np < 2) - { - return; - } - - // Now clipPoints2 contains the clipped points. - manifold->localNormal = localNormal; - manifold->localPoint = planePoint; - - int32 pointCount = 0; - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; - - if (separation <= totalRadius) - { - b2ManifoldPoint* cp = manifold->points + pointCount; - cp->localPoint = b2MulT(xf2, clipPoints2[i].v); - cp->id = clipPoints2[i].id; - if (flip) - { - // Swap features - b2ContactFeature cf = cp->id.cf; - cp->id.cf.indexA = cf.indexB; - cp->id.cf.indexB = cf.indexA; - cp->id.cf.typeA = cf.typeB; - cp->id.cf.typeB = cf.typeA; - } - ++pointCount; - } - } - - manifold->pointCount = pointCount; + manifold->pointCount = 0; + float32 totalRadius = polyA->m_radius + polyB->m_radius; + + int32 edgeA = 0; + float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); + if (separationA > totalRadius) return; + + int32 edgeB = 0; + float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); + if (separationB > totalRadius) return; + + const b2PolygonShape * poly1; // reference polygon + const b2PolygonShape * poly2; // incident polygon + b2Transform xf1, xf2; + int32 edge1; // reference edge + uint8 flip; + const float32 k_tol = 0.1f * b2_linearSlop; + + if (separationB > separationA + k_tol) { + poly1 = polyB; + poly2 = polyA; + xf1 = xfB; + xf2 = xfA; + edge1 = edgeB; + manifold->type = b2Manifold::e_faceB; + flip = 1; + } else { + poly1 = polyA; + poly2 = polyB; + xf1 = xfA; + xf2 = xfB; + edge1 = edgeA; + manifold->type = b2Manifold::e_faceA; + flip = 0; + } + + b2ClipVertex incidentEdge[2]; + b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); + + int32 count1 = poly1->m_count; + const b2Vec2 * vertices1 = poly1->m_vertices; + + int32 iv1 = edge1; + int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; + + b2Vec2 v11 = vertices1[iv1]; + b2Vec2 v12 = vertices1[iv2]; + + b2Vec2 localTangent = v12 - v11; + localTangent.Normalize(); + + b2Vec2 localNormal = b2Cross(localTangent, 1.0f); + b2Vec2 planePoint = 0.5f * (v11 + v12); + + b2Vec2 tangent = b2Mul(xf1.q, localTangent); + b2Vec2 normal = b2Cross(tangent, 1.0f); + + v11 = b2Mul(xf1, v11); + v12 = b2Mul(xf1, v12); + + // Face offset. + float32 frontOffset = b2Dot(normal, v11); + + // Side offsets, extended by polytope skin thickness. + float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius; + float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius; + + // Clip incident edge against extruded edge1 side edges. + b2ClipVertex clipPoints1[2]; + b2ClipVertex clipPoints2[2]; + int np; + + // Clip to box side 1 + np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1); + + if (np < 2) return; + + // Clip to negative box side 1 + np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2); + + if (np < 2) { + return; + } + + // Now clipPoints2 contains the clipped points. + manifold->localNormal = localNormal; + manifold->localPoint = planePoint; + + int32 pointCount = 0; + for (int32 i = 0; i < b2_maxManifoldPoints; ++i) { + float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset; + + if (separation <= totalRadius) { + b2ManifoldPoint * cp = manifold->points + pointCount; + cp->localPoint = b2MulT(xf2, clipPoints2[i].v); + cp->id = clipPoints2[i].id; + if (flip) { + // Swap features + b2ContactFeature cf = cp->id.cf; + cp->id.cf.indexA = cf.indexB; + cp->id.cf.indexB = cf.indexA; + cp->id.cf.typeA = cf.typeB; + cp->id.cf.typeB = cf.typeA; + } + ++pointCount; + } + } + + manifold->pointCount = pointCount; } diff --git a/flatland_box2d/src/Collision/b2Collision.cpp b/flatland_box2d/src/Collision/b2Collision.cpp index 5b7933f9..2cef7898 100644 --- a/flatland_box2d/src/Collision/b2Collision.cpp +++ b/flatland_box2d/src/Collision/b2Collision.cpp @@ -1,252 +1,227 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Collision/b2Collision.h" + #include "Box2D/Collision/b2Distance.h" -void b2WorldManifold::Initialize(const b2Manifold* manifold, - const b2Transform& xfA, float32 radiusA, - const b2Transform& xfB, float32 radiusB) +void b2WorldManifold::Initialize( + const b2Manifold * manifold, const b2Transform & xfA, float32 radiusA, const b2Transform & xfB, + float32 radiusB) { - if (manifold->pointCount == 0) - { - return; - } - - switch (manifold->type) - { - case b2Manifold::e_circles: - { - normal.Set(1.0f, 0.0f); - b2Vec2 pointA = b2Mul(xfA, manifold->localPoint); - b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint); - if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) - { - normal = pointB - pointA; - normal.Normalize(); - } - - b2Vec2 cA = pointA + radiusA * normal; - b2Vec2 cB = pointB - radiusB * normal; - points[0] = 0.5f * (cA + cB); - separations[0] = b2Dot(cB - cA, normal); - } - break; - - case b2Manifold::e_faceA: - { - normal = b2Mul(xfA.q, manifold->localNormal); - b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint); - - for (int32 i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); - b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; - b2Vec2 cB = clipPoint - radiusB * normal; - points[i] = 0.5f * (cA + cB); - separations[i] = b2Dot(cB - cA, normal); - } - } - break; - - case b2Manifold::e_faceB: - { - normal = b2Mul(xfB.q, manifold->localNormal); - b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint); - - for (int32 i = 0; i < manifold->pointCount; ++i) - { - b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); - b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; - b2Vec2 cA = clipPoint - radiusA * normal; - points[i] = 0.5f * (cA + cB); - separations[i] = b2Dot(cA - cB, normal); - } - - // Ensure normal points from A to B. - normal = -normal; - } - break; - } + if (manifold->pointCount == 0) { + return; + } + + switch (manifold->type) { + case b2Manifold::e_circles: { + normal.Set(1.0f, 0.0f); + b2Vec2 pointA = b2Mul(xfA, manifold->localPoint); + b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint); + if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon) { + normal = pointB - pointA; + normal.Normalize(); + } + + b2Vec2 cA = pointA + radiusA * normal; + b2Vec2 cB = pointB - radiusB * normal; + points[0] = 0.5f * (cA + cB); + separations[0] = b2Dot(cB - cA, normal); + } break; + + case b2Manifold::e_faceA: { + normal = b2Mul(xfA.q, manifold->localNormal); + b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint); + + for (int32 i = 0; i < manifold->pointCount; ++i) { + b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); + b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal; + b2Vec2 cB = clipPoint - radiusB * normal; + points[i] = 0.5f * (cA + cB); + separations[i] = b2Dot(cB - cA, normal); + } + } break; + + case b2Manifold::e_faceB: { + normal = b2Mul(xfB.q, manifold->localNormal); + b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint); + + for (int32 i = 0; i < manifold->pointCount; ++i) { + b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); + b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal; + b2Vec2 cA = clipPoint - radiusA * normal; + points[i] = 0.5f * (cA + cB); + separations[i] = b2Dot(cA - cB, normal); + } + + // Ensure normal points from A to B. + normal = -normal; + } break; + } } -void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], - const b2Manifold* manifold1, const b2Manifold* manifold2) +void b2GetPointStates( + b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints], + const b2Manifold * manifold1, const b2Manifold * manifold2) { - for (int32 i = 0; i < b2_maxManifoldPoints; ++i) - { - state1[i] = b2_nullState; - state2[i] = b2_nullState; - } - - // Detect persists and removes. - for (int32 i = 0; i < manifold1->pointCount; ++i) - { - b2ContactID id = manifold1->points[i].id; - - state1[i] = b2_removeState; - - for (int32 j = 0; j < manifold2->pointCount; ++j) - { - if (manifold2->points[j].id.key == id.key) - { - state1[i] = b2_persistState; - break; - } - } - } - - // Detect persists and adds. - for (int32 i = 0; i < manifold2->pointCount; ++i) - { - b2ContactID id = manifold2->points[i].id; - - state2[i] = b2_addState; - - for (int32 j = 0; j < manifold1->pointCount; ++j) - { - if (manifold1->points[j].id.key == id.key) - { - state2[i] = b2_persistState; - break; - } - } - } + for (int32 i = 0; i < b2_maxManifoldPoints; ++i) { + state1[i] = b2_nullState; + state2[i] = b2_nullState; + } + + // Detect persists and removes. + for (int32 i = 0; i < manifold1->pointCount; ++i) { + b2ContactID id = manifold1->points[i].id; + + state1[i] = b2_removeState; + + for (int32 j = 0; j < manifold2->pointCount; ++j) { + if (manifold2->points[j].id.key == id.key) { + state1[i] = b2_persistState; + break; + } + } + } + + // Detect persists and adds. + for (int32 i = 0; i < manifold2->pointCount; ++i) { + b2ContactID id = manifold2->points[i].id; + + state2[i] = b2_addState; + + for (int32 j = 0; j < manifold1->pointCount; ++j) { + if (manifold1->points[j].id.key == id.key) { + state2[i] = b2_persistState; + break; + } + } + } } // From Real-time Collision Detection, p179. -bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const +bool b2AABB::RayCast(b2RayCastOutput * output, const b2RayCastInput & input) const { - float32 tmin = -b2_maxFloat; - float32 tmax = b2_maxFloat; - - b2Vec2 p = input.p1; - b2Vec2 d = input.p2 - input.p1; - b2Vec2 absD = b2Abs(d); - - b2Vec2 normal; - - for (int32 i = 0; i < 2; ++i) - { - if (absD(i) < b2_epsilon) - { - // Parallel. - if (p(i) < lowerBound(i) || upperBound(i) < p(i)) - { - return false; - } - } - else - { - float32 inv_d = 1.0f / d(i); - float32 t1 = (lowerBound(i) - p(i)) * inv_d; - float32 t2 = (upperBound(i) - p(i)) * inv_d; - - // Sign of the normal vector. - float32 s = -1.0f; - - if (t1 > t2) - { - b2Swap(t1, t2); - s = 1.0f; - } - - // Push the min up - if (t1 > tmin) - { - normal.SetZero(); - normal(i) = s; - tmin = t1; - } - - // Pull the max down - tmax = b2Min(tmax, t2); - - if (tmin > tmax) - { - return false; - } - } - } - - // Does the ray start inside the box? - // Does the ray intersect beyond the max fraction? - if (tmin < 0.0f || input.maxFraction < tmin) - { - return false; - } - - // Intersection. - output->fraction = tmin; - output->normal = normal; - return true; + float32 tmin = -b2_maxFloat; + float32 tmax = b2_maxFloat; + + b2Vec2 p = input.p1; + b2Vec2 d = input.p2 - input.p1; + b2Vec2 absD = b2Abs(d); + + b2Vec2 normal; + + for (int32 i = 0; i < 2; ++i) { + if (absD(i) < b2_epsilon) { + // Parallel. + if (p(i) < lowerBound(i) || upperBound(i) < p(i)) { + return false; + } + } else { + float32 inv_d = 1.0f / d(i); + float32 t1 = (lowerBound(i) - p(i)) * inv_d; + float32 t2 = (upperBound(i) - p(i)) * inv_d; + + // Sign of the normal vector. + float32 s = -1.0f; + + if (t1 > t2) { + b2Swap(t1, t2); + s = 1.0f; + } + + // Push the min up + if (t1 > tmin) { + normal.SetZero(); + normal(i) = s; + tmin = t1; + } + + // Pull the max down + tmax = b2Min(tmax, t2); + + if (tmin > tmax) { + return false; + } + } + } + + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if (tmin < 0.0f || input.maxFraction < tmin) { + return false; + } + + // Intersection. + output->fraction = tmin; + output->normal = normal; + return true; } // Sutherland-Hodgman clipping. -int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2], - const b2Vec2& normal, float32 offset, int32 vertexIndexA) +int32 b2ClipSegmentToLine( + b2ClipVertex vOut[2], const b2ClipVertex vIn[2], const b2Vec2 & normal, float32 offset, + int32 vertexIndexA) { - // Start with no output points - int32 numOut = 0; - - // Calculate the distance of end points to the line - float32 distance0 = b2Dot(normal, vIn[0].v) - offset; - float32 distance1 = b2Dot(normal, vIn[1].v) - offset; - - // If the points are behind the plane - if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; - if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; - - // If the points are on different sides of the plane - if (distance0 * distance1 < 0.0f) - { - // Find intersection point of edge and plane - float32 interp = distance0 / (distance0 - distance1); - vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); - - // VertexA is hitting edgeB. - vOut[numOut].id.cf.indexA = static_cast(vertexIndexA); - vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB; - vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex; - vOut[numOut].id.cf.typeB = b2ContactFeature::e_face; - ++numOut; - } - - return numOut; + // Start with no output points + int32 numOut = 0; + + // Calculate the distance of end points to the line + float32 distance0 = b2Dot(normal, vIn[0].v) - offset; + float32 distance1 = b2Dot(normal, vIn[1].v) - offset; + + // If the points are behind the plane + if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; + if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; + + // If the points are on different sides of the plane + if (distance0 * distance1 < 0.0f) { + // Find intersection point of edge and plane + float32 interp = distance0 / (distance0 - distance1); + vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); + + // VertexA is hitting edgeB. + vOut[numOut].id.cf.indexA = static_cast(vertexIndexA); + vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB; + vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex; + vOut[numOut].id.cf.typeB = b2ContactFeature::e_face; + ++numOut; + } + + return numOut; } -bool b2TestOverlap( const b2Shape* shapeA, int32 indexA, - const b2Shape* shapeB, int32 indexB, - const b2Transform& xfA, const b2Transform& xfB) +bool b2TestOverlap( + const b2Shape * shapeA, int32 indexA, const b2Shape * shapeB, int32 indexB, + const b2Transform & xfA, const b2Transform & xfB) { - b2DistanceInput input; - input.proxyA.Set(shapeA, indexA); - input.proxyB.Set(shapeB, indexB); - input.transformA = xfA; - input.transformB = xfB; - input.useRadii = true; + b2DistanceInput input; + input.proxyA.Set(shapeA, indexA); + input.proxyB.Set(shapeB, indexB); + input.transformA = xfA; + input.transformB = xfB; + input.useRadii = true; - b2SimplexCache cache; - cache.count = 0; + b2SimplexCache cache; + cache.count = 0; - b2DistanceOutput output; + b2DistanceOutput output; - b2Distance(&output, &cache, &input); + b2Distance(&output, &cache, &input); - return output.distance < 10.0f * b2_epsilon; + return output.distance < 10.0f * b2_epsilon; } diff --git a/flatland_box2d/src/Collision/b2Distance.cpp b/flatland_box2d/src/Collision/b2Distance.cpp index 1b11827a..50d6cf66 100644 --- a/flatland_box2d/src/Collision/b2Distance.cpp +++ b/flatland_box2d/src/Collision/b2Distance.cpp @@ -1,275 +1,249 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Collision/b2Distance.h" + +#include "Box2D/Collision/Shapes/b2ChainShape.h" #include "Box2D/Collision/Shapes/b2CircleShape.h" #include "Box2D/Collision/Shapes/b2EdgeShape.h" -#include "Box2D/Collision/Shapes/b2ChainShape.h" #include "Box2D/Collision/Shapes/b2PolygonShape.h" // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates. int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters; -void b2DistanceProxy::Set(const b2Shape* shape, int32 index) +void b2DistanceProxy::Set(const b2Shape * shape, int32 index) { - switch (shape->GetType()) - { - case b2Shape::e_circle: - { - const b2CircleShape* circle = static_cast(shape); - m_vertices = &circle->m_p; - m_count = 1; - m_radius = circle->m_radius; - } - break; - - case b2Shape::e_polygon: - { - const b2PolygonShape* polygon = static_cast(shape); - m_vertices = polygon->m_vertices; - m_count = polygon->m_count; - m_radius = polygon->m_radius; - } - break; - - case b2Shape::e_chain: - { - const b2ChainShape* chain = static_cast(shape); - b2Assert(0 <= index && index < chain->m_count); - - m_buffer[0] = chain->m_vertices[index]; - if (index + 1 < chain->m_count) - { - m_buffer[1] = chain->m_vertices[index + 1]; - } - else - { - m_buffer[1] = chain->m_vertices[0]; - } - - m_vertices = m_buffer; - m_count = 2; - m_radius = chain->m_radius; - } - break; - - case b2Shape::e_edge: - { - const b2EdgeShape* edge = static_cast(shape); - m_vertices = &edge->m_vertex1; - m_count = 2; - m_radius = edge->m_radius; - } - break; - - default: - b2Assert(false); - } + switch (shape->GetType()) { + case b2Shape::e_circle: { + const b2CircleShape * circle = static_cast(shape); + m_vertices = &circle->m_p; + m_count = 1; + m_radius = circle->m_radius; + } break; + + case b2Shape::e_polygon: { + const b2PolygonShape * polygon = static_cast(shape); + m_vertices = polygon->m_vertices; + m_count = polygon->m_count; + m_radius = polygon->m_radius; + } break; + + case b2Shape::e_chain: { + const b2ChainShape * chain = static_cast(shape); + b2Assert(0 <= index && index < chain->m_count); + + m_buffer[0] = chain->m_vertices[index]; + if (index + 1 < chain->m_count) { + m_buffer[1] = chain->m_vertices[index + 1]; + } else { + m_buffer[1] = chain->m_vertices[0]; + } + + m_vertices = m_buffer; + m_count = 2; + m_radius = chain->m_radius; + } break; + + case b2Shape::e_edge: { + const b2EdgeShape * edge = static_cast(shape); + m_vertices = &edge->m_vertex1; + m_count = 2; + m_radius = edge->m_radius; + } break; + + default: + b2Assert(false); + } } - struct b2SimplexVertex { - b2Vec2 wA; // support point in proxyA - b2Vec2 wB; // support point in proxyB - b2Vec2 w; // wB - wA - float32 a; // barycentric coordinate for closest point - int32 indexA; // wA index - int32 indexB; // wB index + b2Vec2 wA; // support point in proxyA + b2Vec2 wB; // support point in proxyB + b2Vec2 w; // wB - wA + float32 a; // barycentric coordinate for closest point + int32 indexA; // wA index + int32 indexB; // wB index }; struct b2Simplex { - void ReadCache( const b2SimplexCache* cache, - const b2DistanceProxy* proxyA, const b2Transform& transformA, - const b2DistanceProxy* proxyB, const b2Transform& transformB) - { - b2Assert(cache->count <= 3); - - // Copy data from cache. - m_count = cache->count; - b2SimplexVertex* vertices = &m_v1; - for (int32 i = 0; i < m_count; ++i) - { - b2SimplexVertex* v = vertices + i; - v->indexA = cache->indexA[i]; - v->indexB = cache->indexB[i]; - b2Vec2 wALocal = proxyA->GetVertex(v->indexA); - b2Vec2 wBLocal = proxyB->GetVertex(v->indexB); - v->wA = b2Mul(transformA, wALocal); - v->wB = b2Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - v->a = 0.0f; - } - - // Compute the new simplex metric, if it is substantially different than - // old metric then flush the simplex. - if (m_count > 1) - { - float32 metric1 = cache->metric; - float32 metric2 = GetMetric(); - if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) - { - // Reset the simplex. - m_count = 0; - } - } - - // If the cache is empty or invalid ... - if (m_count == 0) - { - b2SimplexVertex* v = vertices + 0; - v->indexA = 0; - v->indexB = 0; - b2Vec2 wALocal = proxyA->GetVertex(0); - b2Vec2 wBLocal = proxyB->GetVertex(0); - v->wA = b2Mul(transformA, wALocal); - v->wB = b2Mul(transformB, wBLocal); - v->w = v->wB - v->wA; - v->a = 1.0f; - m_count = 1; - } - } - - void WriteCache(b2SimplexCache* cache) const - { - cache->metric = GetMetric(); - cache->count = uint16(m_count); - const b2SimplexVertex* vertices = &m_v1; - for (int32 i = 0; i < m_count; ++i) - { - cache->indexA[i] = uint8(vertices[i].indexA); - cache->indexB[i] = uint8(vertices[i].indexB); - } - } - - b2Vec2 GetSearchDirection() const - { - switch (m_count) - { - case 1: - return -m_v1.w; - - case 2: - { - b2Vec2 e12 = m_v2.w - m_v1.w; - float32 sgn = b2Cross(e12, -m_v1.w); - if (sgn > 0.0f) - { - // Origin is left of e12. - return b2Cross(1.0f, e12); - } - else - { - // Origin is right of e12. - return b2Cross(e12, 1.0f); - } - } - - default: - b2Assert(false); - return b2Vec2_zero; - } - } - - b2Vec2 GetClosestPoint() const - { - switch (m_count) - { - case 0: - b2Assert(false); - return b2Vec2_zero; - - case 1: - return m_v1.w; - - case 2: - return m_v1.a * m_v1.w + m_v2.a * m_v2.w; - - case 3: - return b2Vec2_zero; - - default: - b2Assert(false); - return b2Vec2_zero; - } - } - - void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const - { - switch (m_count) - { - case 0: - b2Assert(false); - break; - - case 1: - *pA = m_v1.wA; - *pB = m_v1.wB; - break; - - case 2: - *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; - *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; - break; - - case 3: - *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; - *pB = *pA; - break; - - default: - b2Assert(false); - break; - } - } - - float32 GetMetric() const - { - switch (m_count) - { - case 0: - b2Assert(false); - return 0.0f; - - case 1: - return 0.0f; - - case 2: - return b2Distance(m_v1.w, m_v2.w); - - case 3: - return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); - - default: - b2Assert(false); - return 0.0f; - } - } - - void Solve2(); - void Solve3(); - - b2SimplexVertex m_v1, m_v2, m_v3; - int32 m_count; + void ReadCache( + const b2SimplexCache * cache, const b2DistanceProxy * proxyA, const b2Transform & transformA, + const b2DistanceProxy * proxyB, const b2Transform & transformB) + { + b2Assert(cache->count <= 3); + + // Copy data from cache. + m_count = cache->count; + b2SimplexVertex * vertices = &m_v1; + for (int32 i = 0; i < m_count; ++i) { + b2SimplexVertex * v = vertices + i; + v->indexA = cache->indexA[i]; + v->indexB = cache->indexB[i]; + b2Vec2 wALocal = proxyA->GetVertex(v->indexA); + b2Vec2 wBLocal = proxyB->GetVertex(v->indexB); + v->wA = b2Mul(transformA, wALocal); + v->wB = b2Mul(transformB, wBLocal); + v->w = v->wB - v->wA; + v->a = 0.0f; + } + + // Compute the new simplex metric, if it is substantially different than + // old metric then flush the simplex. + if (m_count > 1) { + float32 metric1 = cache->metric; + float32 metric2 = GetMetric(); + if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon) { + // Reset the simplex. + m_count = 0; + } + } + + // If the cache is empty or invalid ... + if (m_count == 0) { + b2SimplexVertex * v = vertices + 0; + v->indexA = 0; + v->indexB = 0; + b2Vec2 wALocal = proxyA->GetVertex(0); + b2Vec2 wBLocal = proxyB->GetVertex(0); + v->wA = b2Mul(transformA, wALocal); + v->wB = b2Mul(transformB, wBLocal); + v->w = v->wB - v->wA; + v->a = 1.0f; + m_count = 1; + } + } + + void WriteCache(b2SimplexCache * cache) const + { + cache->metric = GetMetric(); + cache->count = uint16(m_count); + const b2SimplexVertex * vertices = &m_v1; + for (int32 i = 0; i < m_count; ++i) { + cache->indexA[i] = uint8(vertices[i].indexA); + cache->indexB[i] = uint8(vertices[i].indexB); + } + } + + b2Vec2 GetSearchDirection() const + { + switch (m_count) { + case 1: + return -m_v1.w; + + case 2: { + b2Vec2 e12 = m_v2.w - m_v1.w; + float32 sgn = b2Cross(e12, -m_v1.w); + if (sgn > 0.0f) { + // Origin is left of e12. + return b2Cross(1.0f, e12); + } else { + // Origin is right of e12. + return b2Cross(e12, 1.0f); + } + } + + default: + b2Assert(false); + return b2Vec2_zero; + } + } + + b2Vec2 GetClosestPoint() const + { + switch (m_count) { + case 0: + b2Assert(false); + return b2Vec2_zero; + + case 1: + return m_v1.w; + + case 2: + return m_v1.a * m_v1.w + m_v2.a * m_v2.w; + + case 3: + return b2Vec2_zero; + + default: + b2Assert(false); + return b2Vec2_zero; + } + } + + void GetWitnessPoints(b2Vec2 * pA, b2Vec2 * pB) const + { + switch (m_count) { + case 0: + b2Assert(false); + break; + + case 1: + *pA = m_v1.wA; + *pB = m_v1.wB; + break; + + case 2: + *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA; + *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB; + break; + + case 3: + *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA; + *pB = *pA; + break; + + default: + b2Assert(false); + break; + } + } + + float32 GetMetric() const + { + switch (m_count) { + case 0: + b2Assert(false); + return 0.0f; + + case 1: + return 0.0f; + + case 2: + return b2Distance(m_v1.w, m_v2.w); + + case 3: + return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w); + + default: + b2Assert(false); + return 0.0f; + } + } + + void Solve2(); + void Solve3(); + + b2SimplexVertex m_v1, m_v2, m_v3; + int32 m_count; }; - // Solve a line segment using barycentric coordinates. // // p = a1 * w1 + a2 * w2 @@ -295,36 +269,34 @@ struct b2Simplex // a2 = d12_2 / d12 void b2Simplex::Solve2() { - b2Vec2 w1 = m_v1.w; - b2Vec2 w2 = m_v2.w; - b2Vec2 e12 = w2 - w1; - - // w1 region - float32 d12_2 = -b2Dot(w1, e12); - if (d12_2 <= 0.0f) - { - // a2 <= 0, so we clamp it to 0 - m_v1.a = 1.0f; - m_count = 1; - return; - } - - // w2 region - float32 d12_1 = b2Dot(w2, e12); - if (d12_1 <= 0.0f) - { - // a1 <= 0, so we clamp it to 0 - m_v2.a = 1.0f; - m_count = 1; - m_v1 = m_v2; - return; - } - - // Must be in e12 region. - float32 inv_d12 = 1.0f / (d12_1 + d12_2); - m_v1.a = d12_1 * inv_d12; - m_v2.a = d12_2 * inv_d12; - m_count = 2; + b2Vec2 w1 = m_v1.w; + b2Vec2 w2 = m_v2.w; + b2Vec2 e12 = w2 - w1; + + // w1 region + float32 d12_2 = -b2Dot(w1, e12); + if (d12_2 <= 0.0f) { + // a2 <= 0, so we clamp it to 0 + m_v1.a = 1.0f; + m_count = 1; + return; + } + + // w2 region + float32 d12_1 = b2Dot(w2, e12); + if (d12_1 <= 0.0f) { + // a1 <= 0, so we clamp it to 0 + m_v2.a = 1.0f; + m_count = 1; + m_v1 = m_v2; + return; + } + + // Must be in e12 region. + float32 inv_d12 = 1.0f / (d12_1 + d12_2); + m_v1.a = d12_1 * inv_d12; + m_v2.a = d12_2 * inv_d12; + m_count = 2; } // Possible regions: @@ -334,256 +306,237 @@ void b2Simplex::Solve2() // - inside the triangle void b2Simplex::Solve3() { - b2Vec2 w1 = m_v1.w; - b2Vec2 w2 = m_v2.w; - b2Vec2 w3 = m_v3.w; - - // Edge12 - // [1 1 ][a1] = [1] - // [w1.e12 w2.e12][a2] = [0] - // a3 = 0 - b2Vec2 e12 = w2 - w1; - float32 w1e12 = b2Dot(w1, e12); - float32 w2e12 = b2Dot(w2, e12); - float32 d12_1 = w2e12; - float32 d12_2 = -w1e12; - - // Edge13 - // [1 1 ][a1] = [1] - // [w1.e13 w3.e13][a3] = [0] - // a2 = 0 - b2Vec2 e13 = w3 - w1; - float32 w1e13 = b2Dot(w1, e13); - float32 w3e13 = b2Dot(w3, e13); - float32 d13_1 = w3e13; - float32 d13_2 = -w1e13; - - // Edge23 - // [1 1 ][a2] = [1] - // [w2.e23 w3.e23][a3] = [0] - // a1 = 0 - b2Vec2 e23 = w3 - w2; - float32 w2e23 = b2Dot(w2, e23); - float32 w3e23 = b2Dot(w3, e23); - float32 d23_1 = w3e23; - float32 d23_2 = -w2e23; - - // Triangle123 - float32 n123 = b2Cross(e12, e13); - - float32 d123_1 = n123 * b2Cross(w2, w3); - float32 d123_2 = n123 * b2Cross(w3, w1); - float32 d123_3 = n123 * b2Cross(w1, w2); - - // w1 region - if (d12_2 <= 0.0f && d13_2 <= 0.0f) - { - m_v1.a = 1.0f; - m_count = 1; - return; - } - - // e12 - if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) - { - float32 inv_d12 = 1.0f / (d12_1 + d12_2); - m_v1.a = d12_1 * inv_d12; - m_v2.a = d12_2 * inv_d12; - m_count = 2; - return; - } - - // e13 - if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) - { - float32 inv_d13 = 1.0f / (d13_1 + d13_2); - m_v1.a = d13_1 * inv_d13; - m_v3.a = d13_2 * inv_d13; - m_count = 2; - m_v2 = m_v3; - return; - } - - // w2 region - if (d12_1 <= 0.0f && d23_2 <= 0.0f) - { - m_v2.a = 1.0f; - m_count = 1; - m_v1 = m_v2; - return; - } - - // w3 region - if (d13_1 <= 0.0f && d23_1 <= 0.0f) - { - m_v3.a = 1.0f; - m_count = 1; - m_v1 = m_v3; - return; - } - - // e23 - if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) - { - float32 inv_d23 = 1.0f / (d23_1 + d23_2); - m_v2.a = d23_1 * inv_d23; - m_v3.a = d23_2 * inv_d23; - m_count = 2; - m_v1 = m_v3; - return; - } - - // Must be in triangle123 - float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); - m_v1.a = d123_1 * inv_d123; - m_v2.a = d123_2 * inv_d123; - m_v3.a = d123_3 * inv_d123; - m_count = 3; + b2Vec2 w1 = m_v1.w; + b2Vec2 w2 = m_v2.w; + b2Vec2 w3 = m_v3.w; + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + b2Vec2 e12 = w2 - w1; + float32 w1e12 = b2Dot(w1, e12); + float32 w2e12 = b2Dot(w2, e12); + float32 d12_1 = w2e12; + float32 d12_2 = -w1e12; + + // Edge13 + // [1 1 ][a1] = [1] + // [w1.e13 w3.e13][a3] = [0] + // a2 = 0 + b2Vec2 e13 = w3 - w1; + float32 w1e13 = b2Dot(w1, e13); + float32 w3e13 = b2Dot(w3, e13); + float32 d13_1 = w3e13; + float32 d13_2 = -w1e13; + + // Edge23 + // [1 1 ][a2] = [1] + // [w2.e23 w3.e23][a3] = [0] + // a1 = 0 + b2Vec2 e23 = w3 - w2; + float32 w2e23 = b2Dot(w2, e23); + float32 w3e23 = b2Dot(w3, e23); + float32 d23_1 = w3e23; + float32 d23_2 = -w2e23; + + // Triangle123 + float32 n123 = b2Cross(e12, e13); + + float32 d123_1 = n123 * b2Cross(w2, w3); + float32 d123_2 = n123 * b2Cross(w3, w1); + float32 d123_3 = n123 * b2Cross(w1, w2); + + // w1 region + if (d12_2 <= 0.0f && d13_2 <= 0.0f) { + m_v1.a = 1.0f; + m_count = 1; + return; + } + + // e12 + if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f) { + float32 inv_d12 = 1.0f / (d12_1 + d12_2); + m_v1.a = d12_1 * inv_d12; + m_v2.a = d12_2 * inv_d12; + m_count = 2; + return; + } + + // e13 + if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f) { + float32 inv_d13 = 1.0f / (d13_1 + d13_2); + m_v1.a = d13_1 * inv_d13; + m_v3.a = d13_2 * inv_d13; + m_count = 2; + m_v2 = m_v3; + return; + } + + // w2 region + if (d12_1 <= 0.0f && d23_2 <= 0.0f) { + m_v2.a = 1.0f; + m_count = 1; + m_v1 = m_v2; + return; + } + + // w3 region + if (d13_1 <= 0.0f && d23_1 <= 0.0f) { + m_v3.a = 1.0f; + m_count = 1; + m_v1 = m_v3; + return; + } + + // e23 + if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f) { + float32 inv_d23 = 1.0f / (d23_1 + d23_2); + m_v2.a = d23_1 * inv_d23; + m_v3.a = d23_2 * inv_d23; + m_count = 2; + m_v1 = m_v3; + return; + } + + // Must be in triangle123 + float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3); + m_v1.a = d123_1 * inv_d123; + m_v2.a = d123_2 * inv_d123; + m_v3.a = d123_3 * inv_d123; + m_count = 3; } -void b2Distance(b2DistanceOutput* output, - b2SimplexCache* cache, - const b2DistanceInput* input) +void b2Distance(b2DistanceOutput * output, b2SimplexCache * cache, const b2DistanceInput * input) { - ++b2_gjkCalls; - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - b2Transform transformA = input->transformA; - b2Transform transformB = input->transformB; - - // Initialize the simplex. - b2Simplex simplex; - simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); - - // Get simplex vertices as an array. - b2SimplexVertex* vertices = &simplex.m_v1; - const int32 k_maxIters = 20; - - // These store the vertices of the last simplex so that we - // can check for duplicates and prevent cycling. - int32 saveA[3], saveB[3]; - int32 saveCount = 0; - - // Main iteration loop. - int32 iter = 0; - while (iter < k_maxIters) - { - // Copy simplex so we can identify duplicates. - saveCount = simplex.m_count; - for (int32 i = 0; i < saveCount; ++i) - { - saveA[i] = vertices[i].indexA; - saveB[i] = vertices[i].indexB; - } - - switch (simplex.m_count) - { - case 1: - break; - - case 2: - simplex.Solve2(); - break; - - case 3: - simplex.Solve3(); - break; - - default: - b2Assert(false); - } - - // If we have 3 points, then the origin is in the corresponding triangle. - if (simplex.m_count == 3) - { - break; - } - - // Get search direction. - b2Vec2 d = simplex.GetSearchDirection(); - - // Ensure the search direction is numerically fit. - if (d.LengthSquared() < b2_epsilon * b2_epsilon) - { - // The origin is probably contained by a line segment - // or triangle. Thus the shapes are overlapped. - - // We can't return zero here even though there may be overlap. - // In case the simplex is a point, segment, or triangle it is difficult - // to determine if the origin is contained in the CSO or very close to it. - break; - } - - // Compute a tentative new simplex vertex using support points. - b2SimplexVertex* vertex = vertices + simplex.m_count; - vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); - vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); - b2Vec2 wBLocal; - vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); - vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); - vertex->w = vertex->wB - vertex->wA; - - // Iteration count is equated to the number of support point calls. - ++iter; - ++b2_gjkIters; - - // Check for duplicate support points. This is the main termination criteria. - bool duplicate = false; - for (int32 i = 0; i < saveCount; ++i) - { - if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) - { - duplicate = true; - break; - } - } - - // If we found a duplicate support point we must exit to avoid cycling. - if (duplicate) - { - break; - } - - // New vertex is ok and needed. - ++simplex.m_count; - } - - b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); - - // Prepare output. - simplex.GetWitnessPoints(&output->pointA, &output->pointB); - output->distance = b2Distance(output->pointA, output->pointB); - output->iterations = iter; - - // Cache the simplex. - simplex.WriteCache(cache); - - // Apply radii if requested. - if (input->useRadii) - { - float32 rA = proxyA->m_radius; - float32 rB = proxyB->m_radius; - - if (output->distance > rA + rB && output->distance > b2_epsilon) - { - // Shapes are still no overlapped. - // Move the witness points to the outer surface. - output->distance -= rA + rB; - b2Vec2 normal = output->pointB - output->pointA; - normal.Normalize(); - output->pointA += rA * normal; - output->pointB -= rB * normal; - } - else - { - // Shapes are overlapped when radii are considered. - // Move the witness points to the middle. - b2Vec2 p = 0.5f * (output->pointA + output->pointB); - output->pointA = p; - output->pointB = p; - output->distance = 0.0f; - } - } + ++b2_gjkCalls; + + const b2DistanceProxy * proxyA = &input->proxyA; + const b2DistanceProxy * proxyB = &input->proxyB; + + b2Transform transformA = input->transformA; + b2Transform transformB = input->transformB; + + // Initialize the simplex. + b2Simplex simplex; + simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); + + // Get simplex vertices as an array. + b2SimplexVertex * vertices = &simplex.m_v1; + const int32 k_maxIters = 20; + + // These store the vertices of the last simplex so that we + // can check for duplicates and prevent cycling. + int32 saveA[3], saveB[3]; + int32 saveCount = 0; + + // Main iteration loop. + int32 iter = 0; + while (iter < k_maxIters) { + // Copy simplex so we can identify duplicates. + saveCount = simplex.m_count; + for (int32 i = 0; i < saveCount; ++i) { + saveA[i] = vertices[i].indexA; + saveB[i] = vertices[i].indexB; + } + + switch (simplex.m_count) { + case 1: + break; + + case 2: + simplex.Solve2(); + break; + + case 3: + simplex.Solve3(); + break; + + default: + b2Assert(false); + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if (simplex.m_count == 3) { + break; + } + + // Get search direction. + b2Vec2 d = simplex.GetSearchDirection(); + + // Ensure the search direction is numerically fit. + if (d.LengthSquared() < b2_epsilon * b2_epsilon) { + // The origin is probably contained by a line segment + // or triangle. Thus the shapes are overlapped. + + // We can't return zero here even though there may be overlap. + // In case the simplex is a point, segment, or triangle it is difficult + // to determine if the origin is contained in the CSO or very close to it. + break; + } + + // Compute a tentative new simplex vertex using support points. + b2SimplexVertex * vertex = vertices + simplex.m_count; + vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d)); + vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA)); + b2Vec2 wBLocal; + vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d)); + vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB)); + vertex->w = vertex->wB - vertex->wA; + + // Iteration count is equated to the number of support point calls. + ++iter; + ++b2_gjkIters; + + // Check for duplicate support points. This is the main termination + // criteria. + bool duplicate = false; + for (int32 i = 0; i < saveCount; ++i) { + if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i]) { + duplicate = true; + break; + } + } + + // If we found a duplicate support point we must exit to avoid cycling. + if (duplicate) { + break; + } + + // New vertex is ok and needed. + ++simplex.m_count; + } + + b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter); + + // Prepare output. + simplex.GetWitnessPoints(&output->pointA, &output->pointB); + output->distance = b2Distance(output->pointA, output->pointB); + output->iterations = iter; + + // Cache the simplex. + simplex.WriteCache(cache); + + // Apply radii if requested. + if (input->useRadii) { + float32 rA = proxyA->m_radius; + float32 rB = proxyB->m_radius; + + if (output->distance > rA + rB && output->distance > b2_epsilon) { + // Shapes are still no overlapped. + // Move the witness points to the outer surface. + output->distance -= rA + rB; + b2Vec2 normal = output->pointB - output->pointA; + normal.Normalize(); + output->pointA += rA * normal; + output->pointB -= rB * normal; + } else { + // Shapes are overlapped when radii are considered. + // Move the witness points to the middle. + b2Vec2 p = 0.5f * (output->pointA + output->pointB); + output->pointA = p; + output->pointB = p; + output->distance = 0.0f; + } + } } diff --git a/flatland_box2d/src/Collision/b2DynamicTree.cpp b/flatland_box2d/src/Collision/b2DynamicTree.cpp index ad592c11..1f271b9a 100644 --- a/flatland_box2d/src/Collision/b2DynamicTree.cpp +++ b/flatland_box2d/src/Collision/b2DynamicTree.cpp @@ -1,780 +1,697 @@ /* -* Copyright (c) 2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Collision/b2DynamicTree.h" + #include b2DynamicTree::b2DynamicTree() { - m_root = b2_nullNode; - - m_nodeCapacity = 16; - m_nodeCount = 0; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); - - // Build a linked list for the free list. - for (int32 i = 0; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = 0; - - m_path = 0; - - m_insertionCount = 0; + m_root = b2_nullNode; + + m_nodeCapacity = 16; + m_nodeCount = 0; + m_nodes = (b2TreeNode *)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode)); + + // Build a linked list for the free list. + for (int32 i = 0; i < m_nodeCapacity - 1; ++i) { + m_nodes[i].next = i + 1; + m_nodes[i].height = -1; + } + m_nodes[m_nodeCapacity - 1].next = b2_nullNode; + m_nodes[m_nodeCapacity - 1].height = -1; + m_freeList = 0; + + m_path = 0; + + m_insertionCount = 0; } b2DynamicTree::~b2DynamicTree() { - // This frees the entire tree in one shot. - b2Free(m_nodes); + // This frees the entire tree in one shot. + b2Free(m_nodes); } // Allocate a node from the pool. Grow the pool if necessary. int32 b2DynamicTree::AllocateNode() { - // Expand the node pool as needed. - if (m_freeList == b2_nullNode) - { - b2Assert(m_nodeCount == m_nodeCapacity); - - // The free list is empty. Rebuild a bigger pool. - b2TreeNode* oldNodes = m_nodes; - m_nodeCapacity *= 2; - m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); - memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); - b2Free(oldNodes); - - // Build a linked list for the free list. The parent - // pointer becomes the "next" pointer. - for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) - { - m_nodes[i].next = i + 1; - m_nodes[i].height = -1; - } - m_nodes[m_nodeCapacity-1].next = b2_nullNode; - m_nodes[m_nodeCapacity-1].height = -1; - m_freeList = m_nodeCount; - } - - // Peel a node off the free list. - int32 nodeId = m_freeList; - m_freeList = m_nodes[nodeId].next; - m_nodes[nodeId].parent = b2_nullNode; - m_nodes[nodeId].child1 = b2_nullNode; - m_nodes[nodeId].child2 = b2_nullNode; - m_nodes[nodeId].height = 0; - m_nodes[nodeId].userData = nullptr; - ++m_nodeCount; - return nodeId; + // Expand the node pool as needed. + if (m_freeList == b2_nullNode) { + b2Assert(m_nodeCount == m_nodeCapacity); + + // The free list is empty. Rebuild a bigger pool. + b2TreeNode * oldNodes = m_nodes; + m_nodeCapacity *= 2; + m_nodes = (b2TreeNode *)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode)); + memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode)); + b2Free(oldNodes); + + // Build a linked list for the free list. The parent + // pointer becomes the "next" pointer. + for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i) { + m_nodes[i].next = i + 1; + m_nodes[i].height = -1; + } + m_nodes[m_nodeCapacity - 1].next = b2_nullNode; + m_nodes[m_nodeCapacity - 1].height = -1; + m_freeList = m_nodeCount; + } + + // Peel a node off the free list. + int32 nodeId = m_freeList; + m_freeList = m_nodes[nodeId].next; + m_nodes[nodeId].parent = b2_nullNode; + m_nodes[nodeId].child1 = b2_nullNode; + m_nodes[nodeId].child2 = b2_nullNode; + m_nodes[nodeId].height = 0; + m_nodes[nodeId].userData = nullptr; + ++m_nodeCount; + return nodeId; } // Return a node to the pool. void b2DynamicTree::FreeNode(int32 nodeId) { - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2Assert(0 < m_nodeCount); - m_nodes[nodeId].next = m_freeList; - m_nodes[nodeId].height = -1; - m_freeList = nodeId; - --m_nodeCount; + b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); + b2Assert(0 < m_nodeCount); + m_nodes[nodeId].next = m_freeList; + m_nodes[nodeId].height = -1; + m_freeList = nodeId; + --m_nodeCount; } // Create a proxy in the tree as a leaf node. We return the index // of the node instead of a pointer so that we can grow // the node pool. -int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData) +int32 b2DynamicTree::CreateProxy(const b2AABB & aabb, void * userData) { - int32 proxyId = AllocateNode(); + int32 proxyId = AllocateNode(); - // Fatten the aabb. - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; - m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; - m_nodes[proxyId].userData = userData; - m_nodes[proxyId].height = 0; + // Fatten the aabb. + b2Vec2 r(b2_aabbExtension, b2_aabbExtension); + m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r; + m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r; + m_nodes[proxyId].userData = userData; + m_nodes[proxyId].height = 0; - InsertLeaf(proxyId); + InsertLeaf(proxyId); - return proxyId; + return proxyId; } void b2DynamicTree::DestroyProxy(int32 proxyId) { - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - b2Assert(m_nodes[proxyId].IsLeaf()); + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + b2Assert(m_nodes[proxyId].IsLeaf()); - RemoveLeaf(proxyId); - FreeNode(proxyId); + RemoveLeaf(proxyId); + FreeNode(proxyId); } -bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement) +bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB & aabb, const b2Vec2 & displacement) { - b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); - - b2Assert(m_nodes[proxyId].IsLeaf()); - - if (m_nodes[proxyId].aabb.Contains(aabb)) - { - return false; - } - - RemoveLeaf(proxyId); - - // Extend AABB. - b2AABB b = aabb; - b2Vec2 r(b2_aabbExtension, b2_aabbExtension); - b.lowerBound = b.lowerBound - r; - b.upperBound = b.upperBound + r; - - // Predict AABB displacement. - b2Vec2 d = b2_aabbMultiplier * displacement; - - if (d.x < 0.0f) - { - b.lowerBound.x += d.x; - } - else - { - b.upperBound.x += d.x; - } - - if (d.y < 0.0f) - { - b.lowerBound.y += d.y; - } - else - { - b.upperBound.y += d.y; - } - - m_nodes[proxyId].aabb = b; - - InsertLeaf(proxyId); - return true; + b2Assert(0 <= proxyId && proxyId < m_nodeCapacity); + + b2Assert(m_nodes[proxyId].IsLeaf()); + + if (m_nodes[proxyId].aabb.Contains(aabb)) { + return false; + } + + RemoveLeaf(proxyId); + + // Extend AABB. + b2AABB b = aabb; + b2Vec2 r(b2_aabbExtension, b2_aabbExtension); + b.lowerBound = b.lowerBound - r; + b.upperBound = b.upperBound + r; + + // Predict AABB displacement. + b2Vec2 d = b2_aabbMultiplier * displacement; + + if (d.x < 0.0f) { + b.lowerBound.x += d.x; + } else { + b.upperBound.x += d.x; + } + + if (d.y < 0.0f) { + b.lowerBound.y += d.y; + } else { + b.upperBound.y += d.y; + } + + m_nodes[proxyId].aabb = b; + + InsertLeaf(proxyId); + return true; } void b2DynamicTree::InsertLeaf(int32 leaf) { - ++m_insertionCount; - - if (m_root == b2_nullNode) - { - m_root = leaf; - m_nodes[m_root].parent = b2_nullNode; - return; - } - - // Find the best sibling for this node - b2AABB leafAABB = m_nodes[leaf].aabb; - int32 index = m_root; - while (m_nodes[index].IsLeaf() == false) - { - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - float32 area = m_nodes[index].aabb.GetPerimeter(); - - b2AABB combinedAABB; - combinedAABB.Combine(m_nodes[index].aabb, leafAABB); - float32 combinedArea = combinedAABB.GetPerimeter(); - - // Cost of creating a new parent for this node and the new leaf - float32 cost = 2.0f * combinedArea; - - // Minimum cost of pushing the leaf further down the tree - float32 inheritanceCost = 2.0f * (combinedArea - area); - - // Cost of descending into child1 - float32 cost1; - if (m_nodes[child1].IsLeaf()) - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child1].aabb); - cost1 = aabb.GetPerimeter() + inheritanceCost; - } - else - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child1].aabb); - float32 oldArea = m_nodes[child1].aabb.GetPerimeter(); - float32 newArea = aabb.GetPerimeter(); - cost1 = (newArea - oldArea) + inheritanceCost; - } - - // Cost of descending into child2 - float32 cost2; - if (m_nodes[child2].IsLeaf()) - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child2].aabb); - cost2 = aabb.GetPerimeter() + inheritanceCost; - } - else - { - b2AABB aabb; - aabb.Combine(leafAABB, m_nodes[child2].aabb); - float32 oldArea = m_nodes[child2].aabb.GetPerimeter(); - float32 newArea = aabb.GetPerimeter(); - cost2 = newArea - oldArea + inheritanceCost; - } - - // Descend according to the minimum cost. - if (cost < cost1 && cost < cost2) - { - break; - } - - // Descend - if (cost1 < cost2) - { - index = child1; - } - else - { - index = child2; - } - } - - int32 sibling = index; - - // Create a new parent. - int32 oldParent = m_nodes[sibling].parent; - int32 newParent = AllocateNode(); - m_nodes[newParent].parent = oldParent; - m_nodes[newParent].userData = nullptr; - m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); - m_nodes[newParent].height = m_nodes[sibling].height + 1; - - if (oldParent != b2_nullNode) - { - // The sibling was not the root. - if (m_nodes[oldParent].child1 == sibling) - { - m_nodes[oldParent].child1 = newParent; - } - else - { - m_nodes[oldParent].child2 = newParent; - } - - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - } - else - { - // The sibling was the root. - m_nodes[newParent].child1 = sibling; - m_nodes[newParent].child2 = leaf; - m_nodes[sibling].parent = newParent; - m_nodes[leaf].parent = newParent; - m_root = newParent; - } - - // Walk back up the tree fixing heights and AABBs - index = m_nodes[leaf].parent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - b2Assert(child1 != b2_nullNode); - b2Assert(child2 != b2_nullNode); - - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - - index = m_nodes[index].parent; - } - - //Validate(); + ++m_insertionCount; + + if (m_root == b2_nullNode) { + m_root = leaf; + m_nodes[m_root].parent = b2_nullNode; + return; + } + + // Find the best sibling for this node + b2AABB leafAABB = m_nodes[leaf].aabb; + int32 index = m_root; + while (m_nodes[index].IsLeaf() == false) { + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + float32 area = m_nodes[index].aabb.GetPerimeter(); + + b2AABB combinedAABB; + combinedAABB.Combine(m_nodes[index].aabb, leafAABB); + float32 combinedArea = combinedAABB.GetPerimeter(); + + // Cost of creating a new parent for this node and the new leaf + float32 cost = 2.0f * combinedArea; + + // Minimum cost of pushing the leaf further down the tree + float32 inheritanceCost = 2.0f * (combinedArea - area); + + // Cost of descending into child1 + float32 cost1; + if (m_nodes[child1].IsLeaf()) { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child1].aabb); + cost1 = aabb.GetPerimeter() + inheritanceCost; + } else { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child1].aabb); + float32 oldArea = m_nodes[child1].aabb.GetPerimeter(); + float32 newArea = aabb.GetPerimeter(); + cost1 = (newArea - oldArea) + inheritanceCost; + } + + // Cost of descending into child2 + float32 cost2; + if (m_nodes[child2].IsLeaf()) { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child2].aabb); + cost2 = aabb.GetPerimeter() + inheritanceCost; + } else { + b2AABB aabb; + aabb.Combine(leafAABB, m_nodes[child2].aabb); + float32 oldArea = m_nodes[child2].aabb.GetPerimeter(); + float32 newArea = aabb.GetPerimeter(); + cost2 = newArea - oldArea + inheritanceCost; + } + + // Descend according to the minimum cost. + if (cost < cost1 && cost < cost2) { + break; + } + + // Descend + if (cost1 < cost2) { + index = child1; + } else { + index = child2; + } + } + + int32 sibling = index; + + // Create a new parent. + int32 oldParent = m_nodes[sibling].parent; + int32 newParent = AllocateNode(); + m_nodes[newParent].parent = oldParent; + m_nodes[newParent].userData = nullptr; + m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb); + m_nodes[newParent].height = m_nodes[sibling].height + 1; + + if (oldParent != b2_nullNode) { + // The sibling was not the root. + if (m_nodes[oldParent].child1 == sibling) { + m_nodes[oldParent].child1 = newParent; + } else { + m_nodes[oldParent].child2 = newParent; + } + + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; + } else { + // The sibling was the root. + m_nodes[newParent].child1 = sibling; + m_nodes[newParent].child2 = leaf; + m_nodes[sibling].parent = newParent; + m_nodes[leaf].parent = newParent; + m_root = newParent; + } + + // Walk back up the tree fixing heights and AABBs + index = m_nodes[leaf].parent; + while (index != b2_nullNode) { + index = Balance(index); + + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + b2Assert(child1 != b2_nullNode); + b2Assert(child2 != b2_nullNode); + + m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + + index = m_nodes[index].parent; + } + + // Validate(); } void b2DynamicTree::RemoveLeaf(int32 leaf) { - if (leaf == m_root) - { - m_root = b2_nullNode; - return; - } - - int32 parent = m_nodes[leaf].parent; - int32 grandParent = m_nodes[parent].parent; - int32 sibling; - if (m_nodes[parent].child1 == leaf) - { - sibling = m_nodes[parent].child2; - } - else - { - sibling = m_nodes[parent].child1; - } - - if (grandParent != b2_nullNode) - { - // Destroy parent and connect sibling to grandParent. - if (m_nodes[grandParent].child1 == parent) - { - m_nodes[grandParent].child1 = sibling; - } - else - { - m_nodes[grandParent].child2 = sibling; - } - m_nodes[sibling].parent = grandParent; - FreeNode(parent); - - // Adjust ancestor bounds. - int32 index = grandParent; - while (index != b2_nullNode) - { - index = Balance(index); - - int32 child1 = m_nodes[index].child1; - int32 child2 = m_nodes[index].child2; - - m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); - - index = m_nodes[index].parent; - } - } - else - { - m_root = sibling; - m_nodes[sibling].parent = b2_nullNode; - FreeNode(parent); - } - - //Validate(); + if (leaf == m_root) { + m_root = b2_nullNode; + return; + } + + int32 parent = m_nodes[leaf].parent; + int32 grandParent = m_nodes[parent].parent; + int32 sibling; + if (m_nodes[parent].child1 == leaf) { + sibling = m_nodes[parent].child2; + } else { + sibling = m_nodes[parent].child1; + } + + if (grandParent != b2_nullNode) { + // Destroy parent and connect sibling to grandParent. + if (m_nodes[grandParent].child1 == parent) { + m_nodes[grandParent].child1 = sibling; + } else { + m_nodes[grandParent].child2 = sibling; + } + m_nodes[sibling].parent = grandParent; + FreeNode(parent); + + // Adjust ancestor bounds. + int32 index = grandParent; + while (index != b2_nullNode) { + index = Balance(index); + + int32 child1 = m_nodes[index].child1; + int32 child2 = m_nodes[index].child2; + + m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height); + + index = m_nodes[index].parent; + } + } else { + m_root = sibling; + m_nodes[sibling].parent = b2_nullNode; + FreeNode(parent); + } + + // Validate(); } // Perform a left or right rotation if node A is imbalanced. // Returns the new root index. int32 b2DynamicTree::Balance(int32 iA) { - b2Assert(iA != b2_nullNode); - - b2TreeNode* A = m_nodes + iA; - if (A->IsLeaf() || A->height < 2) - { - return iA; - } - - int32 iB = A->child1; - int32 iC = A->child2; - b2Assert(0 <= iB && iB < m_nodeCapacity); - b2Assert(0 <= iC && iC < m_nodeCapacity); - - b2TreeNode* B = m_nodes + iB; - b2TreeNode* C = m_nodes + iC; - - int32 balance = C->height - B->height; - - // Rotate C up - if (balance > 1) - { - int32 iF = C->child1; - int32 iG = C->child2; - b2TreeNode* F = m_nodes + iF; - b2TreeNode* G = m_nodes + iG; - b2Assert(0 <= iF && iF < m_nodeCapacity); - b2Assert(0 <= iG && iG < m_nodeCapacity); - - // Swap A and C - C->child1 = iA; - C->parent = A->parent; - A->parent = iC; - - // A's old parent should point to C - if (C->parent != b2_nullNode) - { - if (m_nodes[C->parent].child1 == iA) - { - m_nodes[C->parent].child1 = iC; - } - else - { - b2Assert(m_nodes[C->parent].child2 == iA); - m_nodes[C->parent].child2 = iC; - } - } - else - { - m_root = iC; - } - - // Rotate - if (F->height > G->height) - { - C->child2 = iF; - A->child2 = iG; - G->parent = iA; - A->aabb.Combine(B->aabb, G->aabb); - C->aabb.Combine(A->aabb, F->aabb); - - A->height = 1 + b2Max(B->height, G->height); - C->height = 1 + b2Max(A->height, F->height); - } - else - { - C->child2 = iG; - A->child2 = iF; - F->parent = iA; - A->aabb.Combine(B->aabb, F->aabb); - C->aabb.Combine(A->aabb, G->aabb); - - A->height = 1 + b2Max(B->height, F->height); - C->height = 1 + b2Max(A->height, G->height); - } - - return iC; - } - - // Rotate B up - if (balance < -1) - { - int32 iD = B->child1; - int32 iE = B->child2; - b2TreeNode* D = m_nodes + iD; - b2TreeNode* E = m_nodes + iE; - b2Assert(0 <= iD && iD < m_nodeCapacity); - b2Assert(0 <= iE && iE < m_nodeCapacity); - - // Swap A and B - B->child1 = iA; - B->parent = A->parent; - A->parent = iB; - - // A's old parent should point to B - if (B->parent != b2_nullNode) - { - if (m_nodes[B->parent].child1 == iA) - { - m_nodes[B->parent].child1 = iB; - } - else - { - b2Assert(m_nodes[B->parent].child2 == iA); - m_nodes[B->parent].child2 = iB; - } - } - else - { - m_root = iB; - } - - // Rotate - if (D->height > E->height) - { - B->child2 = iD; - A->child1 = iE; - E->parent = iA; - A->aabb.Combine(C->aabb, E->aabb); - B->aabb.Combine(A->aabb, D->aabb); - - A->height = 1 + b2Max(C->height, E->height); - B->height = 1 + b2Max(A->height, D->height); - } - else - { - B->child2 = iE; - A->child1 = iD; - D->parent = iA; - A->aabb.Combine(C->aabb, D->aabb); - B->aabb.Combine(A->aabb, E->aabb); - - A->height = 1 + b2Max(C->height, D->height); - B->height = 1 + b2Max(A->height, E->height); - } - - return iB; - } - - return iA; + b2Assert(iA != b2_nullNode); + + b2TreeNode * A = m_nodes + iA; + if (A->IsLeaf() || A->height < 2) { + return iA; + } + + int32 iB = A->child1; + int32 iC = A->child2; + b2Assert(0 <= iB && iB < m_nodeCapacity); + b2Assert(0 <= iC && iC < m_nodeCapacity); + + b2TreeNode * B = m_nodes + iB; + b2TreeNode * C = m_nodes + iC; + + int32 balance = C->height - B->height; + + // Rotate C up + if (balance > 1) { + int32 iF = C->child1; + int32 iG = C->child2; + b2TreeNode * F = m_nodes + iF; + b2TreeNode * G = m_nodes + iG; + b2Assert(0 <= iF && iF < m_nodeCapacity); + b2Assert(0 <= iG && iG < m_nodeCapacity); + + // Swap A and C + C->child1 = iA; + C->parent = A->parent; + A->parent = iC; + + // A's old parent should point to C + if (C->parent != b2_nullNode) { + if (m_nodes[C->parent].child1 == iA) { + m_nodes[C->parent].child1 = iC; + } else { + b2Assert(m_nodes[C->parent].child2 == iA); + m_nodes[C->parent].child2 = iC; + } + } else { + m_root = iC; + } + + // Rotate + if (F->height > G->height) { + C->child2 = iF; + A->child2 = iG; + G->parent = iA; + A->aabb.Combine(B->aabb, G->aabb); + C->aabb.Combine(A->aabb, F->aabb); + + A->height = 1 + b2Max(B->height, G->height); + C->height = 1 + b2Max(A->height, F->height); + } else { + C->child2 = iG; + A->child2 = iF; + F->parent = iA; + A->aabb.Combine(B->aabb, F->aabb); + C->aabb.Combine(A->aabb, G->aabb); + + A->height = 1 + b2Max(B->height, F->height); + C->height = 1 + b2Max(A->height, G->height); + } + + return iC; + } + + // Rotate B up + if (balance < -1) { + int32 iD = B->child1; + int32 iE = B->child2; + b2TreeNode * D = m_nodes + iD; + b2TreeNode * E = m_nodes + iE; + b2Assert(0 <= iD && iD < m_nodeCapacity); + b2Assert(0 <= iE && iE < m_nodeCapacity); + + // Swap A and B + B->child1 = iA; + B->parent = A->parent; + A->parent = iB; + + // A's old parent should point to B + if (B->parent != b2_nullNode) { + if (m_nodes[B->parent].child1 == iA) { + m_nodes[B->parent].child1 = iB; + } else { + b2Assert(m_nodes[B->parent].child2 == iA); + m_nodes[B->parent].child2 = iB; + } + } else { + m_root = iB; + } + + // Rotate + if (D->height > E->height) { + B->child2 = iD; + A->child1 = iE; + E->parent = iA; + A->aabb.Combine(C->aabb, E->aabb); + B->aabb.Combine(A->aabb, D->aabb); + + A->height = 1 + b2Max(C->height, E->height); + B->height = 1 + b2Max(A->height, D->height); + } else { + B->child2 = iE; + A->child1 = iD; + D->parent = iA; + A->aabb.Combine(C->aabb, D->aabb); + B->aabb.Combine(A->aabb, E->aabb); + + A->height = 1 + b2Max(C->height, D->height); + B->height = 1 + b2Max(A->height, E->height); + } + + return iB; + } + + return iA; } int32 b2DynamicTree::GetHeight() const { - if (m_root == b2_nullNode) - { - return 0; - } + if (m_root == b2_nullNode) { + return 0; + } - return m_nodes[m_root].height; + return m_nodes[m_root].height; } // float32 b2DynamicTree::GetAreaRatio() const { - if (m_root == b2_nullNode) - { - return 0.0f; - } - - const b2TreeNode* root = m_nodes + m_root; - float32 rootArea = root->aabb.GetPerimeter(); - - float32 totalArea = 0.0f; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height < 0) - { - // Free node in pool - continue; - } - - totalArea += node->aabb.GetPerimeter(); - } - - return totalArea / rootArea; + if (m_root == b2_nullNode) { + return 0.0f; + } + + const b2TreeNode * root = m_nodes + m_root; + float32 rootArea = root->aabb.GetPerimeter(); + + float32 totalArea = 0.0f; + for (int32 i = 0; i < m_nodeCapacity; ++i) { + const b2TreeNode * node = m_nodes + i; + if (node->height < 0) { + // Free node in pool + continue; + } + + totalArea += node->aabb.GetPerimeter(); + } + + return totalArea / rootArea; } // Compute the height of a sub-tree. int32 b2DynamicTree::ComputeHeight(int32 nodeId) const { - b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); - b2TreeNode* node = m_nodes + nodeId; + b2Assert(0 <= nodeId && nodeId < m_nodeCapacity); + b2TreeNode * node = m_nodes + nodeId; - if (node->IsLeaf()) - { - return 0; - } + if (node->IsLeaf()) { + return 0; + } - int32 height1 = ComputeHeight(node->child1); - int32 height2 = ComputeHeight(node->child2); - return 1 + b2Max(height1, height2); + int32 height1 = ComputeHeight(node->child1); + int32 height2 = ComputeHeight(node->child2); + return 1 + b2Max(height1, height2); } int32 b2DynamicTree::ComputeHeight() const { - int32 height = ComputeHeight(m_root); - return height; + int32 height = ComputeHeight(m_root); + return height; } void b2DynamicTree::ValidateStructure(int32 index) const { - if (index == b2_nullNode) - { - return; - } + if (index == b2_nullNode) { + return; + } - if (index == m_root) - { - b2Assert(m_nodes[index].parent == b2_nullNode); - } + if (index == m_root) { + b2Assert(m_nodes[index].parent == b2_nullNode); + } - const b2TreeNode* node = m_nodes + index; + const b2TreeNode * node = m_nodes + index; - int32 child1 = node->child1; - int32 child2 = node->child2; + int32 child1 = node->child1; + int32 child2 = node->child2; - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } + if (node->IsLeaf()) { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); - b2Assert(m_nodes[child1].parent == index); - b2Assert(m_nodes[child2].parent == index); + b2Assert(m_nodes[child1].parent == index); + b2Assert(m_nodes[child2].parent == index); - ValidateStructure(child1); - ValidateStructure(child2); + ValidateStructure(child1); + ValidateStructure(child2); } void b2DynamicTree::ValidateMetrics(int32 index) const { - if (index == b2_nullNode) - { - return; - } + if (index == b2_nullNode) { + return; + } - const b2TreeNode* node = m_nodes + index; + const b2TreeNode * node = m_nodes + index; - int32 child1 = node->child1; - int32 child2 = node->child2; + int32 child1 = node->child1; + int32 child2 = node->child2; - if (node->IsLeaf()) - { - b2Assert(child1 == b2_nullNode); - b2Assert(child2 == b2_nullNode); - b2Assert(node->height == 0); - return; - } + if (node->IsLeaf()) { + b2Assert(child1 == b2_nullNode); + b2Assert(child2 == b2_nullNode); + b2Assert(node->height == 0); + return; + } - b2Assert(0 <= child1 && child1 < m_nodeCapacity); - b2Assert(0 <= child2 && child2 < m_nodeCapacity); + b2Assert(0 <= child1 && child1 < m_nodeCapacity); + b2Assert(0 <= child2 && child2 < m_nodeCapacity); - int32 height1 = m_nodes[child1].height; - int32 height2 = m_nodes[child2].height; - int32 height; - height = 1 + b2Max(height1, height2); - b2Assert(node->height == height); + int32 height1 = m_nodes[child1].height; + int32 height2 = m_nodes[child2].height; + int32 height; + height = 1 + b2Max(height1, height2); + b2Assert(node->height == height); - b2AABB aabb; - aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); + b2AABB aabb; + aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb); - b2Assert(aabb.lowerBound == node->aabb.lowerBound); - b2Assert(aabb.upperBound == node->aabb.upperBound); + b2Assert(aabb.lowerBound == node->aabb.lowerBound); + b2Assert(aabb.upperBound == node->aabb.upperBound); - ValidateMetrics(child1); - ValidateMetrics(child2); + ValidateMetrics(child1); + ValidateMetrics(child2); } void b2DynamicTree::Validate() const { #if defined(b2DEBUG) - ValidateStructure(m_root); - ValidateMetrics(m_root); + ValidateStructure(m_root); + ValidateMetrics(m_root); - int32 freeCount = 0; - int32 freeIndex = m_freeList; - while (freeIndex != b2_nullNode) - { - b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); - freeIndex = m_nodes[freeIndex].next; - ++freeCount; - } + int32 freeCount = 0; + int32 freeIndex = m_freeList; + while (freeIndex != b2_nullNode) { + b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity); + freeIndex = m_nodes[freeIndex].next; + ++freeCount; + } - b2Assert(GetHeight() == ComputeHeight()); + b2Assert(GetHeight() == ComputeHeight()); - b2Assert(m_nodeCount + freeCount == m_nodeCapacity); + b2Assert(m_nodeCount + freeCount == m_nodeCapacity); #endif } int32 b2DynamicTree::GetMaxBalance() const { - int32 maxBalance = 0; - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - const b2TreeNode* node = m_nodes + i; - if (node->height <= 1) - { - continue; - } - - b2Assert(node->IsLeaf() == false); - - int32 child1 = node->child1; - int32 child2 = node->child2; - int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); - maxBalance = b2Max(maxBalance, balance); - } - - return maxBalance; + int32 maxBalance = 0; + for (int32 i = 0; i < m_nodeCapacity; ++i) { + const b2TreeNode * node = m_nodes + i; + if (node->height <= 1) { + continue; + } + + b2Assert(node->IsLeaf() == false); + + int32 child1 = node->child1; + int32 child2 = node->child2; + int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height); + maxBalance = b2Max(maxBalance, balance); + } + + return maxBalance; } void b2DynamicTree::RebuildBottomUp() { - int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32)); - int32 count = 0; - - // Build array of leaves. Free the rest. - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - if (m_nodes[i].height < 0) - { - // free node in pool - continue; - } - - if (m_nodes[i].IsLeaf()) - { - m_nodes[i].parent = b2_nullNode; - nodes[count] = i; - ++count; - } - else - { - FreeNode(i); - } - } - - while (count > 1) - { - float32 minCost = b2_maxFloat; - int32 iMin = -1, jMin = -1; - for (int32 i = 0; i < count; ++i) - { - b2AABB aabbi = m_nodes[nodes[i]].aabb; - - for (int32 j = i + 1; j < count; ++j) - { - b2AABB aabbj = m_nodes[nodes[j]].aabb; - b2AABB b; - b.Combine(aabbi, aabbj); - float32 cost = b.GetPerimeter(); - if (cost < minCost) - { - iMin = i; - jMin = j; - minCost = cost; - } - } - } - - int32 index1 = nodes[iMin]; - int32 index2 = nodes[jMin]; - b2TreeNode* child1 = m_nodes + index1; - b2TreeNode* child2 = m_nodes + index2; - - int32 parentIndex = AllocateNode(); - b2TreeNode* parent = m_nodes + parentIndex; - parent->child1 = index1; - parent->child2 = index2; - parent->height = 1 + b2Max(child1->height, child2->height); - parent->aabb.Combine(child1->aabb, child2->aabb); - parent->parent = b2_nullNode; - - child1->parent = parentIndex; - child2->parent = parentIndex; - - nodes[jMin] = nodes[count-1]; - nodes[iMin] = parentIndex; - --count; - } - - m_root = nodes[0]; - b2Free(nodes); - - Validate(); + int32 * nodes = (int32 *)b2Alloc(m_nodeCount * sizeof(int32)); + int32 count = 0; + + // Build array of leaves. Free the rest. + for (int32 i = 0; i < m_nodeCapacity; ++i) { + if (m_nodes[i].height < 0) { + // free node in pool + continue; + } + + if (m_nodes[i].IsLeaf()) { + m_nodes[i].parent = b2_nullNode; + nodes[count] = i; + ++count; + } else { + FreeNode(i); + } + } + + while (count > 1) { + float32 minCost = b2_maxFloat; + int32 iMin = -1, jMin = -1; + for (int32 i = 0; i < count; ++i) { + b2AABB aabbi = m_nodes[nodes[i]].aabb; + + for (int32 j = i + 1; j < count; ++j) { + b2AABB aabbj = m_nodes[nodes[j]].aabb; + b2AABB b; + b.Combine(aabbi, aabbj); + float32 cost = b.GetPerimeter(); + if (cost < minCost) { + iMin = i; + jMin = j; + minCost = cost; + } + } + } + + int32 index1 = nodes[iMin]; + int32 index2 = nodes[jMin]; + b2TreeNode * child1 = m_nodes + index1; + b2TreeNode * child2 = m_nodes + index2; + + int32 parentIndex = AllocateNode(); + b2TreeNode * parent = m_nodes + parentIndex; + parent->child1 = index1; + parent->child2 = index2; + parent->height = 1 + b2Max(child1->height, child2->height); + parent->aabb.Combine(child1->aabb, child2->aabb); + parent->parent = b2_nullNode; + + child1->parent = parentIndex; + child2->parent = parentIndex; + + nodes[jMin] = nodes[count - 1]; + nodes[iMin] = parentIndex; + --count; + } + + m_root = nodes[0]; + b2Free(nodes); + + Validate(); } -void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin) +void b2DynamicTree::ShiftOrigin(const b2Vec2 & newOrigin) { - // Build array of leaves. Free the rest. - for (int32 i = 0; i < m_nodeCapacity; ++i) - { - m_nodes[i].aabb.lowerBound -= newOrigin; - m_nodes[i].aabb.upperBound -= newOrigin; - } + // Build array of leaves. Free the rest. + for (int32 i = 0; i < m_nodeCapacity; ++i) { + m_nodes[i].aabb.lowerBound -= newOrigin; + m_nodes[i].aabb.upperBound -= newOrigin; + } } diff --git a/flatland_box2d/src/Collision/b2TimeOfImpact.cpp b/flatland_box2d/src/Collision/b2TimeOfImpact.cpp index a255973a..338fc9b7 100644 --- a/flatland_box2d/src/Collision/b2TimeOfImpact.cpp +++ b/flatland_box2d/src/Collision/b2TimeOfImpact.cpp @@ -1,30 +1,31 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ -#include "Box2D/Collision/b2Collision.h" -#include "Box2D/Collision/b2Distance.h" #include "Box2D/Collision/b2TimeOfImpact.h" + +#include + #include "Box2D/Collision/Shapes/b2CircleShape.h" #include "Box2D/Collision/Shapes/b2PolygonShape.h" +#include "Box2D/Collision/b2Collision.h" +#include "Box2D/Collision/b2Distance.h" #include "Box2D/Common/b2Timer.h" -#include - float32 b2_toiTime, b2_toiMaxTime; int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters; int32 b2_toiRootIters, b2_toiMaxRootIters; @@ -32,299 +33,275 @@ int32 b2_toiRootIters, b2_toiMaxRootIters; // struct b2SeparationFunction { - enum Type - { - e_points, - e_faceA, - e_faceB - }; - - // TODO_ERIN might not need to return the separation - - float32 Initialize(const b2SimplexCache* cache, - const b2DistanceProxy* proxyA, const b2Sweep& sweepA, - const b2DistanceProxy* proxyB, const b2Sweep& sweepB, - float32 t1) - { - m_proxyA = proxyA; - m_proxyB = proxyB; - int32 count = cache->count; - b2Assert(0 < count && count < 3); - - m_sweepA = sweepA; - m_sweepB = sweepB; - - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t1); - m_sweepB.GetTransform(&xfB, t1); - - if (count == 1) - { - m_type = e_points; - b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - m_axis = pointB - pointA; - float32 s = m_axis.Normalize(); - return s; - } - else if (cache->indexA[0] == cache->indexA[1]) - { - // Two points on B and one on A. - m_type = e_faceB; - b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); - b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); - - m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfB.q, m_axis); + enum Type { e_points, e_faceA, e_faceB }; + + // TODO_ERIN might not need to return the separation + + float32 Initialize( + const b2SimplexCache * cache, const b2DistanceProxy * proxyA, const b2Sweep & sweepA, + const b2DistanceProxy * proxyB, const b2Sweep & sweepB, float32 t1) + { + m_proxyA = proxyA; + m_proxyB = proxyB; + int32 count = cache->count; + b2Assert(0 < count && count < 3); + + m_sweepA = sweepA; + m_sweepB = sweepB; + + b2Transform xfA, xfB; + m_sweepA.GetTransform(&xfA, t1); + m_sweepB.GetTransform(&xfB, t1); + + if (count == 1) { + m_type = e_points; + b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]); + b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); + b2Vec2 pointA = b2Mul(xfA, localPointA); + b2Vec2 pointB = b2Mul(xfB, localPointB); + m_axis = pointB - pointA; + float32 s = m_axis.Normalize(); + return s; + } else if (cache->indexA[0] == cache->indexA[1]) { + // Two points on B and one on A. + m_type = e_faceB; + b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]); + b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]); + + m_axis = b2Cross(localPointB2 - localPointB1, 1.0f); + m_axis.Normalize(); + b2Vec2 normal = b2Mul(xfB.q, m_axis); + + m_localPoint = 0.5f * (localPointB1 + localPointB2); + b2Vec2 pointB = b2Mul(xfB, m_localPoint); + + b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); + b2Vec2 pointA = b2Mul(xfA, localPointA); + + float32 s = b2Dot(pointA - pointB, normal); + if (s < 0.0f) { + m_axis = -m_axis; + s = -s; + } + return s; + } else { + // Two points on A and one or two points on B. + m_type = e_faceA; + b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); + b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); + + m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); + m_axis.Normalize(); + b2Vec2 normal = b2Mul(xfA.q, m_axis); + + m_localPoint = 0.5f * (localPointA1 + localPointA2); + b2Vec2 pointA = b2Mul(xfA, m_localPoint); + + b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float32 s = b2Dot(pointB - pointA, normal); + if (s < 0.0f) { + m_axis = -m_axis; + s = -s; + } + return s; + } + } + + // + float32 FindMinSeparation(int32 * indexA, int32 * indexB, float32 t) const + { + b2Transform xfA, xfB; + m_sweepA.GetTransform(&xfA, t); + m_sweepB.GetTransform(&xfB, t); + + switch (m_type) { + case e_points: { + b2Vec2 axisA = b2MulT(xfA.q, m_axis); + b2Vec2 axisB = b2MulT(xfB.q, -m_axis); + + *indexA = m_proxyA->GetSupport(axisA); + *indexB = m_proxyB->GetSupport(axisB); + + b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); + b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); + + b2Vec2 pointA = b2Mul(xfA, localPointA); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float32 separation = b2Dot(pointB - pointA, m_axis); + return separation; + } + + case e_faceA: { + b2Vec2 normal = b2Mul(xfA.q, m_axis); + b2Vec2 pointA = b2Mul(xfA, m_localPoint); + + b2Vec2 axisB = b2MulT(xfB.q, -normal); + + *indexA = -1; + *indexB = m_proxyB->GetSupport(axisB); + + b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float32 separation = b2Dot(pointB - pointA, normal); + return separation; + } + + case e_faceB: { + b2Vec2 normal = b2Mul(xfB.q, m_axis); + b2Vec2 pointB = b2Mul(xfB, m_localPoint); + + b2Vec2 axisA = b2MulT(xfA.q, -normal); + + *indexB = -1; + *indexA = m_proxyA->GetSupport(axisA); + + b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); + b2Vec2 pointA = b2Mul(xfA, localPointA); + + float32 separation = b2Dot(pointA - pointB, normal); + return separation; + } + + default: + b2Assert(false); + *indexA = -1; + *indexB = -1; + return 0.0f; + } + } + + // + float32 Evaluate(int32 indexA, int32 indexB, float32 t) const + { + b2Transform xfA, xfB; + m_sweepA.GetTransform(&xfA, t); + m_sweepB.GetTransform(&xfB, t); + + switch (m_type) { + case e_points: { + b2Vec2 localPointA = m_proxyA->GetVertex(indexA); + b2Vec2 localPointB = m_proxyB->GetVertex(indexB); + + b2Vec2 pointA = b2Mul(xfA, localPointA); + b2Vec2 pointB = b2Mul(xfB, localPointB); + float32 separation = b2Dot(pointB - pointA, m_axis); + + return separation; + } + + case e_faceA: { + b2Vec2 normal = b2Mul(xfA.q, m_axis); + b2Vec2 pointA = b2Mul(xfA, m_localPoint); + + b2Vec2 localPointB = m_proxyB->GetVertex(indexB); + b2Vec2 pointB = b2Mul(xfB, localPointB); + + float32 separation = b2Dot(pointB - pointA, normal); + return separation; + } + + case e_faceB: { + b2Vec2 normal = b2Mul(xfB.q, m_axis); + b2Vec2 pointB = b2Mul(xfB, m_localPoint); + + b2Vec2 localPointA = m_proxyA->GetVertex(indexA); + b2Vec2 pointA = b2Mul(xfA, localPointA); + + float32 separation = b2Dot(pointA - pointB, normal); + return separation; + } - m_localPoint = 0.5f * (localPointB1 + localPointB2); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 s = b2Dot(pointA - pointB, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - else - { - // Two points on A and one or two points on B. - m_type = e_faceA; - b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]); - b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]); - - m_axis = b2Cross(localPointA2 - localPointA1, 1.0f); - m_axis.Normalize(); - b2Vec2 normal = b2Mul(xfA.q, m_axis); - - m_localPoint = 0.5f * (localPointA1 + localPointA2); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 s = b2Dot(pointB - pointA, normal); - if (s < 0.0f) - { - m_axis = -m_axis; - s = -s; - } - return s; - } - } - - // - float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { - b2Vec2 axisA = b2MulT(xfA.q, m_axis); - b2Vec2 axisB = b2MulT(xfB.q, -m_axis); + default: + b2Assert(false); + return 0.0f; + } + } - *indexA = m_proxyA->GetSupport(axisA); - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, m_axis); - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 axisB = b2MulT(xfB.q, -normal); - - *indexA = -1; - *indexB = m_proxyB->GetSupport(axisB); - - b2Vec2 localPointB = m_proxyB->GetVertex(*indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 axisA = b2MulT(xfA.q, -normal); - - *indexB = -1; - *indexA = m_proxyA->GetSupport(axisA); - - b2Vec2 localPointA = m_proxyA->GetVertex(*indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - *indexA = -1; - *indexB = -1; - return 0.0f; - } - } - - // - float32 Evaluate(int32 indexA, int32 indexB, float32 t) const - { - b2Transform xfA, xfB; - m_sweepA.GetTransform(&xfA, t); - m_sweepB.GetTransform(&xfB, t); - - switch (m_type) - { - case e_points: - { - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - - b2Vec2 pointA = b2Mul(xfA, localPointA); - b2Vec2 pointB = b2Mul(xfB, localPointB); - float32 separation = b2Dot(pointB - pointA, m_axis); - - return separation; - } - - case e_faceA: - { - b2Vec2 normal = b2Mul(xfA.q, m_axis); - b2Vec2 pointA = b2Mul(xfA, m_localPoint); - - b2Vec2 localPointB = m_proxyB->GetVertex(indexB); - b2Vec2 pointB = b2Mul(xfB, localPointB); - - float32 separation = b2Dot(pointB - pointA, normal); - return separation; - } - - case e_faceB: - { - b2Vec2 normal = b2Mul(xfB.q, m_axis); - b2Vec2 pointB = b2Mul(xfB, m_localPoint); - - b2Vec2 localPointA = m_proxyA->GetVertex(indexA); - b2Vec2 pointA = b2Mul(xfA, localPointA); - - float32 separation = b2Dot(pointA - pointB, normal); - return separation; - } - - default: - b2Assert(false); - return 0.0f; - } - } - - const b2DistanceProxy* m_proxyA; - const b2DistanceProxy* m_proxyB; - b2Sweep m_sweepA, m_sweepB; - Type m_type; - b2Vec2 m_localPoint; - b2Vec2 m_axis; + const b2DistanceProxy * m_proxyA; + const b2DistanceProxy * m_proxyB; + b2Sweep m_sweepA, m_sweepB; + Type m_type; + b2Vec2 m_localPoint; + b2Vec2 m_axis; }; // CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. -void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) +void b2TimeOfImpact(b2TOIOutput * output, const b2TOIInput * input) { - b2Timer timer; - - ++b2_toiCalls; - - output->state = b2TOIOutput::e_unknown; - output->t = input->tMax; - - const b2DistanceProxy* proxyA = &input->proxyA; - const b2DistanceProxy* proxyB = &input->proxyB; - - b2Sweep sweepA = input->sweepA; - b2Sweep sweepB = input->sweepB; - - // Large rotations can make the root finder fail, so we normalize the - // sweep angles. - sweepA.Normalize(); - sweepB.Normalize(); - - float32 tMax = input->tMax; - - float32 totalRadius = proxyA->m_radius + proxyB->m_radius; - float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); - float32 tolerance = 0.25f * b2_linearSlop; - b2Assert(target > tolerance); - - float32 t1 = 0.0f; - const int32 k_maxIterations = 20; // TODO_ERIN b2Settings - int32 iter = 0; - - // Prepare input for distance query. - b2SimplexCache cache; - cache.count = 0; - b2DistanceInput distanceInput; - distanceInput.proxyA = input->proxyA; - distanceInput.proxyB = input->proxyB; - distanceInput.useRadii = false; - - // The outer loop progressively attempts to compute new separating axes. - // This loop terminates when an axis is repeated (no progress is made). - for(;;) - { - b2Transform xfA, xfB; - sweepA.GetTransform(&xfA, t1); - sweepB.GetTransform(&xfB, t1); - - // Get the distance between shapes. We can also use the results - // to get a separating axis. - distanceInput.transformA = xfA; - distanceInput.transformB = xfB; - b2DistanceOutput distanceOutput; - b2Distance(&distanceOutput, &cache, &distanceInput); - - // If the shapes are overlapped, we give up on continuous collision. - if (distanceOutput.distance <= 0.0f) - { - // Failure! - output->state = b2TOIOutput::e_overlapped; - output->t = 0.0f; - break; - } - - if (distanceOutput.distance < target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_touching; - output->t = t1; - break; - } - - // Initialize the separating axis. - b2SeparationFunction fcn; - fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); + b2Timer timer; + + ++b2_toiCalls; + + output->state = b2TOIOutput::e_unknown; + output->t = input->tMax; + + const b2DistanceProxy * proxyA = &input->proxyA; + const b2DistanceProxy * proxyB = &input->proxyB; + + b2Sweep sweepA = input->sweepA; + b2Sweep sweepB = input->sweepB; + + // Large rotations can make the root finder fail, so we normalize the + // sweep angles. + sweepA.Normalize(); + sweepB.Normalize(); + + float32 tMax = input->tMax; + + float32 totalRadius = proxyA->m_radius + proxyB->m_radius; + float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop); + float32 tolerance = 0.25f * b2_linearSlop; + b2Assert(target > tolerance); + + float32 t1 = 0.0f; + const int32 k_maxIterations = 20; // TODO_ERIN b2Settings + int32 iter = 0; + + // Prepare input for distance query. + b2SimplexCache cache; + cache.count = 0; + b2DistanceInput distanceInput; + distanceInput.proxyA = input->proxyA; + distanceInput.proxyB = input->proxyB; + distanceInput.useRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for (;;) { + b2Transform xfA, xfB; + sweepA.GetTransform(&xfA, t1); + sweepB.GetTransform(&xfB, t1); + + // Get the distance between shapes. We can also use the results + // to get a separating axis. + distanceInput.transformA = xfA; + distanceInput.transformB = xfB; + b2DistanceOutput distanceOutput; + b2Distance(&distanceOutput, &cache, &distanceInput); + + // If the shapes are overlapped, we give up on continuous collision. + if (distanceOutput.distance <= 0.0f) { + // Failure! + output->state = b2TOIOutput::e_overlapped; + output->t = 0.0f; + break; + } + + if (distanceOutput.distance < target + tolerance) { + // Victory! + output->state = b2TOIOutput::e_touching; + output->t = t1; + break; + } + + // Initialize the separating axis. + b2SeparationFunction fcn; + fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); #if 0 // Dump the curve seen by the root finder { @@ -351,136 +328,120 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input) } #endif - // Compute the TOI on the separating axis. We do this by successively - // resolving the deepest point. This loop is bounded by the number of vertices. - bool done = false; - float32 t2 = tMax; - int32 pushBackIter = 0; - for (;;) - { - // Find the deepest point at t2. Store the witness point indices. - int32 indexA, indexB; - float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); - - // Is the final configuration separated? - if (s2 > target + tolerance) - { - // Victory! - output->state = b2TOIOutput::e_separated; - output->t = tMax; - done = true; - break; - } - - // Has the separation reached tolerance? - if (s2 > target - tolerance) - { - // Advance the sweeps - t1 = t2; - break; - } - - // Compute the initial separation of the witness points. - float32 s1 = fcn.Evaluate(indexA, indexB, t1); - - // Check for initial overlap. This might happen if the root finder - // runs out of iterations. - if (s1 < target - tolerance) - { - output->state = b2TOIOutput::e_failed; - output->t = t1; - done = true; - break; - } - - // Check for touching - if (s1 <= target + tolerance) - { - // Victory! t1 should hold the TOI (could be 0.0). - output->state = b2TOIOutput::e_touching; - output->t = t1; - done = true; - break; - } - - // Compute 1D root of: f(x) - target = 0 - int32 rootIterCount = 0; - float32 a1 = t1, a2 = t2; - for (;;) - { - // Use a mix of the secant rule and bisection. - float32 t; - if (rootIterCount & 1) - { - // Secant rule to improve convergence. - t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); - } - else - { - // Bisection to guarantee progress. - t = 0.5f * (a1 + a2); - } - - ++rootIterCount; - ++b2_toiRootIters; - - float32 s = fcn.Evaluate(indexA, indexB, t); - - if (b2Abs(s - target) < tolerance) - { - // t2 holds a tentative value for t1 - t2 = t; - break; - } - - // Ensure we continue to bracket the root. - if (s > target) - { - a1 = t; - s1 = s; - } - else - { - a2 = t; - s2 = s; - } - - if (rootIterCount == 50) - { - break; - } - } - - b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); - - ++pushBackIter; - - if (pushBackIter == b2_maxPolygonVertices) - { - break; - } - } - - ++iter; - ++b2_toiIters; - - if (done) - { - break; - } - - if (iter == k_maxIterations) - { - // Root finder got stuck. Semi-victory. - output->state = b2TOIOutput::e_failed; - output->t = t1; - break; - } - } - - b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); - - float32 time = timer.GetMilliseconds(); - b2_toiMaxTime = b2Max(b2_toiMaxTime, time); - b2_toiTime += time; + // Compute the TOI on the separating axis. We do this by successively + // resolving the deepest point. This loop is bounded by the number of + // vertices. + bool done = false; + float32 t2 = tMax; + int32 pushBackIter = 0; + for (;;) { + // Find the deepest point at t2. Store the witness point indices. + int32 indexA, indexB; + float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2); + + // Is the final configuration separated? + if (s2 > target + tolerance) { + // Victory! + output->state = b2TOIOutput::e_separated; + output->t = tMax; + done = true; + break; + } + + // Has the separation reached tolerance? + if (s2 > target - tolerance) { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points. + float32 s1 = fcn.Evaluate(indexA, indexB, t1); + + // Check for initial overlap. This might happen if the root finder + // runs out of iterations. + if (s1 < target - tolerance) { + output->state = b2TOIOutput::e_failed; + output->t = t1; + done = true; + break; + } + + // Check for touching + if (s1 <= target + tolerance) { + // Victory! t1 should hold the TOI (could be 0.0). + output->state = b2TOIOutput::e_touching; + output->t = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int32 rootIterCount = 0; + float32 a1 = t1, a2 = t2; + for (;;) { + // Use a mix of the secant rule and bisection. + float32 t; + if (rootIterCount & 1) { + // Secant rule to improve convergence. + t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); + } else { + // Bisection to guarantee progress. + t = 0.5f * (a1 + a2); + } + + ++rootIterCount; + ++b2_toiRootIters; + + float32 s = fcn.Evaluate(indexA, indexB, t); + + if (b2Abs(s - target) < tolerance) { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if (s > target) { + a1 = t; + s1 = s; + } else { + a2 = t; + s2 = s; + } + + if (rootIterCount == 50) { + break; + } + } + + b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount); + + ++pushBackIter; + + if (pushBackIter == b2_maxPolygonVertices) { + break; + } + } + + ++iter; + ++b2_toiIters; + + if (done) { + break; + } + + if (iter == k_maxIterations) { + // Root finder got stuck. Semi-victory. + output->state = b2TOIOutput::e_failed; + output->t = t1; + break; + } + } + + b2_toiMaxIters = b2Max(b2_toiMaxIters, iter); + + float32 time = timer.GetMilliseconds(); + b2_toiMaxTime = b2Max(b2_toiMaxTime, time); + b2_toiTime += time; } diff --git a/flatland_box2d/src/Common/b2BlockAllocator.cpp b/flatland_box2d/src/Common/b2BlockAllocator.cpp index b916cc61..00a9b973 100644 --- a/flatland_box2d/src/Common/b2BlockAllocator.cpp +++ b/flatland_box2d/src/Common/b2BlockAllocator.cpp @@ -1,215 +1,197 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Common/b2BlockAllocator.h" + #include -#include #include +#include -int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = -{ - 16, // 0 - 32, // 1 - 64, // 2 - 96, // 3 - 128, // 4 - 160, // 5 - 192, // 6 - 224, // 7 - 256, // 8 - 320, // 9 - 384, // 10 - 448, // 11 - 512, // 12 - 640, // 13 +int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = { + 16, // 0 + 32, // 1 + 64, // 2 + 96, // 3 + 128, // 4 + 160, // 5 + 192, // 6 + 224, // 7 + 256, // 8 + 320, // 9 + 384, // 10 + 448, // 11 + 512, // 12 + 640, // 13 }; uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1]; bool b2BlockAllocator::s_blockSizeLookupInitialized; struct b2Chunk { - int32 blockSize; - b2Block* blocks; + int32 blockSize; + b2Block * blocks; }; struct b2Block { - b2Block* next; + b2Block * next; }; b2BlockAllocator::b2BlockAllocator() { - b2Assert(b2_blockSizes < UCHAR_MAX); - - m_chunkSpace = b2_chunkArrayIncrement; - m_chunkCount = 0; - m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); - - memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); - memset(m_freeLists, 0, sizeof(m_freeLists)); - - if (s_blockSizeLookupInitialized == false) - { - int32 j = 0; - for (int32 i = 1; i <= b2_maxBlockSize; ++i) - { - b2Assert(j < b2_blockSizes); - if (i <= s_blockSizes[j]) - { - s_blockSizeLookup[i] = (uint8)j; - } - else - { - ++j; - s_blockSizeLookup[i] = (uint8)j; - } - } - - s_blockSizeLookupInitialized = true; - } + b2Assert(b2_blockSizes < UCHAR_MAX); + + m_chunkSpace = b2_chunkArrayIncrement; + m_chunkCount = 0; + m_chunks = (b2Chunk *)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); + + memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); + memset(m_freeLists, 0, sizeof(m_freeLists)); + + if (s_blockSizeLookupInitialized == false) { + int32 j = 0; + for (int32 i = 1; i <= b2_maxBlockSize; ++i) { + b2Assert(j < b2_blockSizes); + if (i <= s_blockSizes[j]) { + s_blockSizeLookup[i] = (uint8)j; + } else { + ++j; + s_blockSizeLookup[i] = (uint8)j; + } + } + + s_blockSizeLookupInitialized = true; + } } b2BlockAllocator::~b2BlockAllocator() { - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Free(m_chunks[i].blocks); - } + for (int32 i = 0; i < m_chunkCount; ++i) { + b2Free(m_chunks[i].blocks); + } - b2Free(m_chunks); + b2Free(m_chunks); } -void* b2BlockAllocator::Allocate(int32 size) +void * b2BlockAllocator::Allocate(int32 size) { - if (size == 0) - return nullptr; - - b2Assert(0 < size); - - if (size > b2_maxBlockSize) - { - return b2Alloc(size); - } - - int32 index = s_blockSizeLookup[size]; - b2Assert(0 <= index && index < b2_blockSizes); - - if (m_freeLists[index]) - { - b2Block* block = m_freeLists[index]; - m_freeLists[index] = block->next; - return block; - } - else - { - if (m_chunkCount == m_chunkSpace) - { - b2Chunk* oldChunks = m_chunks; - m_chunkSpace += b2_chunkArrayIncrement; - m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); - memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk)); - memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk)); - b2Free(oldChunks); - } - - b2Chunk* chunk = m_chunks + m_chunkCount; - chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize); + if (size == 0) return nullptr; + + b2Assert(0 < size); + + if (size > b2_maxBlockSize) { + return b2Alloc(size); + } + + int32 index = s_blockSizeLookup[size]; + b2Assert(0 <= index && index < b2_blockSizes); + + if (m_freeLists[index]) { + b2Block * block = m_freeLists[index]; + m_freeLists[index] = block->next; + return block; + } else { + if (m_chunkCount == m_chunkSpace) { + b2Chunk * oldChunks = m_chunks; + m_chunkSpace += b2_chunkArrayIncrement; + m_chunks = (b2Chunk *)b2Alloc(m_chunkSpace * sizeof(b2Chunk)); + memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk)); + memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk)); + b2Free(oldChunks); + } + + b2Chunk * chunk = m_chunks + m_chunkCount; + chunk->blocks = (b2Block *)b2Alloc(b2_chunkSize); #if defined(_DEBUG) - memset(chunk->blocks, 0xcd, b2_chunkSize); + memset(chunk->blocks, 0xcd, b2_chunkSize); #endif - int32 blockSize = s_blockSizes[index]; - chunk->blockSize = blockSize; - int32 blockCount = b2_chunkSize / blockSize; - b2Assert(blockCount * blockSize <= b2_chunkSize); - for (int32 i = 0; i < blockCount - 1; ++i) - { - b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i); - b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1)); - block->next = next; - } - b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1)); - last->next = nullptr; - - m_freeLists[index] = chunk->blocks->next; - ++m_chunkCount; - - return chunk->blocks; - } + int32 blockSize = s_blockSizes[index]; + chunk->blockSize = blockSize; + int32 blockCount = b2_chunkSize / blockSize; + b2Assert(blockCount * blockSize <= b2_chunkSize); + for (int32 i = 0; i < blockCount - 1; ++i) { + b2Block * block = (b2Block *)((int8 *)chunk->blocks + blockSize * i); + b2Block * next = (b2Block *)((int8 *)chunk->blocks + blockSize * (i + 1)); + block->next = next; + } + b2Block * last = (b2Block *)((int8 *)chunk->blocks + blockSize * (blockCount - 1)); + last->next = nullptr; + + m_freeLists[index] = chunk->blocks->next; + ++m_chunkCount; + + return chunk->blocks; + } } -void b2BlockAllocator::Free(void* p, int32 size) +void b2BlockAllocator::Free(void * p, int32 size) { - if (size == 0) - { - return; - } + if (size == 0) { + return; + } - b2Assert(0 < size); + b2Assert(0 < size); - if (size > b2_maxBlockSize) - { - b2Free(p); - return; - } + if (size > b2_maxBlockSize) { + b2Free(p); + return; + } - int32 index = s_blockSizeLookup[size]; - b2Assert(0 <= index && index < b2_blockSizes); + int32 index = s_blockSizeLookup[size]; + b2Assert(0 <= index && index < b2_blockSizes); #ifdef _DEBUG - // Verify the memory address and size is valid. - int32 blockSize = s_blockSizes[index]; - bool found = false; - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Chunk* chunk = m_chunks + i; - if (chunk->blockSize != blockSize) - { - b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks || - (int8*)chunk->blocks + b2_chunkSize <= (int8*)p); - } - else - { - if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize) - { - found = true; - } - } - } - - b2Assert(found); - - memset(p, 0xfd, blockSize); + // Verify the memory address and size is valid. + int32 blockSize = s_blockSizes[index]; + bool found = false; + for (int32 i = 0; i < m_chunkCount; ++i) { + b2Chunk * chunk = m_chunks + i; + if (chunk->blockSize != blockSize) { + b2Assert( + (int8 *)p + blockSize <= (int8 *)chunk->blocks || + (int8 *)chunk->blocks + b2_chunkSize <= (int8 *)p); + } else { + if ( + (int8 *)chunk->blocks <= (int8 *)p && + (int8 *)p + blockSize <= (int8 *)chunk->blocks + b2_chunkSize) { + found = true; + } + } + } + + b2Assert(found); + + memset(p, 0xfd, blockSize); #endif - b2Block* block = (b2Block*)p; - block->next = m_freeLists[index]; - m_freeLists[index] = block; + b2Block * block = (b2Block *)p; + block->next = m_freeLists[index]; + m_freeLists[index] = block; } void b2BlockAllocator::Clear() { - for (int32 i = 0; i < m_chunkCount; ++i) - { - b2Free(m_chunks[i].blocks); - } + for (int32 i = 0; i < m_chunkCount; ++i) { + b2Free(m_chunks[i].blocks); + } - m_chunkCount = 0; - memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); + m_chunkCount = 0; + memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk)); - memset(m_freeLists, 0, sizeof(m_freeLists)); + memset(m_freeLists, 0, sizeof(m_freeLists)); } diff --git a/flatland_box2d/src/Common/b2Draw.cpp b/flatland_box2d/src/Common/b2Draw.cpp index 354fb8d2..4c457be2 100644 --- a/flatland_box2d/src/Common/b2Draw.cpp +++ b/flatland_box2d/src/Common/b2Draw.cpp @@ -1,44 +1,29 @@ /* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2011 Erin Catto http://box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Common/b2Draw.h" -b2Draw::b2Draw() -{ - m_drawFlags = 0; -} +b2Draw::b2Draw() { m_drawFlags = 0; } -void b2Draw::SetFlags(uint32 flags) -{ - m_drawFlags = flags; -} +void b2Draw::SetFlags(uint32 flags) { m_drawFlags = flags; } -uint32 b2Draw::GetFlags() const -{ - return m_drawFlags; -} +uint32 b2Draw::GetFlags() const { return m_drawFlags; } -void b2Draw::AppendFlags(uint32 flags) -{ - m_drawFlags |= flags; -} +void b2Draw::AppendFlags(uint32 flags) { m_drawFlags |= flags; } -void b2Draw::ClearFlags(uint32 flags) -{ - m_drawFlags &= ~flags; -} +void b2Draw::ClearFlags(uint32 flags) { m_drawFlags &= ~flags; } diff --git a/flatland_box2d/src/Common/b2Math.cpp b/flatland_box2d/src/Common/b2Math.cpp index 258035e1..e921c5e3 100644 --- a/flatland_box2d/src/Common/b2Math.cpp +++ b/flatland_box2d/src/Common/b2Math.cpp @@ -1,20 +1,20 @@ /* -* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Common/b2Math.h" @@ -22,73 +22,75 @@ const b2Vec2 b2Vec2_zero(0.0f, 0.0f); /// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. -b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const +b2Vec3 b2Mat33::Solve33(const b2Vec3 & b) const { - float32 det = b2Dot(ex, b2Cross(ey, ez)); - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec3 x; - x.x = det * b2Dot(b, b2Cross(ey, ez)); - x.y = det * b2Dot(ex, b2Cross(b, ez)); - x.z = det * b2Dot(ex, b2Cross(ey, b)); - return x; + float32 det = b2Dot(ex, b2Cross(ey, ez)); + if (det != 0.0f) { + det = 1.0f / det; + } + b2Vec3 x; + x.x = det * b2Dot(b, b2Cross(ey, ez)); + x.y = det * b2Dot(ex, b2Cross(b, ez)); + x.z = det * b2Dot(ex, b2Cross(ey, b)); + return x; } /// Solve A * x = b, where b is a column vector. This is more efficient /// than computing the inverse in one-shot cases. -b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const +b2Vec2 b2Mat33::Solve22(const b2Vec2 & b) const { - float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; - float32 det = a11 * a22 - a12 * a21; - if (det != 0.0f) - { - det = 1.0f / det; - } - b2Vec2 x; - x.x = det * (a22 * b.x - a12 * b.y); - x.y = det * (a11 * b.y - a21 * b.x); - return x; + float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y; + float32 det = a11 * a22 - a12 * a21; + if (det != 0.0f) { + det = 1.0f / det; + } + b2Vec2 x; + x.x = det * (a22 * b.x - a12 * b.y); + x.y = det * (a11 * b.y - a21 * b.x); + return x; } /// -void b2Mat33::GetInverse22(b2Mat33* M) const +void b2Mat33::GetInverse22(b2Mat33 * M) const { - float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; - float32 det = a * d - b * c; - if (det != 0.0f) - { - det = 1.0f / det; - } + float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y; + float32 det = a * d - b * c; + if (det != 0.0f) { + det = 1.0f / det; + } - M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f; - M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f; - M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f; + M->ex.x = det * d; + M->ey.x = -det * b; + M->ex.z = 0.0f; + M->ex.y = -det * c; + M->ey.y = det * a; + M->ey.z = 0.0f; + M->ez.x = 0.0f; + M->ez.y = 0.0f; + M->ez.z = 0.0f; } /// Returns the zero matrix if singular. -void b2Mat33::GetSymInverse33(b2Mat33* M) const +void b2Mat33::GetSymInverse33(b2Mat33 * M) const { - float32 det = b2Dot(ex, b2Cross(ey, ez)); - if (det != 0.0f) - { - det = 1.0f / det; - } + float32 det = b2Dot(ex, b2Cross(ey, ez)); + if (det != 0.0f) { + det = 1.0f / det; + } - float32 a11 = ex.x, a12 = ey.x, a13 = ez.x; - float32 a22 = ey.y, a23 = ez.y; - float32 a33 = ez.z; + float32 a11 = ex.x, a12 = ey.x, a13 = ez.x; + float32 a22 = ey.y, a23 = ez.y; + float32 a33 = ez.z; - M->ex.x = det * (a22 * a33 - a23 * a23); - M->ex.y = det * (a13 * a23 - a12 * a33); - M->ex.z = det * (a12 * a23 - a13 * a22); + M->ex.x = det * (a22 * a33 - a23 * a23); + M->ex.y = det * (a13 * a23 - a12 * a33); + M->ex.z = det * (a12 * a23 - a13 * a22); - M->ey.x = M->ex.y; - M->ey.y = det * (a11 * a33 - a13 * a13); - M->ey.z = det * (a13 * a12 - a11 * a23); + M->ey.x = M->ex.y; + M->ey.y = det * (a11 * a33 - a13 * a13); + M->ey.z = det * (a13 * a12 - a11 * a23); - M->ez.x = M->ex.z; - M->ez.y = M->ey.z; - M->ez.z = det * (a11 * a22 - a12 * a12); + M->ez.x = M->ex.z; + M->ez.y = M->ey.z; + M->ez.z = det * (a11 * a22 - a12 * a12); } diff --git a/flatland_box2d/src/Common/b2Settings.cpp b/flatland_box2d/src/Common/b2Settings.cpp index e2ddbdbe..dd84ccfc 100644 --- a/flatland_box2d/src/Common/b2Settings.cpp +++ b/flatland_box2d/src/Common/b2Settings.cpp @@ -1,44 +1,39 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Common/b2Settings.h" -#include + #include +#include #include b2Version b2_version = {2, 3, 2}; // Memory allocators. Modify these to use your own allocator. -void* b2Alloc(int32 size) -{ - return malloc(size); -} +void * b2Alloc(int32 size) { return malloc(size); } -void b2Free(void* mem) -{ - free(mem); -} +void b2Free(void * mem) { free(mem); } // You can modify this to use your logging facility. -void b2Log(const char* string, ...) +void b2Log(const char * string, ...) { - va_list args; - va_start(args, string); - vprintf(string, args); - va_end(args); + va_list args; + va_start(args, string); + vprintf(string, args); + va_end(args); } diff --git a/flatland_box2d/src/Common/b2StackAllocator.cpp b/flatland_box2d/src/Common/b2StackAllocator.cpp index 6923d0d9..7d8ce8f2 100644 --- a/flatland_box2d/src/Common/b2StackAllocator.cpp +++ b/flatland_box2d/src/Common/b2StackAllocator.cpp @@ -1,83 +1,75 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Common/b2StackAllocator.h" + #include "Box2D/Common/b2Math.h" b2StackAllocator::b2StackAllocator() { - m_index = 0; - m_allocation = 0; - m_maxAllocation = 0; - m_entryCount = 0; + m_index = 0; + m_allocation = 0; + m_maxAllocation = 0; + m_entryCount = 0; } b2StackAllocator::~b2StackAllocator() { - b2Assert(m_index == 0); - b2Assert(m_entryCount == 0); + b2Assert(m_index == 0); + b2Assert(m_entryCount == 0); } -void* b2StackAllocator::Allocate(int32 size) +void * b2StackAllocator::Allocate(int32 size) { - b2Assert(m_entryCount < b2_maxStackEntries); + b2Assert(m_entryCount < b2_maxStackEntries); - b2StackEntry* entry = m_entries + m_entryCount; - entry->size = size; - if (m_index + size > b2_stackSize) - { - entry->data = (char*)b2Alloc(size); - entry->usedMalloc = true; - } - else - { - entry->data = m_data + m_index; - entry->usedMalloc = false; - m_index += size; - } + b2StackEntry * entry = m_entries + m_entryCount; + entry->size = size; + if (m_index + size > b2_stackSize) { + entry->data = (char *)b2Alloc(size); + entry->usedMalloc = true; + } else { + entry->data = m_data + m_index; + entry->usedMalloc = false; + m_index += size; + } - m_allocation += size; - m_maxAllocation = b2Max(m_maxAllocation, m_allocation); - ++m_entryCount; + m_allocation += size; + m_maxAllocation = b2Max(m_maxAllocation, m_allocation); + ++m_entryCount; - return entry->data; + return entry->data; } -void b2StackAllocator::Free(void* p) +void b2StackAllocator::Free(void * p) { - b2Assert(m_entryCount > 0); - b2StackEntry* entry = m_entries + m_entryCount - 1; - b2Assert(p == entry->data); - if (entry->usedMalloc) - { - b2Free(p); - } - else - { - m_index -= entry->size; - } - m_allocation -= entry->size; - --m_entryCount; + b2Assert(m_entryCount > 0); + b2StackEntry * entry = m_entries + m_entryCount - 1; + b2Assert(p == entry->data); + if (entry->usedMalloc) { + b2Free(p); + } else { + m_index -= entry->size; + } + m_allocation -= entry->size; + --m_entryCount; - p = nullptr; + p = nullptr; } -int32 b2StackAllocator::GetMaxAllocation() const -{ - return m_maxAllocation; -} +int32 b2StackAllocator::GetMaxAllocation() const { return m_maxAllocation; } diff --git a/flatland_box2d/src/Common/b2Timer.cpp b/flatland_box2d/src/Common/b2Timer.cpp index e67719e9..e7b92362 100644 --- a/flatland_box2d/src/Common/b2Timer.cpp +++ b/flatland_box2d/src/Common/b2Timer.cpp @@ -1,20 +1,20 @@ /* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2011 Erin Catto http://box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Common/b2Timer.h" @@ -30,75 +30,63 @@ float64 b2Timer::s_invFrequency = 0.0f; b2Timer::b2Timer() { - LARGE_INTEGER largeInteger; - - if (s_invFrequency == 0.0f) - { - QueryPerformanceFrequency(&largeInteger); - s_invFrequency = float64(largeInteger.QuadPart); - if (s_invFrequency > 0.0f) - { - s_invFrequency = 1000.0f / s_invFrequency; - } - } - - QueryPerformanceCounter(&largeInteger); - m_start = float64(largeInteger.QuadPart); + LARGE_INTEGER largeInteger; + + if (s_invFrequency == 0.0f) { + QueryPerformanceFrequency(&largeInteger); + s_invFrequency = float64(largeInteger.QuadPart); + if (s_invFrequency > 0.0f) { + s_invFrequency = 1000.0f / s_invFrequency; + } + } + + QueryPerformanceCounter(&largeInteger); + m_start = float64(largeInteger.QuadPart); } void b2Timer::Reset() { - LARGE_INTEGER largeInteger; - QueryPerformanceCounter(&largeInteger); - m_start = float64(largeInteger.QuadPart); + LARGE_INTEGER largeInteger; + QueryPerformanceCounter(&largeInteger); + m_start = float64(largeInteger.QuadPart); } float32 b2Timer::GetMilliseconds() const { - LARGE_INTEGER largeInteger; - QueryPerformanceCounter(&largeInteger); - float64 count = float64(largeInteger.QuadPart); - float32 ms = float32(s_invFrequency * (count - m_start)); - return ms; + LARGE_INTEGER largeInteger; + QueryPerformanceCounter(&largeInteger); + float64 count = float64(largeInteger.QuadPart); + float32 ms = float32(s_invFrequency * (count - m_start)); + return ms; } -#elif defined(__linux__) || defined (__APPLE__) +#elif defined(__linux__) || defined(__APPLE__) #include -b2Timer::b2Timer() -{ - Reset(); -} +b2Timer::b2Timer() { Reset(); } void b2Timer::Reset() { - timeval t; - gettimeofday(&t, 0); - m_start_sec = t.tv_sec; - m_start_usec = t.tv_usec; + timeval t; + gettimeofday(&t, 0); + m_start_sec = t.tv_sec; + m_start_usec = t.tv_usec; } float32 b2Timer::GetMilliseconds() const { - timeval t; - gettimeofday(&t, 0); - return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec); + timeval t; + gettimeofday(&t, 0); + return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec); } #else -b2Timer::b2Timer() -{ -} +b2Timer::b2Timer() {} -void b2Timer::Reset() -{ -} +void b2Timer::Reset() {} -float32 b2Timer::GetMilliseconds() const -{ - return 0.0f; -} +float32 b2Timer::GetMilliseconds() const { return 0.0f; } #endif diff --git a/flatland_box2d/src/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2ChainAndCircleContact.cpp index 4807fff7..4a9eac0c 100644 --- a/flatland_box2d/src/Dynamics/Contacts/b2ChainAndCircleContact.cpp +++ b/flatland_box2d/src/Dynamics/Contacts/b2ChainAndCircleContact.cpp @@ -1,53 +1,57 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h" -#include "Box2D/Common/b2BlockAllocator.h" -#include "Box2D/Dynamics/b2Fixture.h" -#include "Box2D/Collision/Shapes/b2ChainShape.h" -#include "Box2D/Collision/Shapes/b2EdgeShape.h" #include -b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) +#include "Box2D/Collision/Shapes/b2ChainShape.h" +#include "Box2D/Collision/Shapes/b2EdgeShape.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" + +b2Contact * b2ChainAndCircleContact::Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator) { - void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact)); - return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB); + void * mem = allocator->Allocate(sizeof(b2ChainAndCircleContact)); + return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB); } -void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +void b2ChainAndCircleContact::Destroy(b2Contact * contact, b2BlockAllocator * allocator) { - ((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact(); - allocator->Free(contact, sizeof(b2ChainAndCircleContact)); + ((b2ChainAndCircleContact *)contact)->~b2ChainAndCircleContact(); + allocator->Free(contact, sizeof(b2ChainAndCircleContact)); } -b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) +b2ChainAndCircleContact::b2ChainAndCircleContact( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB) : b2Contact(fixtureA, indexA, fixtureB, indexB) { - b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); + b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); } -void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +void b2ChainAndCircleContact::Evaluate( + b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) { - b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); - b2EdgeShape edge; - chain->GetChildEdge(&edge, m_indexA); - b2CollideEdgeAndCircle( manifold, &edge, xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); + b2ChainShape * chain = (b2ChainShape *)m_fixtureA->GetShape(); + b2EdgeShape edge; + chain->GetChildEdge(&edge, m_indexA); + b2CollideEdgeAndCircle(manifold, &edge, xfA, (b2CircleShape *)m_fixtureB->GetShape(), xfB); } diff --git a/flatland_box2d/src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp index 8dfd759e..3a3bb034 100644 --- a/flatland_box2d/src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp +++ b/flatland_box2d/src/Dynamics/Contacts/b2ChainAndPolygonContact.cpp @@ -1,53 +1,57 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h" -#include "Box2D/Common/b2BlockAllocator.h" -#include "Box2D/Dynamics/b2Fixture.h" -#include "Box2D/Collision/Shapes/b2ChainShape.h" -#include "Box2D/Collision/Shapes/b2EdgeShape.h" #include -b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) +#include "Box2D/Collision/Shapes/b2ChainShape.h" +#include "Box2D/Collision/Shapes/b2EdgeShape.h" +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" + +b2Contact * b2ChainAndPolygonContact::Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator) { - void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact)); - return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB); + void * mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact)); + return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB); } -void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +void b2ChainAndPolygonContact::Destroy(b2Contact * contact, b2BlockAllocator * allocator) { - ((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact(); - allocator->Free(contact, sizeof(b2ChainAndPolygonContact)); + ((b2ChainAndPolygonContact *)contact)->~b2ChainAndPolygonContact(); + allocator->Free(contact, sizeof(b2ChainAndPolygonContact)); } -b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB) +b2ChainAndPolygonContact::b2ChainAndPolygonContact( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB) : b2Contact(fixtureA, indexA, fixtureB, indexB) { - b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); - b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); + b2Assert(m_fixtureA->GetType() == b2Shape::e_chain); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); } -void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +void b2ChainAndPolygonContact::Evaluate( + b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) { - b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape(); - b2EdgeShape edge; - chain->GetChildEdge(&edge, m_indexA); - b2CollideEdgeAndPolygon( manifold, &edge, xfA, - (b2PolygonShape*)m_fixtureB->GetShape(), xfB); + b2ChainShape * chain = (b2ChainShape *)m_fixtureA->GetShape(); + b2EdgeShape edge; + chain->GetChildEdge(&edge, m_indexA); + b2CollideEdgeAndPolygon(manifold, &edge, xfA, (b2PolygonShape *)m_fixtureB->GetShape(), xfB); } diff --git a/flatland_box2d/src/Dynamics/Contacts/b2CircleContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2CircleContact.cpp index 45b45495..24ea7af7 100644 --- a/flatland_box2d/src/Dynamics/Contacts/b2CircleContact.cpp +++ b/flatland_box2d/src/Dynamics/Contacts/b2CircleContact.cpp @@ -1,52 +1,55 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Contacts/b2CircleContact.h" + +#include + +#include "Box2D/Collision/b2TimeOfImpact.h" +#include "Box2D/Common/b2BlockAllocator.h" #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2Fixture.h" #include "Box2D/Dynamics/b2WorldCallbacks.h" -#include "Box2D/Common/b2BlockAllocator.h" -#include "Box2D/Collision/b2TimeOfImpact.h" - -#include -b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +b2Contact * b2CircleContact::Create( + b2Fixture * fixtureA, int32, b2Fixture * fixtureB, int32, b2BlockAllocator * allocator) { - void* mem = allocator->Allocate(sizeof(b2CircleContact)); - return new (mem) b2CircleContact(fixtureA, fixtureB); + void * mem = allocator->Allocate(sizeof(b2CircleContact)); + return new (mem) b2CircleContact(fixtureA, fixtureB); } -void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +void b2CircleContact::Destroy(b2Contact * contact, b2BlockAllocator * allocator) { - ((b2CircleContact*)contact)->~b2CircleContact(); - allocator->Free(contact, sizeof(b2CircleContact)); + ((b2CircleContact *)contact)->~b2CircleContact(); + allocator->Free(contact, sizeof(b2CircleContact)); } -b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, 0, fixtureB, 0) +b2CircleContact::b2CircleContact(b2Fixture * fixtureA, b2Fixture * fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) { - b2Assert(m_fixtureA->GetType() == b2Shape::e_circle); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); + b2Assert(m_fixtureA->GetType() == b2Shape::e_circle); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); } -void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +void b2CircleContact::Evaluate( + b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) { - b2CollideCircles(manifold, - (b2CircleShape*)m_fixtureA->GetShape(), xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); + b2CollideCircles( + manifold, (b2CircleShape *)m_fixtureA->GetShape(), xfA, (b2CircleShape *)m_fixtureB->GetShape(), + xfB); } diff --git a/flatland_box2d/src/Dynamics/Contacts/b2Contact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2Contact.cpp index b4aa9608..e0dda499 100644 --- a/flatland_box2d/src/Dynamics/Contacts/b2Contact.cpp +++ b/flatland_box2d/src/Dynamics/Contacts/b2Contact.cpp @@ -1,35 +1,35 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Contacts/b2Contact.h" -#include "Box2D/Dynamics/Contacts/b2CircleContact.h" -#include "Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h" -#include "Box2D/Dynamics/Contacts/b2PolygonContact.h" -#include "Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h" -#include "Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h" -#include "Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h" -#include "Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h" -#include "Box2D/Dynamics/Contacts/b2ContactSolver.h" +#include "Box2D/Collision/Shapes/b2Shape.h" #include "Box2D/Collision/b2Collision.h" #include "Box2D/Collision/b2TimeOfImpact.h" -#include "Box2D/Collision/Shapes/b2Shape.h" #include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h" +#include "Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h" +#include "Box2D/Dynamics/Contacts/b2CircleContact.h" +#include "Box2D/Dynamics/Contacts/b2ContactSolver.h" +#include "Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h" +#include "Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h" +#include "Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h" +#include "Box2D/Dynamics/Contacts/b2PolygonContact.h" #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2Fixture.h" #include "Box2D/Dynamics/b2World.h" @@ -39,209 +39,201 @@ bool b2Contact::s_initialized = false; void b2Contact::InitializeRegisters() { - AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle); - AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle); - AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon); - AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle); - AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon); - AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle); - AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon); + AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle); + AddType( + b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, + b2Shape::e_circle); + AddType( + b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon); + AddType( + b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, + b2Shape::e_circle); + AddType( + b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, + b2Shape::e_polygon); + AddType( + b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, + b2Shape::e_circle); + AddType( + b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, + b2Shape::e_polygon); } -void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn, - b2Shape::Type type1, b2Shape::Type type2) +void b2Contact::AddType( + b2ContactCreateFcn * createFcn, b2ContactDestroyFcn * destoryFcn, b2Shape::Type type1, + b2Shape::Type type2) { - b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); - b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); - - s_registers[type1][type2].createFcn = createFcn; - s_registers[type1][type2].destroyFcn = destoryFcn; - s_registers[type1][type2].primary = true; - - if (type1 != type2) - { - s_registers[type2][type1].createFcn = createFcn; - s_registers[type2][type1].destroyFcn = destoryFcn; - s_registers[type2][type1].primary = false; - } + b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); + b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); + + s_registers[type1][type2].createFcn = createFcn; + s_registers[type1][type2].destroyFcn = destoryFcn; + s_registers[type1][type2].primary = true; + + if (type1 != type2) { + s_registers[type2][type1].createFcn = createFcn; + s_registers[type2][type1].destroyFcn = destoryFcn; + s_registers[type2][type1].primary = false; + } } -b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator) +b2Contact * b2Contact::Create( + b2Fixture * fixtureA, int32 indexA, b2Fixture * fixtureB, int32 indexB, + b2BlockAllocator * allocator) { - if (s_initialized == false) - { - InitializeRegisters(); - s_initialized = true; - } - - b2Shape::Type type1 = fixtureA->GetType(); - b2Shape::Type type2 = fixtureB->GetType(); - - b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); - b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); - - b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn; - if (createFcn) - { - if (s_registers[type1][type2].primary) - { - return createFcn(fixtureA, indexA, fixtureB, indexB, allocator); - } - else - { - return createFcn(fixtureB, indexB, fixtureA, indexA, allocator); - } - } - else - { - return nullptr; - } + if (s_initialized == false) { + InitializeRegisters(); + s_initialized = true; + } + + b2Shape::Type type1 = fixtureA->GetType(); + b2Shape::Type type2 = fixtureB->GetType(); + + b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount); + b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount); + + b2ContactCreateFcn * createFcn = s_registers[type1][type2].createFcn; + if (createFcn) { + if (s_registers[type1][type2].primary) { + return createFcn(fixtureA, indexA, fixtureB, indexB, allocator); + } else { + return createFcn(fixtureB, indexB, fixtureA, indexA, allocator); + } + } else { + return nullptr; + } } -void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +void b2Contact::Destroy(b2Contact * contact, b2BlockAllocator * allocator) { - b2Assert(s_initialized == true); + b2Assert(s_initialized == true); - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; + b2Fixture * fixtureA = contact->m_fixtureA; + b2Fixture * fixtureB = contact->m_fixtureB; - if (contact->m_manifold.pointCount > 0 && - fixtureA->IsSensor() == false && - fixtureB->IsSensor() == false) - { - fixtureA->GetBody()->SetAwake(true); - fixtureB->GetBody()->SetAwake(true); - } + if ( + contact->m_manifold.pointCount > 0 && fixtureA->IsSensor() == false && + fixtureB->IsSensor() == false) { + fixtureA->GetBody()->SetAwake(true); + fixtureB->GetBody()->SetAwake(true); + } - b2Shape::Type typeA = fixtureA->GetType(); - b2Shape::Type typeB = fixtureB->GetType(); + b2Shape::Type typeA = fixtureA->GetType(); + b2Shape::Type typeB = fixtureB->GetType(); - b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); - b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); + b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); + b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount); - b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn; - destroyFcn(contact, allocator); + b2ContactDestroyFcn * destroyFcn = s_registers[typeA][typeB].destroyFcn; + destroyFcn(contact, allocator); } -b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB) +b2Contact::b2Contact(b2Fixture * fA, int32 indexA, b2Fixture * fB, int32 indexB) { - m_flags = e_enabledFlag; + m_flags = e_enabledFlag; - m_fixtureA = fA; - m_fixtureB = fB; + m_fixtureA = fA; + m_fixtureB = fB; - m_indexA = indexA; - m_indexB = indexB; + m_indexA = indexA; + m_indexB = indexB; - m_manifold.pointCount = 0; + m_manifold.pointCount = 0; - m_prev = nullptr; - m_next = nullptr; + m_prev = nullptr; + m_next = nullptr; - m_nodeA.contact = nullptr; - m_nodeA.prev = nullptr; - m_nodeA.next = nullptr; - m_nodeA.other = nullptr; + m_nodeA.contact = nullptr; + m_nodeA.prev = nullptr; + m_nodeA.next = nullptr; + m_nodeA.other = nullptr; - m_nodeB.contact = nullptr; - m_nodeB.prev = nullptr; - m_nodeB.next = nullptr; - m_nodeB.other = nullptr; + m_nodeB.contact = nullptr; + m_nodeB.prev = nullptr; + m_nodeB.next = nullptr; + m_nodeB.other = nullptr; - m_toiCount = 0; + m_toiCount = 0; - m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); - m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); + m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction); + m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution); - m_tangentSpeed = 0.0f; + m_tangentSpeed = 0.0f; } // Update the contact manifold and touching status. // Note: do not assume the fixture AABBs are overlapping or are valid. -void b2Contact::Update(b2ContactListener* listener) +void b2Contact::Update(b2ContactListener * listener) { - b2Manifold oldManifold = m_manifold; - - // Re-enable this contact. - m_flags |= e_enabledFlag; - - bool touching = false; - bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag; - - bool sensorA = m_fixtureA->IsSensor(); - bool sensorB = m_fixtureB->IsSensor(); - bool sensor = sensorA || sensorB; - - b2Body* bodyA = m_fixtureA->GetBody(); - b2Body* bodyB = m_fixtureB->GetBody(); - const b2Transform& xfA = bodyA->GetTransform(); - const b2Transform& xfB = bodyB->GetTransform(); - - // Is this contact a sensor? - if (sensor) - { - const b2Shape* shapeA = m_fixtureA->GetShape(); - const b2Shape* shapeB = m_fixtureB->GetShape(); - touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB); - - // Sensors don't generate manifolds. - m_manifold.pointCount = 0; - } - else - { - Evaluate(&m_manifold, xfA, xfB); - touching = m_manifold.pointCount > 0; - - // Match old contact ids to new contact ids and copy the - // stored impulses to warm start the solver. - for (int32 i = 0; i < m_manifold.pointCount; ++i) - { - b2ManifoldPoint* mp2 = m_manifold.points + i; - mp2->normalImpulse = 0.0f; - mp2->tangentImpulse = 0.0f; - b2ContactID id2 = mp2->id; - - for (int32 j = 0; j < oldManifold.pointCount; ++j) - { - b2ManifoldPoint* mp1 = oldManifold.points + j; - - if (mp1->id.key == id2.key) - { - mp2->normalImpulse = mp1->normalImpulse; - mp2->tangentImpulse = mp1->tangentImpulse; - break; - } - } - } - - if (touching != wasTouching) - { - bodyA->SetAwake(true); - bodyB->SetAwake(true); - } - } - - if (touching) - { - m_flags |= e_touchingFlag; - } - else - { - m_flags &= ~e_touchingFlag; - } - - if (wasTouching == false && touching == true && listener) - { - listener->BeginContact(this); - } - - if (wasTouching == true && touching == false && listener) - { - listener->EndContact(this); - } - - if (sensor == false && touching && listener) - { - listener->PreSolve(this, &oldManifold); - } + b2Manifold oldManifold = m_manifold; + + // Re-enable this contact. + m_flags |= e_enabledFlag; + + bool touching = false; + bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag; + + bool sensorA = m_fixtureA->IsSensor(); + bool sensorB = m_fixtureB->IsSensor(); + bool sensor = sensorA || sensorB; + + b2Body * bodyA = m_fixtureA->GetBody(); + b2Body * bodyB = m_fixtureB->GetBody(); + const b2Transform & xfA = bodyA->GetTransform(); + const b2Transform & xfB = bodyB->GetTransform(); + + // Is this contact a sensor? + if (sensor) { + const b2Shape * shapeA = m_fixtureA->GetShape(); + const b2Shape * shapeB = m_fixtureB->GetShape(); + touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB); + + // Sensors don't generate manifolds. + m_manifold.pointCount = 0; + } else { + Evaluate(&m_manifold, xfA, xfB); + touching = m_manifold.pointCount > 0; + + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + for (int32 i = 0; i < m_manifold.pointCount; ++i) { + b2ManifoldPoint * mp2 = m_manifold.points + i; + mp2->normalImpulse = 0.0f; + mp2->tangentImpulse = 0.0f; + b2ContactID id2 = mp2->id; + + for (int32 j = 0; j < oldManifold.pointCount; ++j) { + b2ManifoldPoint * mp1 = oldManifold.points + j; + + if (mp1->id.key == id2.key) { + mp2->normalImpulse = mp1->normalImpulse; + mp2->tangentImpulse = mp1->tangentImpulse; + break; + } + } + } + + if (touching != wasTouching) { + bodyA->SetAwake(true); + bodyB->SetAwake(true); + } + } + + if (touching) { + m_flags |= e_touchingFlag; + } else { + m_flags &= ~e_touchingFlag; + } + + if (wasTouching == false && touching == true && listener) { + listener->BeginContact(this); + } + + if (wasTouching == true && touching == false && listener) { + listener->EndContact(this); + } + + if (sensor == false && touching && listener) { + listener->PreSolve(this, &oldManifold); + } } diff --git a/flatland_box2d/src/Dynamics/Contacts/b2ContactSolver.cpp b/flatland_box2d/src/Dynamics/Contacts/b2ContactSolver.cpp index 79e72e49..5eba003d 100644 --- a/flatland_box2d/src/Dynamics/Contacts/b2ContactSolver.cpp +++ b/flatland_box2d/src/Dynamics/Contacts/b2ContactSolver.cpp @@ -1,838 +1,808 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Contacts/b2ContactSolver.h" +#include "Box2D/Common/b2StackAllocator.h" #include "Box2D/Dynamics/Contacts/b2Contact.h" #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2Fixture.h" #include "Box2D/Dynamics/b2World.h" -#include "Box2D/Common/b2StackAllocator.h" -// Solver debugging is normally disabled because the block solver sometimes has to deal with a poorly conditioned effective mass matrix. +// Solver debugging is normally disabled because the block solver sometimes has +// to deal with a poorly conditioned effective mass matrix. #define B2_DEBUG_SOLVER 0 bool g_blockSolve = true; struct b2ContactPositionConstraint { - b2Vec2 localPoints[b2_maxManifoldPoints]; - b2Vec2 localNormal; - b2Vec2 localPoint; - int32 indexA; - int32 indexB; - float32 invMassA, invMassB; - b2Vec2 localCenterA, localCenterB; - float32 invIA, invIB; - b2Manifold::Type type; - float32 radiusA, radiusB; - int32 pointCount; + b2Vec2 localPoints[b2_maxManifoldPoints]; + b2Vec2 localNormal; + b2Vec2 localPoint; + int32 indexA; + int32 indexB; + float32 invMassA, invMassB; + b2Vec2 localCenterA, localCenterB; + float32 invIA, invIB; + b2Manifold::Type type; + float32 radiusA, radiusB; + int32 pointCount; }; -b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def) +b2ContactSolver::b2ContactSolver(b2ContactSolverDef * def) { - m_step = def->step; - m_allocator = def->allocator; - m_count = def->count; - m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint)); - m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint)); - m_positions = def->positions; - m_velocities = def->velocities; - m_contacts = def->contacts; - - // Initialize position independent portions of the constraints. - for (int32 i = 0; i < m_count; ++i) - { - b2Contact* contact = m_contacts[i]; - - b2Fixture* fixtureA = contact->m_fixtureA; - b2Fixture* fixtureB = contact->m_fixtureB; - b2Shape* shapeA = fixtureA->GetShape(); - b2Shape* shapeB = fixtureB->GetShape(); - float32 radiusA = shapeA->m_radius; - float32 radiusB = shapeB->m_radius; - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - b2Manifold* manifold = contact->GetManifold(); - - int32 pointCount = manifold->pointCount; - b2Assert(pointCount > 0); - - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - vc->friction = contact->m_friction; - vc->restitution = contact->m_restitution; - vc->tangentSpeed = contact->m_tangentSpeed; - vc->indexA = bodyA->m_islandIndex; - vc->indexB = bodyB->m_islandIndex; - vc->invMassA = bodyA->m_invMass; - vc->invMassB = bodyB->m_invMass; - vc->invIA = bodyA->m_invI; - vc->invIB = bodyB->m_invI; - vc->contactIndex = i; - vc->pointCount = pointCount; - vc->K.SetZero(); - vc->normalMass.SetZero(); - - b2ContactPositionConstraint* pc = m_positionConstraints + i; - pc->indexA = bodyA->m_islandIndex; - pc->indexB = bodyB->m_islandIndex; - pc->invMassA = bodyA->m_invMass; - pc->invMassB = bodyB->m_invMass; - pc->localCenterA = bodyA->m_sweep.localCenter; - pc->localCenterB = bodyB->m_sweep.localCenter; - pc->invIA = bodyA->m_invI; - pc->invIB = bodyB->m_invI; - pc->localNormal = manifold->localNormal; - pc->localPoint = manifold->localPoint; - pc->pointCount = pointCount; - pc->radiusA = radiusA; - pc->radiusB = radiusB; - pc->type = manifold->type; - - for (int32 j = 0; j < pointCount; ++j) - { - b2ManifoldPoint* cp = manifold->points + j; - b2VelocityConstraintPoint* vcp = vc->points + j; - - if (m_step.warmStarting) - { - vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse; - vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse; - } - else - { - vcp->normalImpulse = 0.0f; - vcp->tangentImpulse = 0.0f; - } - - vcp->rA.SetZero(); - vcp->rB.SetZero(); - vcp->normalMass = 0.0f; - vcp->tangentMass = 0.0f; - vcp->velocityBias = 0.0f; - - pc->localPoints[j] = cp->localPoint; - } - } + m_step = def->step; + m_allocator = def->allocator; + m_count = def->count; + m_positionConstraints = (b2ContactPositionConstraint *)m_allocator->Allocate( + m_count * sizeof(b2ContactPositionConstraint)); + m_velocityConstraints = (b2ContactVelocityConstraint *)m_allocator->Allocate( + m_count * sizeof(b2ContactVelocityConstraint)); + m_positions = def->positions; + m_velocities = def->velocities; + m_contacts = def->contacts; + + // Initialize position independent portions of the constraints. + for (int32 i = 0; i < m_count; ++i) { + b2Contact * contact = m_contacts[i]; + + b2Fixture * fixtureA = contact->m_fixtureA; + b2Fixture * fixtureB = contact->m_fixtureB; + b2Shape * shapeA = fixtureA->GetShape(); + b2Shape * shapeB = fixtureB->GetShape(); + float32 radiusA = shapeA->m_radius; + float32 radiusB = shapeB->m_radius; + b2Body * bodyA = fixtureA->GetBody(); + b2Body * bodyB = fixtureB->GetBody(); + b2Manifold * manifold = contact->GetManifold(); + + int32 pointCount = manifold->pointCount; + b2Assert(pointCount > 0); + + b2ContactVelocityConstraint * vc = m_velocityConstraints + i; + vc->friction = contact->m_friction; + vc->restitution = contact->m_restitution; + vc->tangentSpeed = contact->m_tangentSpeed; + vc->indexA = bodyA->m_islandIndex; + vc->indexB = bodyB->m_islandIndex; + vc->invMassA = bodyA->m_invMass; + vc->invMassB = bodyB->m_invMass; + vc->invIA = bodyA->m_invI; + vc->invIB = bodyB->m_invI; + vc->contactIndex = i; + vc->pointCount = pointCount; + vc->K.SetZero(); + vc->normalMass.SetZero(); + + b2ContactPositionConstraint * pc = m_positionConstraints + i; + pc->indexA = bodyA->m_islandIndex; + pc->indexB = bodyB->m_islandIndex; + pc->invMassA = bodyA->m_invMass; + pc->invMassB = bodyB->m_invMass; + pc->localCenterA = bodyA->m_sweep.localCenter; + pc->localCenterB = bodyB->m_sweep.localCenter; + pc->invIA = bodyA->m_invI; + pc->invIB = bodyB->m_invI; + pc->localNormal = manifold->localNormal; + pc->localPoint = manifold->localPoint; + pc->pointCount = pointCount; + pc->radiusA = radiusA; + pc->radiusB = radiusB; + pc->type = manifold->type; + + for (int32 j = 0; j < pointCount; ++j) { + b2ManifoldPoint * cp = manifold->points + j; + b2VelocityConstraintPoint * vcp = vc->points + j; + + if (m_step.warmStarting) { + vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse; + vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse; + } else { + vcp->normalImpulse = 0.0f; + vcp->tangentImpulse = 0.0f; + } + + vcp->rA.SetZero(); + vcp->rB.SetZero(); + vcp->normalMass = 0.0f; + vcp->tangentMass = 0.0f; + vcp->velocityBias = 0.0f; + + pc->localPoints[j] = cp->localPoint; + } + } } b2ContactSolver::~b2ContactSolver() { - m_allocator->Free(m_velocityConstraints); - m_allocator->Free(m_positionConstraints); + m_allocator->Free(m_velocityConstraints); + m_allocator->Free(m_positionConstraints); } // Initialize position dependent portions of the velocity constraints. void b2ContactSolver::InitializeVelocityConstraints() { - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - float32 radiusA = pc->radiusA; - float32 radiusB = pc->radiusB; - b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); - - int32 indexA = vc->indexA; - int32 indexB = vc->indexB; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - float32 iA = vc->invIA; - float32 iB = vc->invIB; - b2Vec2 localCenterA = pc->localCenterA; - b2Vec2 localCenterB = pc->localCenterB; - - b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; - b2Vec2 vA = m_velocities[indexA].v; - float32 wA = m_velocities[indexA].w; - - b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; - b2Vec2 vB = m_velocities[indexB].v; - float32 wB = m_velocities[indexB].w; - - b2Assert(manifold->pointCount > 0); - - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2WorldManifold worldManifold; - worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB); - - vc->normal = worldManifold.normal; - - int32 pointCount = vc->pointCount; - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - - vcp->rA = worldManifold.points[j] - cA; - vcp->rB = worldManifold.points[j] - cB; - - float32 rnA = b2Cross(vcp->rA, vc->normal); - float32 rnB = b2Cross(vcp->rB, vc->normal); - - float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; - - b2Vec2 tangent = b2Cross(vc->normal, 1.0f); - - float32 rtA = b2Cross(vcp->rA, tangent); - float32 rtB = b2Cross(vcp->rB, tangent); - - float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; - - vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; - - // Setup a velocity bias for restitution. - vcp->velocityBias = 0.0f; - float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA)); - if (vRel < -b2_velocityThreshold) - { - vcp->velocityBias = -vc->restitution * vRel; - } - } - - // If we have two points, then prepare the block solver. - if (vc->pointCount == 2 && g_blockSolve) - { - b2VelocityConstraintPoint* vcp1 = vc->points + 0; - b2VelocityConstraintPoint* vcp2 = vc->points + 1; - - float32 rn1A = b2Cross(vcp1->rA, vc->normal); - float32 rn1B = b2Cross(vcp1->rB, vc->normal); - float32 rn2A = b2Cross(vcp2->rA, vc->normal); - float32 rn2B = b2Cross(vcp2->rB, vc->normal); - - float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; - float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; - float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; - - // Ensure a reasonable condition number. - const float32 k_maxConditionNumber = 1000.0f; - if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) - { - // K is safe to invert. - vc->K.ex.Set(k11, k12); - vc->K.ey.Set(k12, k22); - vc->normalMass = vc->K.GetInverse(); - } - else - { - // The constraints are redundant, just use one. - // TODO_ERIN use deepest? - vc->pointCount = 1; - } - } - } + for (int32 i = 0; i < m_count; ++i) { + b2ContactVelocityConstraint * vc = m_velocityConstraints + i; + b2ContactPositionConstraint * pc = m_positionConstraints + i; + + float32 radiusA = pc->radiusA; + float32 radiusB = pc->radiusB; + b2Manifold * manifold = m_contacts[vc->contactIndex]->GetManifold(); + + int32 indexA = vc->indexA; + int32 indexB = vc->indexB; + + float32 mA = vc->invMassA; + float32 mB = vc->invMassB; + float32 iA = vc->invIA; + float32 iB = vc->invIB; + b2Vec2 localCenterA = pc->localCenterA; + b2Vec2 localCenterB = pc->localCenterB; + + b2Vec2 cA = m_positions[indexA].c; + float32 aA = m_positions[indexA].a; + b2Vec2 vA = m_velocities[indexA].v; + float32 wA = m_velocities[indexA].w; + + b2Vec2 cB = m_positions[indexB].c; + float32 aB = m_positions[indexB].a; + b2Vec2 vB = m_velocities[indexB].v; + float32 wB = m_velocities[indexB].w; + + b2Assert(manifold->pointCount > 0); + + b2Transform xfA, xfB; + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - b2Mul(xfA.q, localCenterA); + xfB.p = cB - b2Mul(xfB.q, localCenterB); + + b2WorldManifold worldManifold; + worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB); + + vc->normal = worldManifold.normal; + + int32 pointCount = vc->pointCount; + for (int32 j = 0; j < pointCount; ++j) { + b2VelocityConstraintPoint * vcp = vc->points + j; + + vcp->rA = worldManifold.points[j] - cA; + vcp->rB = worldManifold.points[j] - cB; + + float32 rnA = b2Cross(vcp->rA, vc->normal); + float32 rnB = b2Cross(vcp->rB, vc->normal); + + float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + b2Vec2 tangent = b2Cross(vc->normal, 1.0f); + + float32 rtA = b2Cross(vcp->rA, tangent); + float32 rtB = b2Cross(vcp->rB, tangent); + + float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + + vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // Setup a velocity bias for restitution. + vcp->velocityBias = 0.0f; + float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA)); + if (vRel < -b2_velocityThreshold) { + vcp->velocityBias = -vc->restitution * vRel; + } + } + + // If we have two points, then prepare the block solver. + if (vc->pointCount == 2 && g_blockSolve) { + b2VelocityConstraintPoint * vcp1 = vc->points + 0; + b2VelocityConstraintPoint * vcp2 = vc->points + 1; + + float32 rn1A = b2Cross(vcp1->rA, vc->normal); + float32 rn1B = b2Cross(vcp1->rB, vc->normal); + float32 rn2A = b2Cross(vcp2->rA, vc->normal); + float32 rn2B = b2Cross(vcp2->rB, vc->normal); + + float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; + float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; + float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; + + // Ensure a reasonable condition number. + const float32 k_maxConditionNumber = 1000.0f; + if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { + // K is safe to invert. + vc->K.ex.Set(k11, k12); + vc->K.ey.Set(k12, k22); + vc->normalMass = vc->K.GetInverse(); + } else { + // The constraints are redundant, just use one. + // TODO_ERIN use deepest? + vc->pointCount = 1; + } + } + } } void b2ContactSolver::WarmStart() { - // Warm start. - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - - int32 indexA = vc->indexA; - int32 indexB = vc->indexB; - float32 mA = vc->invMassA; - float32 iA = vc->invIA; - float32 mB = vc->invMassB; - float32 iB = vc->invIB; - int32 pointCount = vc->pointCount; - - b2Vec2 vA = m_velocities[indexA].v; - float32 wA = m_velocities[indexA].w; - b2Vec2 vB = m_velocities[indexB].v; - float32 wB = m_velocities[indexB].w; - - b2Vec2 normal = vc->normal; - b2Vec2 tangent = b2Cross(normal, 1.0f); - - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent; - wA -= iA * b2Cross(vcp->rA, P); - vA -= mA * P; - wB += iB * b2Cross(vcp->rB, P); - vB += mB * P; - } - - m_velocities[indexA].v = vA; - m_velocities[indexA].w = wA; - m_velocities[indexB].v = vB; - m_velocities[indexB].w = wB; - } + // Warm start. + for (int32 i = 0; i < m_count; ++i) { + b2ContactVelocityConstraint * vc = m_velocityConstraints + i; + + int32 indexA = vc->indexA; + int32 indexB = vc->indexB; + float32 mA = vc->invMassA; + float32 iA = vc->invIA; + float32 mB = vc->invMassB; + float32 iB = vc->invIB; + int32 pointCount = vc->pointCount; + + b2Vec2 vA = m_velocities[indexA].v; + float32 wA = m_velocities[indexA].w; + b2Vec2 vB = m_velocities[indexB].v; + float32 wB = m_velocities[indexB].w; + + b2Vec2 normal = vc->normal; + b2Vec2 tangent = b2Cross(normal, 1.0f); + + for (int32 j = 0; j < pointCount; ++j) { + b2VelocityConstraintPoint * vcp = vc->points + j; + b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent; + wA -= iA * b2Cross(vcp->rA, P); + vA -= mA * P; + wB += iB * b2Cross(vcp->rB, P); + vB += mB * P; + } + + m_velocities[indexA].v = vA; + m_velocities[indexA].w = wA; + m_velocities[indexB].v = vB; + m_velocities[indexB].w = wB; + } } void b2ContactSolver::SolveVelocityConstraints() { - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - - int32 indexA = vc->indexA; - int32 indexB = vc->indexB; - float32 mA = vc->invMassA; - float32 iA = vc->invIA; - float32 mB = vc->invMassB; - float32 iB = vc->invIB; - int32 pointCount = vc->pointCount; - - b2Vec2 vA = m_velocities[indexA].v; - float32 wA = m_velocities[indexA].w; - b2Vec2 vB = m_velocities[indexB].v; - float32 wB = m_velocities[indexB].w; - - b2Vec2 normal = vc->normal; - b2Vec2 tangent = b2Cross(normal, 1.0f); - float32 friction = vc->friction; - - b2Assert(pointCount == 1 || pointCount == 2); - - // Solve tangent constraints first because non-penetration is more important - // than friction. - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - - // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); - - // Compute tangent force - float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed; - float32 lambda = vcp->tangentMass * (-vt); - - // b2Clamp the accumulated force - float32 maxFriction = friction * vcp->normalImpulse; - float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction); - lambda = newImpulse - vcp->tangentImpulse; - vcp->tangentImpulse = newImpulse; - - // Apply contact impulse - b2Vec2 P = lambda * tangent; - - vA -= mA * P; - wA -= iA * b2Cross(vcp->rA, P); - - vB += mB * P; - wB += iB * b2Cross(vcp->rB, P); - } - - // Solve normal constraints - if (pointCount == 1 || g_blockSolve == false) - { - for (int32 j = 0; j < pointCount; ++j) - { - b2VelocityConstraintPoint* vcp = vc->points + j; - - // Relative velocity at contact - b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); - - // Compute normal impulse - float32 vn = b2Dot(dv, normal); - float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias); - - // b2Clamp the accumulated impulse - float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); - lambda = newImpulse - vcp->normalImpulse; - vcp->normalImpulse = newImpulse; - - // Apply contact impulse - b2Vec2 P = lambda * normal; - vA -= mA * P; - wA -= iA * b2Cross(vcp->rA, P); - - vB += mB * P; - wB += iB * b2Cross(vcp->rB, P); - } - } - else - { - // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite). - // Build the mini LCP for this contact patch - // - // vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 - // - // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) - // b = vn0 - velocityBias - // - // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i - // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases - // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid - // solution that satisfies the problem is chosen. - // - // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires - // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i). - // - // Substitute: - // - // x = a + d - // - // a := old total impulse - // x := new total impulse - // d := incremental impulse - // - // For the current iteration we extend the formula for the incremental impulse - // to compute the new total impulse: - // - // vn = A * d + b - // = A * (x - a) + b - // = A * x + b - A * a - // = A * x + b' - // b' = b - A * a; - - b2VelocityConstraintPoint* cp1 = vc->points + 0; - b2VelocityConstraintPoint* cp2 = vc->points + 1; - - b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse); - b2Assert(a.x >= 0.0f && a.y >= 0.0f); - - // Relative velocity at contact - b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - - // Compute normal velocity - float32 vn1 = b2Dot(dv1, normal); - float32 vn2 = b2Dot(dv2, normal); - - b2Vec2 b; - b.x = vn1 - cp1->velocityBias; - b.y = vn2 - cp2->velocityBias; - - // Compute b' - b -= b2Mul(vc->K, a); - - const float32 k_errorTol = 1e-3f; - B2_NOT_USED(k_errorTol); - - for (;;) - { - // - // Case 1: vn = 0 - // - // 0 = A * x + b' - // - // Solve for x: - // - // x = - inv(A) * b' - // - b2Vec2 x = - b2Mul(vc->normalMass, b); - - if (x.x >= 0.0f && x.y >= 0.0f) - { - // Get the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; + for (int32 i = 0; i < m_count; ++i) { + b2ContactVelocityConstraint * vc = m_velocityConstraints + i; + + int32 indexA = vc->indexA; + int32 indexB = vc->indexB; + float32 mA = vc->invMassA; + float32 iA = vc->invIA; + float32 mB = vc->invMassB; + float32 iB = vc->invIB; + int32 pointCount = vc->pointCount; + + b2Vec2 vA = m_velocities[indexA].v; + float32 wA = m_velocities[indexA].w; + b2Vec2 vB = m_velocities[indexB].v; + float32 wB = m_velocities[indexB].w; + + b2Vec2 normal = vc->normal; + b2Vec2 tangent = b2Cross(normal, 1.0f); + float32 friction = vc->friction; + + b2Assert(pointCount == 1 || pointCount == 2); + + // Solve tangent constraints first because non-penetration is more important + // than friction. + for (int32 j = 0; j < pointCount; ++j) { + b2VelocityConstraintPoint * vcp = vc->points + j; + + // Relative velocity at contact + b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); + + // Compute tangent force + float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed; + float32 lambda = vcp->tangentMass * (-vt); + + // b2Clamp the accumulated force + float32 maxFriction = friction * vcp->normalImpulse; + float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction); + lambda = newImpulse - vcp->tangentImpulse; + vcp->tangentImpulse = newImpulse; + + // Apply contact impulse + b2Vec2 P = lambda * tangent; + + vA -= mA * P; + wA -= iA * b2Cross(vcp->rA, P); + + vB += mB * P; + wB += iB * b2Cross(vcp->rB, P); + } + + // Solve normal constraints + if (pointCount == 1 || g_blockSolve == false) { + for (int32 j = 0; j < pointCount; ++j) { + b2VelocityConstraintPoint * vcp = vc->points + j; + + // Relative velocity at contact + b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA); + + // Compute normal impulse + float32 vn = b2Dot(dv, normal); + float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias); + + // b2Clamp the accumulated impulse + float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f); + lambda = newImpulse - vcp->normalImpulse; + vcp->normalImpulse = newImpulse; + + // Apply contact impulse + b2Vec2 P = lambda * normal; + vA -= mA * P; + wA -= iA * b2Cross(vcp->rA, P); + + vB += mB * P; + wB += iB * b2Cross(vcp->rB, P); + } + } else { + // Block solver developed in collaboration with Dirk Gregorius (back in + // 01/07 on Box2D_Lite). Build the mini LCP for this contact patch + // + // vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2 + // + // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n ) + // b = vn0 - velocityBias + // + // The system is solved using the "Total enumeration method" (s. Murty). + // The complementary constraint vn_i * x_i implies that we must have in + // any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem + // the cases vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, + // x2 = 0 and vn1 = 0 need to be tested. The first valid solution that + // satisfies the problem is chosen. + // + // In order to account of the accumulated impulse 'a' (because of the + // iterative nature of the solver which only requires that the accumulated + // impulse is clamped and not the incremental impulse) we change the + // impulse variable (x_i). + // + // Substitute: + // + // x = a + d + // + // a := old total impulse + // x := new total impulse + // d := incremental impulse + // + // For the current iteration we extend the formula for the incremental + // impulse to compute the new total impulse: + // + // vn = A * d + b + // = A * (x - a) + b + // = A * x + b - A * a + // = A * x + b' + // b' = b - A * a; + + b2VelocityConstraintPoint * cp1 = vc->points + 0; + b2VelocityConstraintPoint * cp2 = vc->points + 1; + + b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse); + b2Assert(a.x >= 0.0f && a.y >= 0.0f); + + // Relative velocity at contact + b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); + b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); + + // Compute normal velocity + float32 vn1 = b2Dot(dv1, normal); + float32 vn2 = b2Dot(dv2, normal); + + b2Vec2 b; + b.x = vn1 - cp1->velocityBias; + b.y = vn2 - cp2->velocityBias; + + // Compute b' + b -= b2Mul(vc->K, a); + + const float32 k_errorTol = 1e-3f; + B2_NOT_USED(k_errorTol); + + for (;;) { + // + // Case 1: vn = 0 + // + // 0 = A * x + b' + // + // Solve for x: + // + // x = - inv(A) * b' + // + b2Vec2 x = -b2Mul(vc->normalMass, b); + + if (x.x >= 0.0f && x.y >= 0.0f) { + // Get the incremental impulse + b2Vec2 d = x - a; + + // Apply incremental impulse + b2Vec2 P1 = d.x * normal; + b2Vec2 P2 = d.y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); + + // Accumulate + cp1->normalImpulse = x.x; + cp2->normalImpulse = x.y; #if B2_DEBUG_SOLVER == 1 - // Postconditions - dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); + // Postconditions + dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); + dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - // Compute normal velocity - vn1 = b2Dot(dv1, normal); - vn2 = b2Dot(dv2, normal); + // Compute normal velocity + vn1 = b2Dot(dv1, normal); + vn2 = b2Dot(dv2, normal); - b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); - b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); + b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); + b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); #endif - break; - } - - // - // Case 2: vn1 = 0 and x2 = 0 - // - // 0 = a11 * x1 + a12 * 0 + b1' - // vn2 = a21 * x1 + a22 * 0 + b2' - // - x.x = - cp1->normalMass * b.x; - x.y = 0.0f; - vn1 = 0.0f; - vn2 = vc->K.ex.y * x.x + b.y; - if (x.x >= 0.0f && vn2 >= 0.0f) - { - // Get the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; + break; + } + + // + // Case 2: vn1 = 0 and x2 = 0 + // + // 0 = a11 * x1 + a12 * 0 + b1' + // vn2 = a21 * x1 + a22 * 0 + b2' + // + x.x = -cp1->normalMass * b.x; + x.y = 0.0f; + vn1 = 0.0f; + vn2 = vc->K.ex.y * x.x + b.y; + if (x.x >= 0.0f && vn2 >= 0.0f) { + // Get the incremental impulse + b2Vec2 d = x - a; + + // Apply incremental impulse + b2Vec2 P1 = d.x * normal; + b2Vec2 P2 = d.y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); + + // Accumulate + cp1->normalImpulse = x.x; + cp2->normalImpulse = x.y; #if B2_DEBUG_SOLVER == 1 - // Postconditions - dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); + // Postconditions + dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA); - // Compute normal velocity - vn1 = b2Dot(dv1, normal); + // Compute normal velocity + vn1 = b2Dot(dv1, normal); - b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); + b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol); #endif - break; - } - - - // - // Case 3: vn2 = 0 and x1 = 0 - // - // vn1 = a11 * 0 + a12 * x2 + b1' - // 0 = a21 * 0 + a22 * x2 + b2' - // - x.x = 0.0f; - x.y = - cp2->normalMass * b.y; - vn1 = vc->K.ey.x * x.y + b.x; - vn2 = 0.0f; - - if (x.y >= 0.0f && vn1 >= 0.0f) - { - // Resubstitute for the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; + break; + } + + // + // Case 3: vn2 = 0 and x1 = 0 + // + // vn1 = a11 * 0 + a12 * x2 + b1' + // 0 = a21 * 0 + a22 * x2 + b2' + // + x.x = 0.0f; + x.y = -cp2->normalMass * b.y; + vn1 = vc->K.ey.x * x.y + b.x; + vn2 = 0.0f; + + if (x.y >= 0.0f && vn1 >= 0.0f) { + // Resubstitute for the incremental impulse + b2Vec2 d = x - a; + + // Apply incremental impulse + b2Vec2 P1 = d.x * normal; + b2Vec2 P2 = d.y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); + + // Accumulate + cp1->normalImpulse = x.x; + cp2->normalImpulse = x.y; #if B2_DEBUG_SOLVER == 1 - // Postconditions - dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); + // Postconditions + dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA); - // Compute normal velocity - vn2 = b2Dot(dv2, normal); + // Compute normal velocity + vn2 = b2Dot(dv2, normal); - b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); + b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol); #endif - break; - } - - // - // Case 4: x1 = 0 and x2 = 0 - // - // vn1 = b1 - // vn2 = b2; - x.x = 0.0f; - x.y = 0.0f; - vn1 = b.x; - vn2 = b.y; - - if (vn1 >= 0.0f && vn2 >= 0.0f ) - { - // Resubstitute for the incremental impulse - b2Vec2 d = x - a; - - // Apply incremental impulse - b2Vec2 P1 = d.x * normal; - b2Vec2 P2 = d.y * normal; - vA -= mA * (P1 + P2); - wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); - - vB += mB * (P1 + P2); - wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); - - // Accumulate - cp1->normalImpulse = x.x; - cp2->normalImpulse = x.y; - - break; - } - - // No solution, give up. This is hit sometimes, but it doesn't seem to matter. - break; - } - } - - m_velocities[indexA].v = vA; - m_velocities[indexA].w = wA; - m_velocities[indexB].v = vB; - m_velocities[indexB].w = wB; - } + break; + } + + // + // Case 4: x1 = 0 and x2 = 0 + // + // vn1 = b1 + // vn2 = b2; + x.x = 0.0f; + x.y = 0.0f; + vn1 = b.x; + vn2 = b.y; + + if (vn1 >= 0.0f && vn2 >= 0.0f) { + // Resubstitute for the incremental impulse + b2Vec2 d = x - a; + + // Apply incremental impulse + b2Vec2 P1 = d.x * normal; + b2Vec2 P2 = d.y * normal; + vA -= mA * (P1 + P2); + wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2)); + + vB += mB * (P1 + P2); + wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2)); + + // Accumulate + cp1->normalImpulse = x.x; + cp2->normalImpulse = x.y; + + break; + } + + // No solution, give up. This is hit sometimes, but it doesn't seem to + // matter. + break; + } + } + + m_velocities[indexA].v = vA; + m_velocities[indexA].w = wA; + m_velocities[indexB].v = vB; + m_velocities[indexB].w = wB; + } } void b2ContactSolver::StoreImpulses() { - for (int32 i = 0; i < m_count; ++i) - { - b2ContactVelocityConstraint* vc = m_velocityConstraints + i; - b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold(); - - for (int32 j = 0; j < vc->pointCount; ++j) - { - manifold->points[j].normalImpulse = vc->points[j].normalImpulse; - manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse; - } - } + for (int32 i = 0; i < m_count; ++i) { + b2ContactVelocityConstraint * vc = m_velocityConstraints + i; + b2Manifold * manifold = m_contacts[vc->contactIndex]->GetManifold(); + + for (int32 j = 0; j < vc->pointCount; ++j) { + manifold->points[j].normalImpulse = vc->points[j].normalImpulse; + manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse; + } + } } struct b2PositionSolverManifold { - void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index) - { - b2Assert(pc->pointCount > 0); - - switch (pc->type) - { - case b2Manifold::e_circles: - { - b2Vec2 pointA = b2Mul(xfA, pc->localPoint); - b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]); - normal = pointB - pointA; - normal.Normalize(); - point = 0.5f * (pointA + pointB); - separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB; - } - break; - - case b2Manifold::e_faceA: - { - normal = b2Mul(xfA.q, pc->localNormal); - b2Vec2 planePoint = b2Mul(xfA, pc->localPoint); - - b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; - point = clipPoint; - } - break; - - case b2Manifold::e_faceB: - { - normal = b2Mul(xfB.q, pc->localNormal); - b2Vec2 planePoint = b2Mul(xfB, pc->localPoint); - - b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]); - separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; - point = clipPoint; - - // Ensure normal points from A to B - normal = -normal; - } - break; - } - } - - b2Vec2 normal; - b2Vec2 point; - float32 separation; + void Initialize( + b2ContactPositionConstraint * pc, const b2Transform & xfA, const b2Transform & xfB, int32 index) + { + b2Assert(pc->pointCount > 0); + + switch (pc->type) { + case b2Manifold::e_circles: { + b2Vec2 pointA = b2Mul(xfA, pc->localPoint); + b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]); + normal = pointB - pointA; + normal.Normalize(); + point = 0.5f * (pointA + pointB); + separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB; + } break; + + case b2Manifold::e_faceA: { + normal = b2Mul(xfA.q, pc->localNormal); + b2Vec2 planePoint = b2Mul(xfA, pc->localPoint); + + b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]); + separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; + point = clipPoint; + } break; + + case b2Manifold::e_faceB: { + normal = b2Mul(xfB.q, pc->localNormal); + b2Vec2 planePoint = b2Mul(xfB, pc->localPoint); + + b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]); + separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB; + point = clipPoint; + + // Ensure normal points from A to B + normal = -normal; + } break; + } + } + + b2Vec2 normal; + b2Vec2 point; + float32 separation; }; // Sequential solver. bool b2ContactSolver::SolvePositionConstraints() { - float32 minSeparation = 0.0f; + float32 minSeparation = 0.0f; - for (int32 i = 0; i < m_count; ++i) - { - b2ContactPositionConstraint* pc = m_positionConstraints + i; + for (int32 i = 0; i < m_count; ++i) { + b2ContactPositionConstraint * pc = m_positionConstraints + i; - int32 indexA = pc->indexA; - int32 indexB = pc->indexB; - b2Vec2 localCenterA = pc->localCenterA; - float32 mA = pc->invMassA; - float32 iA = pc->invIA; - b2Vec2 localCenterB = pc->localCenterB; - float32 mB = pc->invMassB; - float32 iB = pc->invIB; - int32 pointCount = pc->pointCount; + int32 indexA = pc->indexA; + int32 indexB = pc->indexB; + b2Vec2 localCenterA = pc->localCenterA; + float32 mA = pc->invMassA; + float32 iA = pc->invIA; + b2Vec2 localCenterB = pc->localCenterB; + float32 mB = pc->invMassB; + float32 iB = pc->invIB; + int32 pointCount = pc->pointCount; - b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; + b2Vec2 cA = m_positions[indexA].c; + float32 aA = m_positions[indexA].a; - b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; + b2Vec2 cB = m_positions[indexB].c; + float32 aB = m_positions[indexB].a; - // Solve normal constraints - for (int32 j = 0; j < pointCount; ++j) - { - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); + // Solve normal constraints + for (int32 j = 0; j < pointCount; ++j) { + b2Transform xfA, xfB; + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - b2Mul(xfA.q, localCenterA); + xfB.p = cB - b2Mul(xfB.q, localCenterB); - b2PositionSolverManifold psm; - psm.Initialize(pc, xfA, xfB, j); - b2Vec2 normal = psm.normal; + b2PositionSolverManifold psm; + psm.Initialize(pc, xfA, xfB, j); + b2Vec2 normal = psm.normal; - b2Vec2 point = psm.point; - float32 separation = psm.separation; + b2Vec2 point = psm.point; + float32 separation = psm.separation; - b2Vec2 rA = point - cA; - b2Vec2 rB = point - cB; + b2Vec2 rA = point - cA; + b2Vec2 rB = point - cB; - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); + // Track max constraint error. + minSeparation = b2Min(minSeparation, separation); - // Prevent large corrections and allow slop. - float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); + // Prevent large corrections and allow slop. + float32 C = + b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - // Compute the effective mass. - float32 rnA = b2Cross(rA, normal); - float32 rnB = b2Cross(rB, normal); - float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + // Compute the effective mass. + float32 rnA = b2Cross(rA, normal); + float32 rnB = b2Cross(rB, normal); + float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; + // Compute normal impulse + float32 impulse = K > 0.0f ? -C / K : 0.0f; - b2Vec2 P = impulse * normal; + b2Vec2 P = impulse * normal; - cA -= mA * P; - aA -= iA * b2Cross(rA, P); + cA -= mA * P; + aA -= iA * b2Cross(rA, P); - cB += mB * P; - aB += iB * b2Cross(rB, P); - } + cB += mB * P; + aB += iB * b2Cross(rB, P); + } - m_positions[indexA].c = cA; - m_positions[indexA].a = aA; + m_positions[indexA].c = cA; + m_positions[indexA].a = aA; - m_positions[indexB].c = cB; - m_positions[indexB].a = aB; - } + m_positions[indexB].c = cB; + m_positions[indexB].a = aB; + } - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -3.0f * b2_linearSlop; + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -3.0f * b2_linearSlop; } // Sequential position solver for position constraints. bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB) { - float32 minSeparation = 0.0f; - - for (int32 i = 0; i < m_count; ++i) - { - b2ContactPositionConstraint* pc = m_positionConstraints + i; - - int32 indexA = pc->indexA; - int32 indexB = pc->indexB; - b2Vec2 localCenterA = pc->localCenterA; - b2Vec2 localCenterB = pc->localCenterB; - int32 pointCount = pc->pointCount; - - float32 mA = 0.0f; - float32 iA = 0.0f; - if (indexA == toiIndexA || indexA == toiIndexB) - { - mA = pc->invMassA; - iA = pc->invIA; - } - - float32 mB = 0.0f; - float32 iB = 0.; - if (indexB == toiIndexA || indexB == toiIndexB) - { - mB = pc->invMassB; - iB = pc->invIB; - } - - b2Vec2 cA = m_positions[indexA].c; - float32 aA = m_positions[indexA].a; - - b2Vec2 cB = m_positions[indexB].c; - float32 aB = m_positions[indexB].a; - - // Solve normal constraints - for (int32 j = 0; j < pointCount; ++j) - { - b2Transform xfA, xfB; - xfA.q.Set(aA); - xfB.q.Set(aB); - xfA.p = cA - b2Mul(xfA.q, localCenterA); - xfB.p = cB - b2Mul(xfB.q, localCenterB); - - b2PositionSolverManifold psm; - psm.Initialize(pc, xfA, xfB, j); - b2Vec2 normal = psm.normal; - - b2Vec2 point = psm.point; - float32 separation = psm.separation; - - b2Vec2 rA = point - cA; - b2Vec2 rB = point - cB; - - // Track max constraint error. - minSeparation = b2Min(minSeparation, separation); - - // Prevent large corrections and allow slop. - float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); - - // Compute the effective mass. - float32 rnA = b2Cross(rA, normal); - float32 rnB = b2Cross(rB, normal); - float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; - - // Compute normal impulse - float32 impulse = K > 0.0f ? - C / K : 0.0f; - - b2Vec2 P = impulse * normal; - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - - m_positions[indexA].c = cA; - m_positions[indexA].a = aA; - - m_positions[indexB].c = cB; - m_positions[indexB].a = aB; - } - - // We can't expect minSpeparation >= -b2_linearSlop because we don't - // push the separation above -b2_linearSlop. - return minSeparation >= -1.5f * b2_linearSlop; + float32 minSeparation = 0.0f; + + for (int32 i = 0; i < m_count; ++i) { + b2ContactPositionConstraint * pc = m_positionConstraints + i; + + int32 indexA = pc->indexA; + int32 indexB = pc->indexB; + b2Vec2 localCenterA = pc->localCenterA; + b2Vec2 localCenterB = pc->localCenterB; + int32 pointCount = pc->pointCount; + + float32 mA = 0.0f; + float32 iA = 0.0f; + if (indexA == toiIndexA || indexA == toiIndexB) { + mA = pc->invMassA; + iA = pc->invIA; + } + + float32 mB = 0.0f; + float32 iB = 0.; + if (indexB == toiIndexA || indexB == toiIndexB) { + mB = pc->invMassB; + iB = pc->invIB; + } + + b2Vec2 cA = m_positions[indexA].c; + float32 aA = m_positions[indexA].a; + + b2Vec2 cB = m_positions[indexB].c; + float32 aB = m_positions[indexB].a; + + // Solve normal constraints + for (int32 j = 0; j < pointCount; ++j) { + b2Transform xfA, xfB; + xfA.q.Set(aA); + xfB.q.Set(aB); + xfA.p = cA - b2Mul(xfA.q, localCenterA); + xfB.p = cB - b2Mul(xfB.q, localCenterB); + + b2PositionSolverManifold psm; + psm.Initialize(pc, xfA, xfB, j); + b2Vec2 normal = psm.normal; + + b2Vec2 point = psm.point; + float32 separation = psm.separation; + + b2Vec2 rA = point - cA; + b2Vec2 rB = point - cB; + + // Track max constraint error. + minSeparation = b2Min(minSeparation, separation); + + // Prevent large corrections and allow slop. + float32 C = + b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f); + + // Compute the effective mass. + float32 rnA = b2Cross(rA, normal); + float32 rnB = b2Cross(rB, normal); + float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + + // Compute normal impulse + float32 impulse = K > 0.0f ? -C / K : 0.0f; + + b2Vec2 P = impulse * normal; + + cA -= mA * P; + aA -= iA * b2Cross(rA, P); + + cB += mB * P; + aB += iB * b2Cross(rB, P); + } + + m_positions[indexA].c = cA; + m_positions[indexA].a = aA; + + m_positions[indexB].c = cB; + m_positions[indexB].a = aB; + } + + // We can't expect minSpeparation >= -b2_linearSlop because we don't + // push the separation above -b2_linearSlop. + return minSeparation >= -1.5f * b2_linearSlop; } diff --git a/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp index 3d67a39f..66367e01 100644 --- a/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp +++ b/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndCircleContact.cpp @@ -1,49 +1,52 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h" -#include "Box2D/Common/b2BlockAllocator.h" -#include "Box2D/Dynamics/b2Fixture.h" #include -b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" + +b2Contact * b2EdgeAndCircleContact::Create( + b2Fixture * fixtureA, int32, b2Fixture * fixtureB, int32, b2BlockAllocator * allocator) { - void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact)); - return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB); + void * mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact)); + return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB); } -void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +void b2EdgeAndCircleContact::Destroy(b2Contact * contact, b2BlockAllocator * allocator) { - ((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact(); - allocator->Free(contact, sizeof(b2EdgeAndCircleContact)); + ((b2EdgeAndCircleContact *)contact)->~b2EdgeAndCircleContact(); + allocator->Free(contact, sizeof(b2EdgeAndCircleContact)); } -b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture * fixtureA, b2Fixture * fixtureB) : b2Contact(fixtureA, 0, fixtureB, 0) { - b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); + b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); } -void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +void b2EdgeAndCircleContact::Evaluate( + b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) { - b2CollideEdgeAndCircle( manifold, - (b2EdgeShape*)m_fixtureA->GetShape(), xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); + b2CollideEdgeAndCircle( + manifold, (b2EdgeShape *)m_fixtureA->GetShape(), xfA, (b2CircleShape *)m_fixtureB->GetShape(), + xfB); } diff --git a/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp index de82ffed..7d6cc4ff 100644 --- a/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp +++ b/flatland_box2d/src/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp @@ -1,49 +1,52 @@ /* -* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2010 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h" -#include "Box2D/Common/b2BlockAllocator.h" -#include "Box2D/Dynamics/b2Fixture.h" #include -b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" + +b2Contact * b2EdgeAndPolygonContact::Create( + b2Fixture * fixtureA, int32, b2Fixture * fixtureB, int32, b2BlockAllocator * allocator) { - void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact)); - return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB); + void * mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact)); + return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB); } -void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +void b2EdgeAndPolygonContact::Destroy(b2Contact * contact, b2BlockAllocator * allocator) { - ((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact(); - allocator->Free(contact, sizeof(b2EdgeAndPolygonContact)); + ((b2EdgeAndPolygonContact *)contact)->~b2EdgeAndPolygonContact(); + allocator->Free(contact, sizeof(b2EdgeAndPolygonContact)); } -b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture * fixtureA, b2Fixture * fixtureB) : b2Contact(fixtureA, 0, fixtureB, 0) { - b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); - b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); + b2Assert(m_fixtureA->GetType() == b2Shape::e_edge); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); } -void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +void b2EdgeAndPolygonContact::Evaluate( + b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) { - b2CollideEdgeAndPolygon( manifold, - (b2EdgeShape*)m_fixtureA->GetShape(), xfA, - (b2PolygonShape*)m_fixtureB->GetShape(), xfB); + b2CollideEdgeAndPolygon( + manifold, (b2EdgeShape *)m_fixtureA->GetShape(), xfA, (b2PolygonShape *)m_fixtureB->GetShape(), + xfB); } diff --git a/flatland_box2d/src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp index 5734e833..a96b3717 100644 --- a/flatland_box2d/src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp +++ b/flatland_box2d/src/Dynamics/Contacts/b2PolygonAndCircleContact.cpp @@ -1,49 +1,52 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h" -#include "Box2D/Common/b2BlockAllocator.h" -#include "Box2D/Dynamics/b2Fixture.h" #include -b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +#include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/b2Fixture.h" + +b2Contact * b2PolygonAndCircleContact::Create( + b2Fixture * fixtureA, int32, b2Fixture * fixtureB, int32, b2BlockAllocator * allocator) { - void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact)); - return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB); + void * mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact)); + return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB); } -void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +void b2PolygonAndCircleContact::Destroy(b2Contact * contact, b2BlockAllocator * allocator) { - ((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact(); - allocator->Free(contact, sizeof(b2PolygonAndCircleContact)); + ((b2PolygonAndCircleContact *)contact)->~b2PolygonAndCircleContact(); + allocator->Free(contact, sizeof(b2PolygonAndCircleContact)); } -b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB) +b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture * fixtureA, b2Fixture * fixtureB) : b2Contact(fixtureA, 0, fixtureB, 0) { - b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); - b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); + b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); + b2Assert(m_fixtureB->GetType() == b2Shape::e_circle); } -void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +void b2PolygonAndCircleContact::Evaluate( + b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) { - b2CollidePolygonAndCircle( manifold, - (b2PolygonShape*)m_fixtureA->GetShape(), xfA, - (b2CircleShape*)m_fixtureB->GetShape(), xfB); + b2CollidePolygonAndCircle( + manifold, (b2PolygonShape *)m_fixtureA->GetShape(), xfA, + (b2CircleShape *)m_fixtureB->GetShape(), xfB); } diff --git a/flatland_box2d/src/Dynamics/Contacts/b2PolygonContact.cpp b/flatland_box2d/src/Dynamics/Contacts/b2PolygonContact.cpp index 0af3b6c3..300dfe13 100644 --- a/flatland_box2d/src/Dynamics/Contacts/b2PolygonContact.cpp +++ b/flatland_box2d/src/Dynamics/Contacts/b2PolygonContact.cpp @@ -1,52 +1,55 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Contacts/b2PolygonContact.h" -#include "Box2D/Common/b2BlockAllocator.h" + +#include + #include "Box2D/Collision/b2TimeOfImpact.h" +#include "Box2D/Common/b2BlockAllocator.h" #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2Fixture.h" #include "Box2D/Dynamics/b2WorldCallbacks.h" -#include - -b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator) +b2Contact * b2PolygonContact::Create( + b2Fixture * fixtureA, int32, b2Fixture * fixtureB, int32, b2BlockAllocator * allocator) { - void* mem = allocator->Allocate(sizeof(b2PolygonContact)); - return new (mem) b2PolygonContact(fixtureA, fixtureB); + void * mem = allocator->Allocate(sizeof(b2PolygonContact)); + return new (mem) b2PolygonContact(fixtureA, fixtureB); } -void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator) +void b2PolygonContact::Destroy(b2Contact * contact, b2BlockAllocator * allocator) { - ((b2PolygonContact*)contact)->~b2PolygonContact(); - allocator->Free(contact, sizeof(b2PolygonContact)); + ((b2PolygonContact *)contact)->~b2PolygonContact(); + allocator->Free(contact, sizeof(b2PolygonContact)); } -b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB) - : b2Contact(fixtureA, 0, fixtureB, 0) +b2PolygonContact::b2PolygonContact(b2Fixture * fixtureA, b2Fixture * fixtureB) +: b2Contact(fixtureA, 0, fixtureB, 0) { - b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); - b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); + b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon); + b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon); } -void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) +void b2PolygonContact::Evaluate( + b2Manifold * manifold, const b2Transform & xfA, const b2Transform & xfB) { - b2CollidePolygons( manifold, - (b2PolygonShape*)m_fixtureA->GetShape(), xfA, - (b2PolygonShape*)m_fixtureB->GetShape(), xfB); + b2CollidePolygons( + manifold, (b2PolygonShape *)m_fixtureA->GetShape(), xfA, + (b2PolygonShape *)m_fixtureB->GetShape(), xfB); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2DistanceJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2DistanceJoint.cpp index 0d8bf7c4..71dfe12e 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2DistanceJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2DistanceJoint.cpp @@ -1,22 +1,23 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2DistanceJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" @@ -26,7 +27,7 @@ // x2 = x1 + h * v2 // 1-D mass-damper-spring system -// m (v2 - v1) + h * d * v2 + h * k * +// m (v2 - v1) + h * d * v2 + h * k * // C = norm(p2 - p1) - L // u = (p2 - p1) / norm(p2 - p1) @@ -35,226 +36,209 @@ // K = J * invM * JT // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 -void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2, - const b2Vec2& anchor1, const b2Vec2& anchor2) +void b2DistanceJointDef::Initialize( + b2Body * b1, b2Body * b2, const b2Vec2 & anchor1, const b2Vec2 & anchor2) { - bodyA = b1; - bodyB = b2; - localAnchorA = bodyA->GetLocalPoint(anchor1); - localAnchorB = bodyB->GetLocalPoint(anchor2); - b2Vec2 d = anchor2 - anchor1; - length = d.Length(); + bodyA = b1; + bodyB = b2; + localAnchorA = bodyA->GetLocalPoint(anchor1); + localAnchorB = bodyB->GetLocalPoint(anchor2); + b2Vec2 d = anchor2 - anchor1; + length = d.Length(); } -b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def) -: b2Joint(def) +b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef * def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_length = def->length; - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - m_impulse = 0.0f; - m_gamma = 0.0f; - m_bias = 0.0f; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_length = def->length; + m_frequencyHz = def->frequencyHz; + m_dampingRatio = def->dampingRatio; + m_impulse = 0.0f; + m_gamma = 0.0f; + m_bias = 0.0f; } -void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data) +void b2DistanceJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - m_u = cB + m_rB - cA - m_rA; - - // Handle singularity. - float32 length = m_u.Length(); - if (length > b2_linearSlop) - { - m_u *= 1.0f / length; - } - else - { - m_u.Set(0.0f, 0.0f); - } - - float32 crAu = b2Cross(m_rA, m_u); - float32 crBu = b2Cross(m_rB, m_u); - float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; - - // Compute the effective mass matrix. - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - - if (m_frequencyHz > 0.0f) - { - float32 C = length - m_length; - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * m_mass * m_dampingRatio * omega; - - // Spring stiffness - float32 k = m_mass * omega * omega; - - // magic formulas - float32 h = data.step.dt; - m_gamma = h * (d + h * k); - m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; - m_bias = C * h * k * m_gamma; - - invMass += m_gamma; - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - } - else - { - m_gamma = 0.0f; - m_bias = 0.0f; - } - - if (data.step.warmStarting) - { - // Scale the impulse to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + m_u = cB + m_rB - cA - m_rA; + + // Handle singularity. + float32 length = m_u.Length(); + if (length > b2_linearSlop) { + m_u *= 1.0f / length; + } else { + m_u.Set(0.0f, 0.0f); + } + + float32 crAu = b2Cross(m_rA, m_u); + float32 crBu = b2Cross(m_rB, m_u); + float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; + + // Compute the effective mass matrix. + m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (m_frequencyHz > 0.0f) { + float32 C = length - m_length; + + // Frequency + float32 omega = 2.0f * b2_pi * m_frequencyHz; + + // Damping coefficient + float32 d = 2.0f * m_mass * m_dampingRatio * omega; + + // Spring stiffness + float32 k = m_mass * omega * omega; + + // magic formulas + float32 h = data.step.dt; + m_gamma = h * (d + h * k); + m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; + m_bias = C * h * k * m_gamma; + + invMass += m_gamma; + m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + } else { + m_gamma = 0.0f; + m_bias = 0.0f; + } + + if (data.step.warmStarting) { + // Scale the impulse to support a variable time step. + m_impulse *= data.step.dtRatio; + + b2Vec2 P = m_impulse * m_u; + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + } else { + m_impulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData & data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float32 Cdot = b2Dot(m_u, vpB - vpA); - - float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse); - m_impulse += impulse; - - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float32 Cdot = b2Dot(m_u, vpB - vpA); + + float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse); + m_impulse += impulse; + + b2Vec2 P = impulse * m_u; + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data) +bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData & data) { - if (m_frequencyHz > 0.0f) - { - // There is no position correction for soft distance constraints. - return true; - } + if (m_frequencyHz > 0.0f) { + // There is no position correction for soft distance constraints. + return true; + } - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; - b2Rot qA(aA), qB(aB); + b2Rot qA(aA), qB(aB); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 u = cB + rB - cA - rA; + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 u = cB + rB - cA - rA; - float32 length = u.Normalize(); - float32 C = length - m_length; - C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection); + float32 length = u.Normalize(); + float32 C = length - m_length; + C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection); - float32 impulse = -m_mass * C; - b2Vec2 P = impulse * u; + float32 impulse = -m_mass * C; + b2Vec2 P = impulse * u; - cA -= m_invMassA * P; - aA -= m_invIA * b2Cross(rA, P); - cB += m_invMassB * P; - aB += m_invIB * b2Cross(rB, P); + cA -= m_invMassA * P; + aA -= m_invIA * b2Cross(rA, P); + cB += m_invMassB * P; + aB += m_invIB * b2Cross(rB, P); - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; - return b2Abs(C) < b2_linearSlop; + return b2Abs(C) < b2_linearSlop; } -b2Vec2 b2DistanceJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} +b2Vec2 b2DistanceJoint::GetAnchorA() const { return m_bodyA->GetWorldPoint(m_localAnchorA); } -b2Vec2 b2DistanceJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} +b2Vec2 b2DistanceJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 F = (inv_dt * m_impulse) * m_u; - return F; + b2Vec2 F = (inv_dt * m_impulse) * m_u; + return F; } float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const { - B2_NOT_USED(inv_dt); - return 0.0f; + B2_NOT_USED(inv_dt); + return 0.0f; } void b2DistanceJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2DistanceJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.length = %.15lef;\n", m_length); - b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); - b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2DistanceJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.length = %.15lef;\n", m_length); + b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); + b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2FrictionJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2FrictionJoint.cpp index 4047041d..615fff5b 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2FrictionJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2FrictionJoint.cpp @@ -1,22 +1,23 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2FrictionJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" @@ -32,220 +33,199 @@ // J = [0 0 -1 0 0 1] // K = invI1 + invI2 -void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) +void b2FrictionJointDef::Initialize(b2Body * bA, b2Body * bB, const b2Vec2 & anchor) { - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); } -b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def) -: b2Joint(def) +b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef * def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; + m_linearImpulse.SetZero(); + m_angularImpulse = 0.0f; - m_maxForce = def->maxForce; - m_maxTorque = def->maxTorque; + m_maxForce = def->maxForce; + m_maxTorque = def->maxTorque; } -void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data) +void b2FrictionJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective mass matrix. - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; - K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; - - m_linearMass = K.GetInverse(); - - m_angularMass = iA + iB; - if (m_angularMass > 0.0f) - { - m_angularMass = 1.0f / m_angularMass; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_linearImpulse *= data.step.dtRatio; - m_angularImpulse *= data.step.dtRatio; - - b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); - } - else - { - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + // Compute the effective mass matrix. + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] [ + // -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Mat22 K; + K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; + K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; + K.ey.x = K.ex.y; + K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; + + m_linearMass = K.GetInverse(); + + m_angularMass = iA + iB; + if (m_angularMass > 0.0f) { + m_angularMass = 1.0f / m_angularMass; + } + + if (data.step.warmStarting) { + // Scale impulses to support a variable time step. + m_linearImpulse *= data.step.dtRatio; + m_angularImpulse *= data.step.dtRatio; + + b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); + } else { + m_linearImpulse.SetZero(); + m_angularImpulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData & data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - float32 h = data.step.dt; + float32 h = data.step.dt; - // Solve angular friction - { - float32 Cdot = wB - wA; - float32 impulse = -m_angularMass * Cdot; + // Solve angular friction + { + float32 Cdot = wB - wA; + float32 impulse = -m_angularMass * Cdot; - float32 oldImpulse = m_angularImpulse; - float32 maxImpulse = h * m_maxTorque; - m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_angularImpulse - oldImpulse; + float32 oldImpulse = m_angularImpulse; + float32 maxImpulse = h * m_maxTorque; + m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_angularImpulse - oldImpulse; - wA -= iA * impulse; - wB += iB * impulse; - } + wA -= iA * impulse; + wB += iB * impulse; + } - // Solve linear friction - { - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + // Solve linear friction + { + b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); - b2Vec2 oldImpulse = m_linearImpulse; - m_linearImpulse += impulse; + b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); + b2Vec2 oldImpulse = m_linearImpulse; + m_linearImpulse += impulse; - float32 maxImpulse = h * m_maxForce; + float32 maxImpulse = h * m_maxForce; - if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_linearImpulse.Normalize(); - m_linearImpulse *= maxImpulse; - } + if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { + m_linearImpulse.Normalize(); + m_linearImpulse *= maxImpulse; + } - impulse = m_linearImpulse - oldImpulse; + impulse = m_linearImpulse - oldImpulse; - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); + vA -= mA * impulse; + wA -= iA * b2Cross(m_rA, impulse); - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } + vB += mB * impulse; + wB += iB * b2Cross(m_rB, impulse); + } - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data) +bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData & data) { - B2_NOT_USED(data); + B2_NOT_USED(data); - return true; + return true; } -b2Vec2 b2FrictionJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} +b2Vec2 b2FrictionJoint::GetAnchorA() const { return m_bodyA->GetWorldPoint(m_localAnchorA); } -b2Vec2 b2FrictionJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} +b2Vec2 b2FrictionJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } -b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * m_linearImpulse; -} +b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const { return inv_dt * m_linearImpulse; } float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const { - return inv_dt * m_angularImpulse; + return inv_dt * m_angularImpulse; } void b2FrictionJoint::SetMaxForce(float32 force) { - b2Assert(b2IsValid(force) && force >= 0.0f); - m_maxForce = force; + b2Assert(b2IsValid(force) && force >= 0.0f); + m_maxForce = force; } -float32 b2FrictionJoint::GetMaxForce() const -{ - return m_maxForce; -} +float32 b2FrictionJoint::GetMaxForce() const { return m_maxForce; } void b2FrictionJoint::SetMaxTorque(float32 torque) { - b2Assert(b2IsValid(torque) && torque >= 0.0f); - m_maxTorque = torque; + b2Assert(b2IsValid(torque) && torque >= 0.0f); + m_maxTorque = torque; } -float32 b2FrictionJoint::GetMaxTorque() const -{ - return m_maxTorque; -} +float32 b2FrictionJoint::GetMaxTorque() const { return m_maxTorque; } void b2FrictionJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2FrictionJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); - b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2FrictionJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); + b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2GearJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2GearJoint.cpp index a9010f82..dcbc22fd 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2GearJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2GearJoint.cpp @@ -1,24 +1,25 @@ /* -* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2007-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2GearJoint.h" -#include "Box2D/Dynamics/Joints/b2RevoluteJoint.h" + #include "Box2D/Dynamics/Joints/b2PrismaticJoint.h" +#include "Box2D/Dynamics/Joints/b2RevoluteJoint.h" #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" @@ -41,379 +42,347 @@ // J = [ug cross(r, ug)] // K = J * invM * JT = invMass + invI * cross(r, ug)^2 -b2GearJoint::b2GearJoint(const b2GearJointDef* def) -: b2Joint(def) +b2GearJoint::b2GearJoint(const b2GearJointDef * def) : b2Joint(def) { - m_joint1 = def->joint1; - m_joint2 = def->joint2; - - m_typeA = m_joint1->GetType(); - m_typeB = m_joint2->GetType(); - - b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint); - b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint); - - float32 coordinateA, coordinateB; - - // TODO_ERIN there might be some problem with the joint edges in b2Joint. - - m_bodyC = m_joint1->GetBodyA(); - m_bodyA = m_joint1->GetBodyB(); - - // Get geometry of joint1 - b2Transform xfA = m_bodyA->m_xf; - float32 aA = m_bodyA->m_sweep.a; - b2Transform xfC = m_bodyC->m_xf; - float32 aC = m_bodyC->m_sweep.a; - - if (m_typeA == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1; - m_localAnchorC = revolute->m_localAnchorA; - m_localAnchorA = revolute->m_localAnchorB; - m_referenceAngleA = revolute->m_referenceAngle; - m_localAxisC.SetZero(); - - coordinateA = aA - aC - m_referenceAngleA; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1; - m_localAnchorC = prismatic->m_localAnchorA; - m_localAnchorA = prismatic->m_localAnchorB; - m_referenceAngleA = prismatic->m_referenceAngle; - m_localAxisC = prismatic->m_localXAxisA; - - b2Vec2 pC = m_localAnchorC; - b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - } - - m_bodyD = m_joint2->GetBodyA(); - m_bodyB = m_joint2->GetBodyB(); - - // Get geometry of joint2 - b2Transform xfB = m_bodyB->m_xf; - float32 aB = m_bodyB->m_sweep.a; - b2Transform xfD = m_bodyD->m_xf; - float32 aD = m_bodyD->m_sweep.a; - - if (m_typeB == e_revoluteJoint) - { - b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2; - m_localAnchorD = revolute->m_localAnchorA; - m_localAnchorB = revolute->m_localAnchorB; - m_referenceAngleB = revolute->m_referenceAngle; - m_localAxisD.SetZero(); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2; - m_localAnchorD = prismatic->m_localAnchorA; - m_localAnchorB = prismatic->m_localAnchorB; - m_referenceAngleB = prismatic->m_referenceAngle; - m_localAxisD = prismatic->m_localXAxisA; - - b2Vec2 pD = m_localAnchorD; - b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - m_ratio = def->ratio; - - m_constant = coordinateA + m_ratio * coordinateB; - - m_impulse = 0.0f; + m_joint1 = def->joint1; + m_joint2 = def->joint2; + + m_typeA = m_joint1->GetType(); + m_typeB = m_joint2->GetType(); + + b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint); + b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint); + + float32 coordinateA, coordinateB; + + // TODO_ERIN there might be some problem with the joint edges in b2Joint. + + m_bodyC = m_joint1->GetBodyA(); + m_bodyA = m_joint1->GetBodyB(); + + // Get geometry of joint1 + b2Transform xfA = m_bodyA->m_xf; + float32 aA = m_bodyA->m_sweep.a; + b2Transform xfC = m_bodyC->m_xf; + float32 aC = m_bodyC->m_sweep.a; + + if (m_typeA == e_revoluteJoint) { + b2RevoluteJoint * revolute = (b2RevoluteJoint *)def->joint1; + m_localAnchorC = revolute->m_localAnchorA; + m_localAnchorA = revolute->m_localAnchorB; + m_referenceAngleA = revolute->m_referenceAngle; + m_localAxisC.SetZero(); + + coordinateA = aA - aC - m_referenceAngleA; + } else { + b2PrismaticJoint * prismatic = (b2PrismaticJoint *)def->joint1; + m_localAnchorC = prismatic->m_localAnchorA; + m_localAnchorA = prismatic->m_localAnchorB; + m_referenceAngleA = prismatic->m_referenceAngle; + m_localAxisC = prismatic->m_localXAxisA; + + b2Vec2 pC = m_localAnchorC; + b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p)); + coordinateA = b2Dot(pA - pC, m_localAxisC); + } + + m_bodyD = m_joint2->GetBodyA(); + m_bodyB = m_joint2->GetBodyB(); + + // Get geometry of joint2 + b2Transform xfB = m_bodyB->m_xf; + float32 aB = m_bodyB->m_sweep.a; + b2Transform xfD = m_bodyD->m_xf; + float32 aD = m_bodyD->m_sweep.a; + + if (m_typeB == e_revoluteJoint) { + b2RevoluteJoint * revolute = (b2RevoluteJoint *)def->joint2; + m_localAnchorD = revolute->m_localAnchorA; + m_localAnchorB = revolute->m_localAnchorB; + m_referenceAngleB = revolute->m_referenceAngle; + m_localAxisD.SetZero(); + + coordinateB = aB - aD - m_referenceAngleB; + } else { + b2PrismaticJoint * prismatic = (b2PrismaticJoint *)def->joint2; + m_localAnchorD = prismatic->m_localAnchorA; + m_localAnchorB = prismatic->m_localAnchorB; + m_referenceAngleB = prismatic->m_referenceAngle; + m_localAxisD = prismatic->m_localXAxisA; + + b2Vec2 pD = m_localAnchorD; + b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p)); + coordinateB = b2Dot(pB - pD, m_localAxisD); + } + + m_ratio = def->ratio; + + m_constant = coordinateA + m_ratio * coordinateB; + + m_impulse = 0.0f; } -void b2GearJoint::InitVelocityConstraints(const b2SolverData& data) +void b2GearJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_indexC = m_bodyC->m_islandIndex; - m_indexD = m_bodyD->m_islandIndex; - m_lcA = m_bodyA->m_sweep.localCenter; - m_lcB = m_bodyB->m_sweep.localCenter; - m_lcC = m_bodyC->m_sweep.localCenter; - m_lcD = m_bodyD->m_sweep.localCenter; - m_mA = m_bodyA->m_invMass; - m_mB = m_bodyB->m_invMass; - m_mC = m_bodyC->m_invMass; - m_mD = m_bodyD->m_invMass; - m_iA = m_bodyA->m_invI; - m_iB = m_bodyB->m_invI; - m_iC = m_bodyC->m_invI; - m_iD = m_bodyD->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 aC = data.positions[m_indexC].a; - b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; - - float32 aD = data.positions[m_indexD].a; - b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - m_mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - m_JvAC.SetZero(); - m_JwA = 1.0f; - m_JwC = 1.0f; - m_mass += m_iA + m_iC; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - m_JvAC = u; - m_JwC = b2Cross(rC, u); - m_JwA = b2Cross(rA, u); - m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; - } - - if (m_typeB == e_revoluteJoint) - { - m_JvBD.SetZero(); - m_JwB = m_ratio; - m_JwD = m_ratio; - m_mass += m_ratio * m_ratio * (m_iB + m_iD); - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - m_JvBD = m_ratio * u; - m_JwD = m_ratio * b2Cross(rD, u); - m_JwB = m_ratio * b2Cross(rB, u); - m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; - } - - // Compute effective mass. - m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; - - if (data.step.warmStarting) - { - vA += (m_mA * m_impulse) * m_JvAC; - wA += m_iA * m_impulse * m_JwA; - vB += (m_mB * m_impulse) * m_JvBD; - wB += m_iB * m_impulse * m_JwB; - vC -= (m_mC * m_impulse) * m_JvAC; - wC -= m_iC * m_impulse * m_JwC; - vD -= (m_mD * m_impulse) * m_JvBD; - wD -= m_iD * m_impulse * m_JwD; - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_indexC = m_bodyC->m_islandIndex; + m_indexD = m_bodyD->m_islandIndex; + m_lcA = m_bodyA->m_sweep.localCenter; + m_lcB = m_bodyB->m_sweep.localCenter; + m_lcC = m_bodyC->m_sweep.localCenter; + m_lcD = m_bodyD->m_sweep.localCenter; + m_mA = m_bodyA->m_invMass; + m_mB = m_bodyB->m_invMass; + m_mC = m_bodyC->m_invMass; + m_mD = m_bodyD->m_invMass; + m_iA = m_bodyA->m_invI; + m_iB = m_bodyB->m_invI; + m_iC = m_bodyC->m_invI; + m_iD = m_bodyD->m_invI; + + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + float32 aC = data.positions[m_indexC].a; + b2Vec2 vC = data.velocities[m_indexC].v; + float32 wC = data.velocities[m_indexC].w; + + float32 aD = data.positions[m_indexD].a; + b2Vec2 vD = data.velocities[m_indexD].v; + float32 wD = data.velocities[m_indexD].w; + + b2Rot qA(aA), qB(aB), qC(aC), qD(aD); + + m_mass = 0.0f; + + if (m_typeA == e_revoluteJoint) { + m_JvAC.SetZero(); + m_JwA = 1.0f; + m_JwC = 1.0f; + m_mass += m_iA + m_iC; + } else { + b2Vec2 u = b2Mul(qC, m_localAxisC); + b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); + m_JvAC = u; + m_JwC = b2Cross(rC, u); + m_JwA = b2Cross(rA, u); + m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; + } + + if (m_typeB == e_revoluteJoint) { + m_JvBD.SetZero(); + m_JwB = m_ratio; + m_JwD = m_ratio; + m_mass += m_ratio * m_ratio * (m_iB + m_iD); + } else { + b2Vec2 u = b2Mul(qD, m_localAxisD); + b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); + m_JvBD = m_ratio * u; + m_JwD = m_ratio * b2Cross(rD, u); + m_JwB = m_ratio * b2Cross(rB, u); + m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; + } + + // Compute effective mass. + m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; + + if (data.step.warmStarting) { + vA += (m_mA * m_impulse) * m_JvAC; + wA += m_iA * m_impulse * m_JwA; + vB += (m_mB * m_impulse) * m_JvBD; + wB += m_iB * m_impulse * m_JwB; + vC -= (m_mC * m_impulse) * m_JvAC; + wC -= m_iC * m_impulse * m_JwC; + vD -= (m_mD * m_impulse) * m_JvBD; + wD -= m_iD * m_impulse * m_JwD; + } else { + m_impulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; + data.velocities[m_indexC].v = vC; + data.velocities[m_indexC].w = wC; + data.velocities[m_indexD].v = vD; + data.velocities[m_indexD].w = wD; } -void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2GearJoint::SolveVelocityConstraints(const b2SolverData & data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - b2Vec2 vC = data.velocities[m_indexC].v; - float32 wC = data.velocities[m_indexC].w; - b2Vec2 vD = data.velocities[m_indexD].v; - float32 wD = data.velocities[m_indexD].w; - - float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); - Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); - - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - vA += (m_mA * impulse) * m_JvAC; - wA += m_iA * impulse * m_JwA; - vB += (m_mB * impulse) * m_JvBD; - wB += m_iB * impulse * m_JwB; - vC -= (m_mC * impulse) * m_JvAC; - wC -= m_iC * impulse * m_JwC; - vD -= (m_mD * impulse) * m_JvBD; - wD -= m_iD * impulse * m_JwD; - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; - data.velocities[m_indexC].v = vC; - data.velocities[m_indexC].w = wC; - data.velocities[m_indexD].v = vD; - data.velocities[m_indexD].w = wD; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + b2Vec2 vC = data.velocities[m_indexC].v; + float32 wC = data.velocities[m_indexC].w; + b2Vec2 vD = data.velocities[m_indexD].v; + float32 wD = data.velocities[m_indexD].w; + + float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD); + Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD); + + float32 impulse = -m_mass * Cdot; + m_impulse += impulse; + + vA += (m_mA * impulse) * m_JvAC; + wA += m_iA * impulse * m_JwA; + vB += (m_mB * impulse) * m_JvBD; + wB += m_iB * impulse * m_JwB; + vC -= (m_mC * impulse) * m_JvAC; + wC -= m_iC * impulse * m_JwC; + vD -= (m_mD * impulse) * m_JvBD; + wD -= m_iD * impulse * m_JwD; + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; + data.velocities[m_indexC].v = vC; + data.velocities[m_indexC].w = wC; + data.velocities[m_indexD].v = vD; + data.velocities[m_indexD].w = wD; } -bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data) +bool b2GearJoint::SolvePositionConstraints(const b2SolverData & data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 cC = data.positions[m_indexC].c; - float32 aC = data.positions[m_indexC].a; - b2Vec2 cD = data.positions[m_indexD].c; - float32 aD = data.positions[m_indexD].a; - - b2Rot qA(aA), qB(aB), qC(aC), qD(aD); - - float32 linearError = 0.0f; - - float32 coordinateA, coordinateB; - - b2Vec2 JvAC, JvBD; - float32 JwA, JwB, JwC, JwD; - float32 mass = 0.0f; - - if (m_typeA == e_revoluteJoint) - { - JvAC.SetZero(); - JwA = 1.0f; - JwC = 1.0f; - mass += m_iA + m_iC; - - coordinateA = aA - aC - m_referenceAngleA; - } - else - { - b2Vec2 u = b2Mul(qC, m_localAxisC); - b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); - JvAC = u; - JwC = b2Cross(rC, u); - JwA = b2Cross(rA, u); - mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; - - b2Vec2 pC = m_localAnchorC - m_lcC; - b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); - coordinateA = b2Dot(pA - pC, m_localAxisC); - } - - if (m_typeB == e_revoluteJoint) - { - JvBD.SetZero(); - JwB = m_ratio; - JwD = m_ratio; - mass += m_ratio * m_ratio * (m_iB + m_iD); - - coordinateB = aB - aD - m_referenceAngleB; - } - else - { - b2Vec2 u = b2Mul(qD, m_localAxisD); - b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); - JvBD = m_ratio * u; - JwD = m_ratio * b2Cross(rD, u); - JwB = m_ratio * b2Cross(rB, u); - mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; - - b2Vec2 pD = m_localAnchorD - m_lcD; - b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); - coordinateB = b2Dot(pB - pD, m_localAxisD); - } - - float32 C = (coordinateA + m_ratio * coordinateB) - m_constant; - - float32 impulse = 0.0f; - if (mass > 0.0f) - { - impulse = -C / mass; - } - - cA += m_mA * impulse * JvAC; - aA += m_iA * impulse * JwA; - cB += m_mB * impulse * JvBD; - aB += m_iB * impulse * JwB; - cC -= m_mC * impulse * JvAC; - aC -= m_iC * impulse * JwC; - cD -= m_mD * impulse * JvBD; - aD -= m_iD * impulse * JwD; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - data.positions[m_indexC].c = cC; - data.positions[m_indexC].a = aC; - data.positions[m_indexD].c = cD; - data.positions[m_indexD].a = aD; - - // TODO_ERIN not implemented - return linearError < b2_linearSlop; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 cC = data.positions[m_indexC].c; + float32 aC = data.positions[m_indexC].a; + b2Vec2 cD = data.positions[m_indexD].c; + float32 aD = data.positions[m_indexD].a; + + b2Rot qA(aA), qB(aB), qC(aC), qD(aD); + + float32 linearError = 0.0f; + + float32 coordinateA, coordinateB; + + b2Vec2 JvAC, JvBD; + float32 JwA, JwB, JwC, JwD; + float32 mass = 0.0f; + + if (m_typeA == e_revoluteJoint) { + JvAC.SetZero(); + JwA = 1.0f; + JwC = 1.0f; + mass += m_iA + m_iC; + + coordinateA = aA - aC - m_referenceAngleA; + } else { + b2Vec2 u = b2Mul(qC, m_localAxisC); + b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA); + JvAC = u; + JwC = b2Cross(rC, u); + JwA = b2Cross(rA, u); + mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; + + b2Vec2 pC = m_localAnchorC - m_lcC; + b2Vec2 pA = b2MulT(qC, rA + (cA - cC)); + coordinateA = b2Dot(pA - pC, m_localAxisC); + } + + if (m_typeB == e_revoluteJoint) { + JvBD.SetZero(); + JwB = m_ratio; + JwD = m_ratio; + mass += m_ratio * m_ratio * (m_iB + m_iD); + + coordinateB = aB - aD - m_referenceAngleB; + } else { + b2Vec2 u = b2Mul(qD, m_localAxisD); + b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB); + JvBD = m_ratio * u; + JwD = m_ratio * b2Cross(rD, u); + JwB = m_ratio * b2Cross(rB, u); + mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; + + b2Vec2 pD = m_localAnchorD - m_lcD; + b2Vec2 pB = b2MulT(qD, rB + (cB - cD)); + coordinateB = b2Dot(pB - pD, m_localAxisD); + } + + float32 C = (coordinateA + m_ratio * coordinateB) - m_constant; + + float32 impulse = 0.0f; + if (mass > 0.0f) { + impulse = -C / mass; + } + + cA += m_mA * impulse * JvAC; + aA += m_iA * impulse * JwA; + cB += m_mB * impulse * JvBD; + aB += m_iB * impulse * JwB; + cC -= m_mC * impulse * JvAC; + aC -= m_iC * impulse * JwC; + cD -= m_mD * impulse * JvBD; + aD -= m_iD * impulse * JwD; + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + data.positions[m_indexC].c = cC; + data.positions[m_indexC].a = aC; + data.positions[m_indexD].c = cD; + data.positions[m_indexD].a = aD; + + // TODO_ERIN not implemented + return linearError < b2_linearSlop; } -b2Vec2 b2GearJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} +b2Vec2 b2GearJoint::GetAnchorA() const { return m_bodyA->GetWorldPoint(m_localAnchorA); } -b2Vec2 b2GearJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} +b2Vec2 b2GearJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 P = m_impulse * m_JvAC; - return inv_dt * P; + b2Vec2 P = m_impulse * m_JvAC; + return inv_dt * P; } float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const { - float32 L = m_impulse * m_JwA; - return inv_dt * L; + float32 L = m_impulse * m_JwA; + return inv_dt * L; } void b2GearJoint::SetRatio(float32 ratio) { - b2Assert(b2IsValid(ratio)); - m_ratio = ratio; + b2Assert(b2IsValid(ratio)); + m_ratio = ratio; } -float32 b2GearJoint::GetRatio() const -{ - return m_ratio; -} +float32 b2GearJoint::GetRatio() const { return m_ratio; } void b2GearJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - int32 index1 = m_joint1->m_index; - int32 index2 = m_joint2->m_index; - - b2Log(" b2GearJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.joint1 = joints[%d];\n", index1); - b2Log(" jd.joint2 = joints[%d];\n", index2); - b2Log(" jd.ratio = %.15lef;\n", m_ratio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + int32 index1 = m_joint1->m_index; + int32 index2 = m_joint2->m_index; + + b2Log(" b2GearJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.joint1 = joints[%d];\n", index1); + b2Log(" jd.joint2 = joints[%d];\n", index2); + b2Log(" jd.ratio = %.15lef;\n", m_ratio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2Joint.cpp b/flatland_box2d/src/Dynamics/Joints/b2Joint.cpp index fed072ae..21932275 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2Joint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2Joint.cpp @@ -1,211 +1,185 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2Joint.h" + +#include + +#include "Box2D/Common/b2BlockAllocator.h" #include "Box2D/Dynamics/Joints/b2DistanceJoint.h" -#include "Box2D/Dynamics/Joints/b2WheelJoint.h" +#include "Box2D/Dynamics/Joints/b2FrictionJoint.h" +#include "Box2D/Dynamics/Joints/b2GearJoint.h" +#include "Box2D/Dynamics/Joints/b2MotorJoint.h" #include "Box2D/Dynamics/Joints/b2MouseJoint.h" -#include "Box2D/Dynamics/Joints/b2RevoluteJoint.h" #include "Box2D/Dynamics/Joints/b2PrismaticJoint.h" #include "Box2D/Dynamics/Joints/b2PulleyJoint.h" -#include "Box2D/Dynamics/Joints/b2GearJoint.h" -#include "Box2D/Dynamics/Joints/b2WeldJoint.h" -#include "Box2D/Dynamics/Joints/b2FrictionJoint.h" +#include "Box2D/Dynamics/Joints/b2RevoluteJoint.h" #include "Box2D/Dynamics/Joints/b2RopeJoint.h" -#include "Box2D/Dynamics/Joints/b2MotorJoint.h" +#include "Box2D/Dynamics/Joints/b2WeldJoint.h" +#include "Box2D/Dynamics/Joints/b2WheelJoint.h" #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2World.h" -#include "Box2D/Common/b2BlockAllocator.h" - -#include -b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) +b2Joint * b2Joint::Create(const b2JointDef * def, b2BlockAllocator * allocator) { - b2Joint* joint = nullptr; - - switch (def->type) - { - case e_distanceJoint: - { - void* mem = allocator->Allocate(sizeof(b2DistanceJoint)); - joint = new (mem) b2DistanceJoint(static_cast(def)); - } - break; - - case e_mouseJoint: - { - void* mem = allocator->Allocate(sizeof(b2MouseJoint)); - joint = new (mem) b2MouseJoint(static_cast(def)); - } - break; - - case e_prismaticJoint: - { - void* mem = allocator->Allocate(sizeof(b2PrismaticJoint)); - joint = new (mem) b2PrismaticJoint(static_cast(def)); - } - break; - - case e_revoluteJoint: - { - void* mem = allocator->Allocate(sizeof(b2RevoluteJoint)); - joint = new (mem) b2RevoluteJoint(static_cast(def)); - } - break; - - case e_pulleyJoint: - { - void* mem = allocator->Allocate(sizeof(b2PulleyJoint)); - joint = new (mem) b2PulleyJoint(static_cast(def)); - } - break; - - case e_gearJoint: - { - void* mem = allocator->Allocate(sizeof(b2GearJoint)); - joint = new (mem) b2GearJoint(static_cast(def)); - } - break; - - case e_wheelJoint: - { - void* mem = allocator->Allocate(sizeof(b2WheelJoint)); - joint = new (mem) b2WheelJoint(static_cast(def)); - } - break; - - case e_weldJoint: - { - void* mem = allocator->Allocate(sizeof(b2WeldJoint)); - joint = new (mem) b2WeldJoint(static_cast(def)); - } - break; - - case e_frictionJoint: - { - void* mem = allocator->Allocate(sizeof(b2FrictionJoint)); - joint = new (mem) b2FrictionJoint(static_cast(def)); - } - break; - - case e_ropeJoint: - { - void* mem = allocator->Allocate(sizeof(b2RopeJoint)); - joint = new (mem) b2RopeJoint(static_cast(def)); - } - break; - - case e_motorJoint: - { - void* mem = allocator->Allocate(sizeof(b2MotorJoint)); - joint = new (mem) b2MotorJoint(static_cast(def)); - } - break; - - default: - b2Assert(false); - break; - } - - return joint; + b2Joint * joint = nullptr; + + switch (def->type) { + case e_distanceJoint: { + void * mem = allocator->Allocate(sizeof(b2DistanceJoint)); + joint = new (mem) b2DistanceJoint(static_cast(def)); + } break; + + case e_mouseJoint: { + void * mem = allocator->Allocate(sizeof(b2MouseJoint)); + joint = new (mem) b2MouseJoint(static_cast(def)); + } break; + + case e_prismaticJoint: { + void * mem = allocator->Allocate(sizeof(b2PrismaticJoint)); + joint = new (mem) b2PrismaticJoint(static_cast(def)); + } break; + + case e_revoluteJoint: { + void * mem = allocator->Allocate(sizeof(b2RevoluteJoint)); + joint = new (mem) b2RevoluteJoint(static_cast(def)); + } break; + + case e_pulleyJoint: { + void * mem = allocator->Allocate(sizeof(b2PulleyJoint)); + joint = new (mem) b2PulleyJoint(static_cast(def)); + } break; + + case e_gearJoint: { + void * mem = allocator->Allocate(sizeof(b2GearJoint)); + joint = new (mem) b2GearJoint(static_cast(def)); + } break; + + case e_wheelJoint: { + void * mem = allocator->Allocate(sizeof(b2WheelJoint)); + joint = new (mem) b2WheelJoint(static_cast(def)); + } break; + + case e_weldJoint: { + void * mem = allocator->Allocate(sizeof(b2WeldJoint)); + joint = new (mem) b2WeldJoint(static_cast(def)); + } break; + + case e_frictionJoint: { + void * mem = allocator->Allocate(sizeof(b2FrictionJoint)); + joint = new (mem) b2FrictionJoint(static_cast(def)); + } break; + + case e_ropeJoint: { + void * mem = allocator->Allocate(sizeof(b2RopeJoint)); + joint = new (mem) b2RopeJoint(static_cast(def)); + } break; + + case e_motorJoint: { + void * mem = allocator->Allocate(sizeof(b2MotorJoint)); + joint = new (mem) b2MotorJoint(static_cast(def)); + } break; + + default: + b2Assert(false); + break; + } + + return joint; } -void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) +void b2Joint::Destroy(b2Joint * joint, b2BlockAllocator * allocator) { - joint->~b2Joint(); - switch (joint->m_type) - { - case e_distanceJoint: - allocator->Free(joint, sizeof(b2DistanceJoint)); - break; - - case e_mouseJoint: - allocator->Free(joint, sizeof(b2MouseJoint)); - break; - - case e_prismaticJoint: - allocator->Free(joint, sizeof(b2PrismaticJoint)); - break; - - case e_revoluteJoint: - allocator->Free(joint, sizeof(b2RevoluteJoint)); - break; - - case e_pulleyJoint: - allocator->Free(joint, sizeof(b2PulleyJoint)); - break; - - case e_gearJoint: - allocator->Free(joint, sizeof(b2GearJoint)); - break; - - case e_wheelJoint: - allocator->Free(joint, sizeof(b2WheelJoint)); - break; - - case e_weldJoint: - allocator->Free(joint, sizeof(b2WeldJoint)); - break; - - case e_frictionJoint: - allocator->Free(joint, sizeof(b2FrictionJoint)); - break; - - case e_ropeJoint: - allocator->Free(joint, sizeof(b2RopeJoint)); - break; - - case e_motorJoint: - allocator->Free(joint, sizeof(b2MotorJoint)); - break; - - default: - b2Assert(false); - break; - } + joint->~b2Joint(); + switch (joint->m_type) { + case e_distanceJoint: + allocator->Free(joint, sizeof(b2DistanceJoint)); + break; + + case e_mouseJoint: + allocator->Free(joint, sizeof(b2MouseJoint)); + break; + + case e_prismaticJoint: + allocator->Free(joint, sizeof(b2PrismaticJoint)); + break; + + case e_revoluteJoint: + allocator->Free(joint, sizeof(b2RevoluteJoint)); + break; + + case e_pulleyJoint: + allocator->Free(joint, sizeof(b2PulleyJoint)); + break; + + case e_gearJoint: + allocator->Free(joint, sizeof(b2GearJoint)); + break; + + case e_wheelJoint: + allocator->Free(joint, sizeof(b2WheelJoint)); + break; + + case e_weldJoint: + allocator->Free(joint, sizeof(b2WeldJoint)); + break; + + case e_frictionJoint: + allocator->Free(joint, sizeof(b2FrictionJoint)); + break; + + case e_ropeJoint: + allocator->Free(joint, sizeof(b2RopeJoint)); + break; + + case e_motorJoint: + allocator->Free(joint, sizeof(b2MotorJoint)); + break; + + default: + b2Assert(false); + break; + } } -b2Joint::b2Joint(const b2JointDef* def) +b2Joint::b2Joint(const b2JointDef * def) { - b2Assert(def->bodyA != def->bodyB); - - m_type = def->type; - m_prev = nullptr; - m_next = nullptr; - m_bodyA = def->bodyA; - m_bodyB = def->bodyB; - m_index = 0; - m_collideConnected = def->collideConnected; - m_islandFlag = false; - m_userData = def->userData; - - m_edgeA.joint = nullptr; - m_edgeA.other = nullptr; - m_edgeA.prev = nullptr; - m_edgeA.next = nullptr; - - m_edgeB.joint = nullptr; - m_edgeB.other = nullptr; - m_edgeB.prev = nullptr; - m_edgeB.next = nullptr; + b2Assert(def->bodyA != def->bodyB); + + m_type = def->type; + m_prev = nullptr; + m_next = nullptr; + m_bodyA = def->bodyA; + m_bodyB = def->bodyB; + m_index = 0; + m_collideConnected = def->collideConnected; + m_islandFlag = false; + m_userData = def->userData; + + m_edgeA.joint = nullptr; + m_edgeA.other = nullptr; + m_edgeA.prev = nullptr; + m_edgeA.next = nullptr; + + m_edgeB.joint = nullptr; + m_edgeB.other = nullptr; + m_edgeB.prev = nullptr; + m_edgeB.next = nullptr; } -bool b2Joint::IsActive() const -{ - return m_bodyA->IsActive() && m_bodyB->IsActive(); -} +bool b2Joint::IsActive() const { return m_bodyA->IsActive() && m_bodyB->IsActive(); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2MotorJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2MotorJoint.cpp index 1fc1277b..1dd3ea6d 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2MotorJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2MotorJoint.cpp @@ -1,22 +1,23 @@ /* -* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2012 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2MotorJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" @@ -32,273 +33,239 @@ // J = [0 0 -1 0 0 1] // K = invI1 + invI2 -void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB) +void b2MotorJointDef::Initialize(b2Body * bA, b2Body * bB) { - bodyA = bA; - bodyB = bB; - b2Vec2 xB = bodyB->GetPosition(); - linearOffset = bodyA->GetLocalPoint(xB); - - float32 angleA = bodyA->GetAngle(); - float32 angleB = bodyB->GetAngle(); - angularOffset = angleB - angleA; + bodyA = bA; + bodyB = bB; + b2Vec2 xB = bodyB->GetPosition(); + linearOffset = bodyA->GetLocalPoint(xB); + + float32 angleA = bodyA->GetAngle(); + float32 angleB = bodyB->GetAngle(); + angularOffset = angleB - angleA; } -b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def) -: b2Joint(def) +b2MotorJoint::b2MotorJoint(const b2MotorJointDef * def) : b2Joint(def) { - m_linearOffset = def->linearOffset; - m_angularOffset = def->angularOffset; + m_linearOffset = def->linearOffset; + m_angularOffset = def->angularOffset; - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; + m_linearImpulse.SetZero(); + m_angularImpulse = 0.0f; - m_maxForce = def->maxForce; - m_maxTorque = def->maxTorque; - m_correctionFactor = def->correctionFactor; + m_maxForce = def->maxForce; + m_maxTorque = def->maxTorque; + m_correctionFactor = def->correctionFactor; } -void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data) +void b2MotorJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective mass matrix. - m_rA = b2Mul(qA, -m_localCenterA); - m_rB = b2Mul(qB, -m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; - K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; - - m_linearMass = K.GetInverse(); - - m_angularMass = iA + iB; - if (m_angularMass > 0.0f) - { - m_angularMass = 1.0f / m_angularMass; - } - - m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset); - m_angularError = aB - aA - m_angularOffset; - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_linearImpulse *= data.step.dtRatio; - m_angularImpulse *= data.step.dtRatio; - - b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); - } - else - { - m_linearImpulse.SetZero(); - m_angularImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + // Compute the effective mass matrix. + m_rA = b2Mul(qA, -m_localCenterA); + m_rB = b2Mul(qB, -m_localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] [ + // -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Mat22 K; + K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; + K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; + K.ey.x = K.ex.y; + K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; + + m_linearMass = K.GetInverse(); + + m_angularMass = iA + iB; + if (m_angularMass > 0.0f) { + m_angularMass = 1.0f / m_angularMass; + } + + m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset); + m_angularError = aB - aA - m_angularOffset; + + if (data.step.warmStarting) { + // Scale impulses to support a variable time step. + m_linearImpulse *= data.step.dtRatio; + m_angularImpulse *= data.step.dtRatio; + + b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y); + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse); + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_angularImpulse); + } else { + m_linearImpulse.SetZero(); + m_angularImpulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2MotorJoint::SolveVelocityConstraints(const b2SolverData & data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - float32 h = data.step.dt; - float32 inv_h = data.step.inv_dt; + float32 h = data.step.dt; + float32 inv_h = data.step.inv_dt; - // Solve angular friction - { - float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; - float32 impulse = -m_angularMass * Cdot; + // Solve angular friction + { + float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError; + float32 impulse = -m_angularMass * Cdot; - float32 oldImpulse = m_angularImpulse; - float32 maxImpulse = h * m_maxTorque; - m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_angularImpulse - oldImpulse; + float32 oldImpulse = m_angularImpulse; + float32 maxImpulse = h * m_maxTorque; + m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_angularImpulse - oldImpulse; - wA -= iA * impulse; - wB += iB * impulse; - } + wA -= iA * impulse; + wB += iB * impulse; + } - // Solve linear friction - { - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError; + // Solve linear friction + { + b2Vec2 Cdot = + vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError; - b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); - b2Vec2 oldImpulse = m_linearImpulse; - m_linearImpulse += impulse; + b2Vec2 impulse = -b2Mul(m_linearMass, Cdot); + b2Vec2 oldImpulse = m_linearImpulse; + m_linearImpulse += impulse; - float32 maxImpulse = h * m_maxForce; + float32 maxImpulse = h * m_maxForce; - if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_linearImpulse.Normalize(); - m_linearImpulse *= maxImpulse; - } + if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { + m_linearImpulse.Normalize(); + m_linearImpulse *= maxImpulse; + } - impulse = m_linearImpulse - oldImpulse; + impulse = m_linearImpulse - oldImpulse; - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); + vA -= mA * impulse; + wA -= iA * b2Cross(m_rA, impulse); - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } + vB += mB * impulse; + wB += iB * b2Cross(m_rB, impulse); + } - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data) +bool b2MotorJoint::SolvePositionConstraints(const b2SolverData & data) { - B2_NOT_USED(data); + B2_NOT_USED(data); - return true; + return true; } -b2Vec2 b2MotorJoint::GetAnchorA() const -{ - return m_bodyA->GetPosition(); -} +b2Vec2 b2MotorJoint::GetAnchorA() const { return m_bodyA->GetPosition(); } -b2Vec2 b2MotorJoint::GetAnchorB() const -{ - return m_bodyB->GetPosition(); -} +b2Vec2 b2MotorJoint::GetAnchorB() const { return m_bodyB->GetPosition(); } -b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * m_linearImpulse; -} +b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const { return inv_dt * m_linearImpulse; } -float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_angularImpulse; -} +float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const { return inv_dt * m_angularImpulse; } void b2MotorJoint::SetMaxForce(float32 force) { - b2Assert(b2IsValid(force) && force >= 0.0f); - m_maxForce = force; + b2Assert(b2IsValid(force) && force >= 0.0f); + m_maxForce = force; } -float32 b2MotorJoint::GetMaxForce() const -{ - return m_maxForce; -} +float32 b2MotorJoint::GetMaxForce() const { return m_maxForce; } void b2MotorJoint::SetMaxTorque(float32 torque) { - b2Assert(b2IsValid(torque) && torque >= 0.0f); - m_maxTorque = torque; + b2Assert(b2IsValid(torque) && torque >= 0.0f); + m_maxTorque = torque; } -float32 b2MotorJoint::GetMaxTorque() const -{ - return m_maxTorque; -} +float32 b2MotorJoint::GetMaxTorque() const { return m_maxTorque; } void b2MotorJoint::SetCorrectionFactor(float32 factor) { - b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f); - m_correctionFactor = factor; + b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f); + m_correctionFactor = factor; } -float32 b2MotorJoint::GetCorrectionFactor() const -{ - return m_correctionFactor; -} +float32 b2MotorJoint::GetCorrectionFactor() const { return m_correctionFactor; } -void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset) +void b2MotorJoint::SetLinearOffset(const b2Vec2 & linearOffset) { - if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_linearOffset = linearOffset; - } + if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_linearOffset = linearOffset; + } } -const b2Vec2& b2MotorJoint::GetLinearOffset() const -{ - return m_linearOffset; -} +const b2Vec2 & b2MotorJoint::GetLinearOffset() const { return m_linearOffset; } void b2MotorJoint::SetAngularOffset(float32 angularOffset) { - if (angularOffset != m_angularOffset) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_angularOffset = angularOffset; - } + if (angularOffset != m_angularOffset) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_angularOffset = angularOffset; + } } -float32 b2MotorJoint::GetAngularOffset() const -{ - return m_angularOffset; -} +float32 b2MotorJoint::GetAngularOffset() const { return m_angularOffset; } void b2MotorJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2MotorJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y); - b2Log(" jd.angularOffset = %.15lef;\n", m_angularOffset); - b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); - b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); - b2Log(" jd.correctionFactor = %.15lef;\n", m_correctionFactor); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2MotorJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y); + b2Log(" jd.angularOffset = %.15lef;\n", m_angularOffset); + b2Log(" jd.maxForce = %.15lef;\n", m_maxForce); + b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque); + b2Log(" jd.correctionFactor = %.15lef;\n", m_correctionFactor); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2MouseJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2MouseJoint.cpp index 37691ac8..9fab75c0 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2MouseJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2MouseJoint.cpp @@ -1,22 +1,23 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2MouseJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" @@ -28,195 +29,155 @@ // Identity used: // w k % (rx i + ry j) = w * (-ry i + rx j) -b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def) -: b2Joint(def) +b2MouseJoint::b2MouseJoint(const b2MouseJointDef * def) : b2Joint(def) { - b2Assert(def->target.IsValid()); - b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f); - b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f); - b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f); + b2Assert(def->target.IsValid()); + b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f); + b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f); + b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f); - m_targetA = def->target; - m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA); + m_targetA = def->target; + m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA); - m_maxForce = def->maxForce; - m_impulse.SetZero(); + m_maxForce = def->maxForce; + m_impulse.SetZero(); - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; + m_frequencyHz = def->frequencyHz; + m_dampingRatio = def->dampingRatio; - m_beta = 0.0f; - m_gamma = 0.0f; + m_beta = 0.0f; + m_gamma = 0.0f; } -void b2MouseJoint::SetTarget(const b2Vec2& target) +void b2MouseJoint::SetTarget(const b2Vec2 & target) { - if (target != m_targetA) - { - m_bodyB->SetAwake(true); - m_targetA = target; - } + if (target != m_targetA) { + m_bodyB->SetAwake(true); + m_targetA = target; + } } -const b2Vec2& b2MouseJoint::GetTarget() const -{ - return m_targetA; -} +const b2Vec2 & b2MouseJoint::GetTarget() const { return m_targetA; } -void b2MouseJoint::SetMaxForce(float32 force) -{ - m_maxForce = force; -} +void b2MouseJoint::SetMaxForce(float32 force) { m_maxForce = force; } -float32 b2MouseJoint::GetMaxForce() const -{ - return m_maxForce; -} +float32 b2MouseJoint::GetMaxForce() const { return m_maxForce; } -void b2MouseJoint::SetFrequency(float32 hz) -{ - m_frequencyHz = hz; -} +void b2MouseJoint::SetFrequency(float32 hz) { m_frequencyHz = hz; } -float32 b2MouseJoint::GetFrequency() const -{ - return m_frequencyHz; -} +float32 b2MouseJoint::GetFrequency() const { return m_frequencyHz; } -void b2MouseJoint::SetDampingRatio(float32 ratio) -{ - m_dampingRatio = ratio; -} +void b2MouseJoint::SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; } -float32 b2MouseJoint::GetDampingRatio() const -{ - return m_dampingRatio; -} +float32 b2MouseJoint::GetDampingRatio() const { return m_dampingRatio; } -void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data) +void b2MouseJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexB = m_bodyB->m_islandIndex; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassB = m_bodyB->m_invMass; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qB(aB); - - float32 mass = m_bodyB->GetMass(); - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * mass * m_dampingRatio * omega; - - // Spring stiffness - float32 k = mass * (omega * omega); - - // magic formulas - // gamma has units of inverse mass. - // beta has units of inverse time. - float32 h = data.step.dt; - b2Assert(d + h * k > b2_epsilon); - m_gamma = h * (d + h * k); - if (m_gamma != 0.0f) - { - m_gamma = 1.0f / m_gamma; - } - m_beta = h * k * m_gamma; - - // Compute the effective mass matrix. - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] - // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] - b2Mat22 K; - K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma; - K.ex.y = -m_invIB * m_rB.x * m_rB.y; - K.ey.x = K.ex.y; - K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma; - - m_mass = K.GetInverse(); - - m_C = cB + m_rB - m_targetA; - m_C *= m_beta; - - // Cheat with some damping - wB *= 0.98f; - - if (data.step.warmStarting) - { - m_impulse *= data.step.dtRatio; - vB += m_invMassB * m_impulse; - wB += m_invIB * b2Cross(m_rB, m_impulse); - } - else - { - m_impulse.SetZero(); - } - - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} + m_indexB = m_bodyB->m_islandIndex; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassB = m_bodyB->m_invMass; + m_invIB = m_bodyB->m_invI; -void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data) -{ - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Cdot = v + cross(w, r) - b2Vec2 Cdot = vB + b2Cross(wB, m_rB); - b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)); - - b2Vec2 oldImpulse = m_impulse; - m_impulse += impulse; - float32 maxImpulse = data.step.dt * m_maxForce; - if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) - { - m_impulse *= maxImpulse / m_impulse.Length(); - } - impulse = m_impulse - oldImpulse; - - vB += m_invMassB * impulse; - wB += m_invIB * b2Cross(m_rB, impulse); - - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; -} + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; -bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data) -{ - B2_NOT_USED(data); - return true; -} + b2Rot qB(aB); -b2Vec2 b2MouseJoint::GetAnchorA() const -{ - return m_targetA; -} + float32 mass = m_bodyB->GetMass(); -b2Vec2 b2MouseJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} + // Frequency + float32 omega = 2.0f * b2_pi * m_frequencyHz; -b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const -{ - return inv_dt * m_impulse; + // Damping coefficient + float32 d = 2.0f * mass * m_dampingRatio * omega; + + // Spring stiffness + float32 k = mass * (omega * omega); + + // magic formulas + // gamma has units of inverse mass. + // beta has units of inverse time. + float32 h = data.step.dt; + b2Assert(d + h * k > b2_epsilon); + m_gamma = h * (d + h * k); + if (m_gamma != 0.0f) { + m_gamma = 1.0f / m_gamma; + } + m_beta = h * k * m_gamma; + + // Compute the effective mass matrix. + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * + // invI2 * skew(r2)] + // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * + // [r1.y*r1.y -r1.x*r1.y] + // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y + // r1.x*r1.x] + b2Mat22 K; + K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma; + K.ex.y = -m_invIB * m_rB.x * m_rB.y; + K.ey.x = K.ex.y; + K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma; + + m_mass = K.GetInverse(); + + m_C = cB + m_rB - m_targetA; + m_C *= m_beta; + + // Cheat with some damping + wB *= 0.98f; + + if (data.step.warmStarting) { + m_impulse *= data.step.dtRatio; + vB += m_invMassB * m_impulse; + wB += m_invIB * b2Cross(m_rB, m_impulse); + } else { + m_impulse.SetZero(); + } + + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const +void b2MouseJoint::SolveVelocityConstraints(const b2SolverData & data) { - return inv_dt * 0.0f; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + // Cdot = v + cross(w, r) + b2Vec2 Cdot = vB + b2Cross(wB, m_rB); + b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)); + + b2Vec2 oldImpulse = m_impulse; + m_impulse += impulse; + float32 maxImpulse = data.step.dt * m_maxForce; + if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) { + m_impulse *= maxImpulse / m_impulse.Length(); + } + impulse = m_impulse - oldImpulse; + + vB += m_invMassB * impulse; + wB += m_invIB * b2Cross(m_rB, impulse); + + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin) +bool b2MouseJoint::SolvePositionConstraints(const b2SolverData & data) { - m_targetA -= newOrigin; + B2_NOT_USED(data); + return true; } + +b2Vec2 b2MouseJoint::GetAnchorA() const { return m_targetA; } + +b2Vec2 b2MouseJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } + +b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const { return inv_dt * m_impulse; } + +float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const { return inv_dt * 0.0f; } + +void b2MouseJoint::ShiftOrigin(const b2Vec2 & newOrigin) { m_targetA -= newOrigin; } diff --git a/flatland_box2d/src/Dynamics/Joints/b2PrismaticJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2PrismaticJoint.cpp index da704d19..aad0d313 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2PrismaticJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2PrismaticJoint.cpp @@ -1,30 +1,33 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2PrismaticJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" // Linear constraint (point-to-line) // d = p2 - p1 = x2 + r2 - x1 - r1 // C = dot(perp, d) -// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) -// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) +// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - +// cross(w1, r1)) +// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + +// dot(cross(r2, perp), v2) // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] // // Angular constraint @@ -40,15 +43,15 @@ // s1 = cross(d + r1, a) = cross(p2 - x1, a) // s2 = cross(r2, a) = cross(p2 - x2, a) - // Motor/Limit linear constraint // C = dot(ax1, d) -// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) -// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] +// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + +// dot(cross(r2, ax1), v2) J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] // Block Solver -// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even -// when the mass has poor distribution (leading to large torques about the joint anchor points). +// We develop a block solver that includes the joint limit. This makes the limit +// stiff (inelastic) even when the mass has poor distribution (leading to large +// torques about the joint anchor points). // // The Jacobian has 3 rows: // J = [-uT -s1 uT s2] // linear @@ -80,563 +83,505 @@ // // Solve for correct f2(1:2) // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1 -// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3) -// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2) -// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) +// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) +// + K(1:2,3) * f1(3) +// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) +// * f1(1:2) f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) +// + f1(1:2) // // Now compute impulse to be applied: // df = f2 - f1 -void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) +void b2PrismaticJointDef::Initialize( + b2Body * bA, b2Body * bB, const b2Vec2 & anchor, const b2Vec2 & axis) { - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); + localAxisA = bodyA->GetLocalVector(axis); + referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); } -b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def) -: b2Joint(def) +b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef * def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_localXAxisA = def->localAxisA; - m_localXAxisA.Normalize(); - m_localYAxisA = b2Cross(1.0f, m_localXAxisA); - m_referenceAngle = def->referenceAngle; - - m_impulse.SetZero(); - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - - m_lowerTranslation = def->lowerTranslation; - m_upperTranslation = def->upperTranslation; - m_maxMotorForce = def->maxMotorForce; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - m_limitState = e_inactiveLimit; - - m_axis.SetZero(); - m_perp.SetZero(); + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_localXAxisA = def->localAxisA; + m_localXAxisA.Normalize(); + m_localYAxisA = b2Cross(1.0f, m_localXAxisA); + m_referenceAngle = def->referenceAngle; + + m_impulse.SetZero(); + m_motorMass = 0.0f; + m_motorImpulse = 0.0f; + + m_lowerTranslation = def->lowerTranslation; + m_upperTranslation = def->upperTranslation; + m_maxMotorForce = def->maxMotorForce; + m_motorSpeed = def->motorSpeed; + m_enableLimit = def->enableLimit; + m_enableMotor = def->enableMotor; + m_limitState = e_inactiveLimit; + + m_axis.SetZero(); + m_perp.SetZero(); } -void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data) +void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective masses. - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = (cB - cA) + rB - rA; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Compute motor Jacobian and effective mass. - { - m_axis = b2Mul(qA, m_localXAxisA); - m_a1 = b2Cross(d + rA, m_axis); - m_a2 = b2Cross(rB, m_axis); - - m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - } - - // Prismatic constraint. - { - m_perp = b2Mul(qA, m_localYAxisA); - - m_s1 = b2Cross(d + rA, m_perp); - m_s2 = b2Cross(rB, m_perp); - - float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; - float32 k12 = iA * m_s1 + iB * m_s2; - float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - // For bodies with fixed rotation. - k22 = 1.0f; - } - float32 k23 = iA * m_a1 + iB * m_a2; - float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; - - m_K.ex.Set(k11, k12, k13); - m_K.ey.Set(k12, k22, k23); - m_K.ez.Set(k13, k23, k33); - } - - // Compute motor and limit terms. - if (m_enableLimit) - { - float32 jointTranslation = b2Dot(m_axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - m_limitState = e_equalLimits; - } - else if (jointTranslation <= m_lowerTranslation) - { - if (m_limitState != e_atLowerLimit) - { - m_limitState = e_atLowerLimit; - m_impulse.z = 0.0f; - } - } - else if (jointTranslation >= m_upperTranslation) - { - if (m_limitState != e_atUpperLimit) - { - m_limitState = e_atUpperLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - - if (m_enableMotor == false) - { - m_motorImpulse = 0.0f; - } - - if (data.step.warmStarting) - { - // Account for variable time step. - m_impulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis; - float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; - float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + // Compute the effective masses. + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = (cB - cA) + rB - rA; + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + // Compute motor Jacobian and effective mass. + { + m_axis = b2Mul(qA, m_localXAxisA); + m_a1 = b2Cross(d + rA, m_axis); + m_a2 = b2Cross(rB, m_axis); + + m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; + if (m_motorMass > 0.0f) { + m_motorMass = 1.0f / m_motorMass; + } + } + + // Prismatic constraint. + { + m_perp = b2Mul(qA, m_localYAxisA); + + m_s1 = b2Cross(d + rA, m_perp); + m_s2 = b2Cross(rB, m_perp); + + float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2; + float32 k12 = iA * m_s1 + iB * m_s2; + float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2; + float32 k22 = iA + iB; + if (k22 == 0.0f) { + // For bodies with fixed rotation. + k22 = 1.0f; + } + float32 k23 = iA * m_a1 + iB * m_a2; + float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2; + + m_K.ex.Set(k11, k12, k13); + m_K.ey.Set(k12, k22, k23); + m_K.ez.Set(k13, k23, k33); + } + + // Compute motor and limit terms. + if (m_enableLimit) { + float32 jointTranslation = b2Dot(m_axis, d); + if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) { + m_limitState = e_equalLimits; + } else if (jointTranslation <= m_lowerTranslation) { + if (m_limitState != e_atLowerLimit) { + m_limitState = e_atLowerLimit; + m_impulse.z = 0.0f; + } + } else if (jointTranslation >= m_upperTranslation) { + if (m_limitState != e_atUpperLimit) { + m_limitState = e_atUpperLimit; + m_impulse.z = 0.0f; + } + } else { + m_limitState = e_inactiveLimit; + m_impulse.z = 0.0f; + } + } else { + m_limitState = e_inactiveLimit; + m_impulse.z = 0.0f; + } + + if (m_enableMotor == false) { + m_motorImpulse = 0.0f; + } + + if (data.step.warmStarting) { + // Account for variable time step. + m_impulse *= data.step.dtRatio; + m_motorImpulse *= data.step.dtRatio; + + b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis; + float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1; + float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } else { + m_impulse.SetZero(); + m_motorImpulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData & data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Solve linear motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits) - { - float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; - float32 impulse = m_motorMass * (m_motorSpeed - Cdot); - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorForce; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - b2Vec2 P = impulse * m_axis; - float32 LA = impulse * m_a1; - float32 LB = impulse * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - b2Vec2 Cdot1; - Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; - Cdot1.y = wB - wA; - - if (m_enableLimit && m_limitState != e_inactiveLimit) - { - // Solve prismatic and limit constraint in block form. - float32 Cdot2; - Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - - b2Vec3 f1 = m_impulse; - b2Vec3 df = m_K.Solve33(-Cdot); - m_impulse += df; - - if (m_limitState == e_atLowerLimit) - { - m_impulse.z = b2Max(m_impulse.z, 0.0f); - } - else if (m_limitState == e_atUpperLimit) - { - m_impulse.z = b2Min(m_impulse.z, 0.0f); - } - - // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) - b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y); - b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y); - m_impulse.x = f2r.x; - m_impulse.y = f2r.y; - - df = m_impulse - f1; - - b2Vec2 P = df.x * m_perp + df.z * m_axis; - float32 LA = df.x * m_s1 + df.y + df.z * m_a1; - float32 LB = df.x * m_s2 + df.y + df.z * m_a2; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - else - { - // Limit is inactive, just solve the prismatic constraint in block form. - b2Vec2 df = m_K.Solve22(-Cdot1); - m_impulse.x += df.x; - m_impulse.y += df.y; - - b2Vec2 P = df.x * m_perp; - float32 LA = df.x * m_s1 + df.y; - float32 LB = df.x * m_s2 + df.y; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + // Solve linear motor constraint. + if (m_enableMotor && m_limitState != e_equalLimits) { + float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; + float32 impulse = m_motorMass * (m_motorSpeed - Cdot); + float32 oldImpulse = m_motorImpulse; + float32 maxImpulse = data.step.dt * m_maxMotorForce; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + b2Vec2 P = impulse * m_axis; + float32 LA = impulse * m_a1; + float32 LB = impulse * m_a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + b2Vec2 Cdot1; + Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA; + Cdot1.y = wB - wA; + + if (m_enableLimit && m_limitState != e_inactiveLimit) { + // Solve prismatic and limit constraint in block form. + float32 Cdot2; + Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA; + b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); + + b2Vec3 f1 = m_impulse; + b2Vec3 df = m_K.Solve33(-Cdot); + m_impulse += df; + + if (m_limitState == e_atLowerLimit) { + m_impulse.z = b2Max(m_impulse.z, 0.0f); + } else if (m_limitState == e_atUpperLimit) { + m_impulse.z = b2Min(m_impulse.z, 0.0f); + } + + // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + + // f1(1:2) + b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y); + b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y); + m_impulse.x = f2r.x; + m_impulse.y = f2r.y; + + df = m_impulse - f1; + + b2Vec2 P = df.x * m_perp + df.z * m_axis; + float32 LA = df.x * m_s1 + df.y + df.z * m_a1; + float32 LB = df.x * m_s2 + df.y + df.z * m_a2; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } else { + // Limit is inactive, just solve the prismatic constraint in block form. + b2Vec2 df = m_K.Solve22(-Cdot1); + m_impulse.x += df.x; + m_impulse.y += df.y; + + b2Vec2 P = df.x * m_perp; + float32 LA = df.x * m_s1 + df.y; + float32 LB = df.x * m_s2 + df.y; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -// A velocity based solver computes reaction forces(impulses) using the velocity constraint solver.Under this context, -// the position solver is not there to resolve forces.It is only there to cope with integration error. +// A velocity based solver computes reaction forces(impulses) using the velocity +// constraint solver.Under this context, the position solver is not there to +// resolve forces.It is only there to cope with integration error. // -// Therefore, the pseudo impulses in the position solver do not have any physical meaning.Thus it is okay if they suck. +// Therefore, the pseudo impulses in the position solver do not have any +// physical meaning.Thus it is okay if they suck. // -// We could take the active state from the velocity solver.However, the joint might push past the limit when the velocity -// solver indicates the limit is inactive. -bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data) +// We could take the active state from the velocity solver.However, the joint +// might push past the limit when the velocity solver indicates the limit is +// inactive. +bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData & data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - // Compute fresh Jacobians - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = cB + rB - cA - rA; - - b2Vec2 axis = b2Mul(qA, m_localXAxisA); - float32 a1 = b2Cross(d + rA, axis); - float32 a2 = b2Cross(rB, axis); - b2Vec2 perp = b2Mul(qA, m_localYAxisA); - - float32 s1 = b2Cross(d + rA, perp); - float32 s2 = b2Cross(rB, perp); - - b2Vec3 impulse; - b2Vec2 C1; - C1.x = b2Dot(perp, d); - C1.y = aB - aA - m_referenceAngle; - - float32 linearError = b2Abs(C1.x); - float32 angularError = b2Abs(C1.y); - - bool active = false; - float32 C2 = 0.0f; - if (m_enableLimit) - { - float32 translation = b2Dot(axis, d); - if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) - { - // Prevent large angular corrections - C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); - linearError = b2Max(linearError, b2Abs(translation)); - active = true; - } - else if (translation <= m_lowerTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); - linearError = b2Max(linearError, m_lowerTranslation - translation); - active = true; - } - else if (translation >= m_upperTranslation) - { - // Prevent large linear corrections and allow some slop. - C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); - linearError = b2Max(linearError, translation - m_upperTranslation); - active = true; - } - } - - if (active) - { - float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; - float32 k12 = iA * s1 + iB * s2; - float32 k13 = iA * s1 * a1 + iB * s2 * a2; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - // For fixed rotation - k22 = 1.0f; - } - float32 k23 = iA * a1 + iB * a2; - float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; - - b2Mat33 K; - K.ex.Set(k11, k12, k13); - K.ey.Set(k12, k22, k23); - K.ez.Set(k13, k23, k33); - - b2Vec3 C; - C.x = C1.x; - C.y = C1.y; - C.z = C2; - - impulse = K.Solve33(-C); - } - else - { - float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; - float32 k12 = iA * s1 + iB * s2; - float32 k22 = iA + iB; - if (k22 == 0.0f) - { - k22 = 1.0f; - } - - b2Mat22 K; - K.ex.Set(k11, k12); - K.ey.Set(k12, k22); - - b2Vec2 impulse1 = K.Solve(-C1); - impulse.x = impulse1.x; - impulse.y = impulse1.y; - impulse.z = 0.0f; - } - - b2Vec2 P = impulse.x * perp + impulse.z * axis; - float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1; - float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2; - - cA -= mA * P; - aA -= iA * LA; - cB += mB * P; - aB += iB * LB; - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return linearError <= b2_linearSlop && angularError <= b2_angularSlop; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + + b2Rot qA(aA), qB(aB); + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + // Compute fresh Jacobians + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = cB + rB - cA - rA; + + b2Vec2 axis = b2Mul(qA, m_localXAxisA); + float32 a1 = b2Cross(d + rA, axis); + float32 a2 = b2Cross(rB, axis); + b2Vec2 perp = b2Mul(qA, m_localYAxisA); + + float32 s1 = b2Cross(d + rA, perp); + float32 s2 = b2Cross(rB, perp); + + b2Vec3 impulse; + b2Vec2 C1; + C1.x = b2Dot(perp, d); + C1.y = aB - aA - m_referenceAngle; + + float32 linearError = b2Abs(C1.x); + float32 angularError = b2Abs(C1.y); + + bool active = false; + float32 C2 = 0.0f; + if (m_enableLimit) { + float32 translation = b2Dot(axis, d); + if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop) { + // Prevent large angular corrections + C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection); + linearError = b2Max(linearError, b2Abs(translation)); + active = true; + } else if (translation <= m_lowerTranslation) { + // Prevent large linear corrections and allow some slop. + C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f); + linearError = b2Max(linearError, m_lowerTranslation - translation); + active = true; + } else if (translation >= m_upperTranslation) { + // Prevent large linear corrections and allow some slop. + C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection); + linearError = b2Max(linearError, translation - m_upperTranslation); + active = true; + } + } + + if (active) { + float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float32 k12 = iA * s1 + iB * s2; + float32 k13 = iA * s1 * a1 + iB * s2 * a2; + float32 k22 = iA + iB; + if (k22 == 0.0f) { + // For fixed rotation + k22 = 1.0f; + } + float32 k23 = iA * a1 + iB * a2; + float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; + + b2Mat33 K; + K.ex.Set(k11, k12, k13); + K.ey.Set(k12, k22, k23); + K.ez.Set(k13, k23, k33); + + b2Vec3 C; + C.x = C1.x; + C.y = C1.y; + C.z = C2; + + impulse = K.Solve33(-C); + } else { + float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float32 k12 = iA * s1 + iB * s2; + float32 k22 = iA + iB; + if (k22 == 0.0f) { + k22 = 1.0f; + } + + b2Mat22 K; + K.ex.Set(k11, k12); + K.ey.Set(k12, k22); + + b2Vec2 impulse1 = K.Solve(-C1); + impulse.x = impulse1.x; + impulse.y = impulse1.y; + impulse.z = 0.0f; + } + + b2Vec2 P = impulse.x * perp + impulse.z * axis; + float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1; + float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2; + + cA -= mA * P; + aA -= iA * LA; + cB += mB * P; + aB += iB * LB; + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + + return linearError <= b2_linearSlop && angularError <= b2_angularSlop; } -b2Vec2 b2PrismaticJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} +b2Vec2 b2PrismaticJoint::GetAnchorA() const { return m_bodyA->GetWorldPoint(m_localAnchorA); } -b2Vec2 b2PrismaticJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} +b2Vec2 b2PrismaticJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const { - return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis); + return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis); } -float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_impulse.y; -} +float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const { return inv_dt * m_impulse.y; } float32 b2PrismaticJoint::GetJointTranslation() const { - b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); - b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); - b2Vec2 d = pB - pA; - b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA); + b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA); + b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB); + b2Vec2 d = pB - pA; + b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA); - float32 translation = b2Dot(d, axis); - return translation; + float32 translation = b2Dot(d, axis); + return translation; } float32 b2PrismaticJoint::GetJointSpeed() const { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - - b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); - b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); - b2Vec2 p1 = bA->m_sweep.c + rA; - b2Vec2 p2 = bB->m_sweep.c + rB; - b2Vec2 d = p2 - p1; - b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); - - b2Vec2 vA = bA->m_linearVelocity; - b2Vec2 vB = bB->m_linearVelocity; - float32 wA = bA->m_angularVelocity; - float32 wB = bB->m_angularVelocity; - - float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); - return speed; + b2Body * bA = m_bodyA; + b2Body * bB = m_bodyB; + + b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); + b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); + b2Vec2 p1 = bA->m_sweep.c + rA; + b2Vec2 p2 = bB->m_sweep.c + rB; + b2Vec2 d = p2 - p1; + b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); + + b2Vec2 vA = bA->m_linearVelocity; + b2Vec2 vB = bB->m_linearVelocity; + float32 wA = bA->m_angularVelocity; + float32 wB = bB->m_angularVelocity; + + float32 speed = + b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); + return speed; } -bool b2PrismaticJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} +bool b2PrismaticJoint::IsLimitEnabled() const { return m_enableLimit; } void b2PrismaticJoint::EnableLimit(bool flag) { - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_impulse.z = 0.0f; - } + if (flag != m_enableLimit) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableLimit = flag; + m_impulse.z = 0.0f; + } } -float32 b2PrismaticJoint::GetLowerLimit() const -{ - return m_lowerTranslation; -} +float32 b2PrismaticJoint::GetLowerLimit() const { return m_lowerTranslation; } -float32 b2PrismaticJoint::GetUpperLimit() const -{ - return m_upperTranslation; -} +float32 b2PrismaticJoint::GetUpperLimit() const { return m_upperTranslation; } void b2PrismaticJoint::SetLimits(float32 lower, float32 upper) { - b2Assert(lower <= upper); - if (lower != m_lowerTranslation || upper != m_upperTranslation) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_lowerTranslation = lower; - m_upperTranslation = upper; - m_impulse.z = 0.0f; - } + b2Assert(lower <= upper); + if (lower != m_lowerTranslation || upper != m_upperTranslation) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_lowerTranslation = lower; + m_upperTranslation = upper; + m_impulse.z = 0.0f; + } } -bool b2PrismaticJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} +bool b2PrismaticJoint::IsMotorEnabled() const { return m_enableMotor; } void b2PrismaticJoint::EnableMotor(bool flag) { - if (flag != m_enableMotor) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; - } + if (flag != m_enableMotor) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableMotor = flag; + } } void b2PrismaticJoint::SetMotorSpeed(float32 speed) { - if (speed != m_motorSpeed) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; - } + if (speed != m_motorSpeed) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; + } } void b2PrismaticJoint::SetMaxMotorForce(float32 force) { - if (force != m_maxMotorForce) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorForce = force; - } + if (force != m_maxMotorForce) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_maxMotorForce = force; + } } -float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} +float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const { return inv_dt * m_motorImpulse; } void b2PrismaticJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2PrismaticJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); - b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); - b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); - b2Log(" jd.lowerTranslation = %.15lef;\n", m_lowerTranslation); - b2Log(" jd.upperTranslation = %.15lef;\n", m_upperTranslation); - b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); - b2Log(" jd.maxMotorForce = %.15lef;\n", m_maxMotorForce); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2PrismaticJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); + b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); + b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); + b2Log(" jd.lowerTranslation = %.15lef;\n", m_lowerTranslation); + b2Log(" jd.upperTranslation = %.15lef;\n", m_upperTranslation); + b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); + b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); + b2Log(" jd.maxMotorForce = %.15lef;\n", m_maxMotorForce); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2PulleyJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2PulleyJoint.cpp index 7e5d54bb..0f36f918 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2PulleyJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2PulleyJoint.cpp @@ -1,22 +1,23 @@ /* -* Copyright (c) 2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2007 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2PulleyJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" @@ -30,319 +31,280 @@ // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2)) // J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)] // K = J * invM * JT -// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2) +// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * +// cross(r2, u2)^2) -void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB, - const b2Vec2& groundA, const b2Vec2& groundB, - const b2Vec2& anchorA, const b2Vec2& anchorB, - float32 r) +void b2PulleyJointDef::Initialize( + b2Body * bA, b2Body * bB, const b2Vec2 & groundA, const b2Vec2 & groundB, const b2Vec2 & anchorA, + const b2Vec2 & anchorB, float32 r) { - bodyA = bA; - bodyB = bB; - groundAnchorA = groundA; - groundAnchorB = groundB; - localAnchorA = bodyA->GetLocalPoint(anchorA); - localAnchorB = bodyB->GetLocalPoint(anchorB); - b2Vec2 dA = anchorA - groundA; - lengthA = dA.Length(); - b2Vec2 dB = anchorB - groundB; - lengthB = dB.Length(); - ratio = r; - b2Assert(ratio > b2_epsilon); + bodyA = bA; + bodyB = bB; + groundAnchorA = groundA; + groundAnchorB = groundB; + localAnchorA = bodyA->GetLocalPoint(anchorA); + localAnchorB = bodyB->GetLocalPoint(anchorB); + b2Vec2 dA = anchorA - groundA; + lengthA = dA.Length(); + b2Vec2 dB = anchorB - groundB; + lengthB = dB.Length(); + ratio = r; + b2Assert(ratio > b2_epsilon); } -b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def) -: b2Joint(def) +b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef * def) : b2Joint(def) { - m_groundAnchorA = def->groundAnchorA; - m_groundAnchorB = def->groundAnchorB; - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; + m_groundAnchorA = def->groundAnchorA; + m_groundAnchorB = def->groundAnchorB; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; - m_lengthA = def->lengthA; - m_lengthB = def->lengthB; + m_lengthA = def->lengthA; + m_lengthB = def->lengthB; - b2Assert(def->ratio != 0.0f); - m_ratio = def->ratio; + b2Assert(def->ratio != 0.0f); + m_ratio = def->ratio; - m_constant = def->lengthA + m_ratio * def->lengthB; + m_constant = def->lengthA + m_ratio * def->lengthB; - m_impulse = 0.0f; + m_impulse = 0.0f; } -void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data) +void b2PulleyJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // Get the pulley axes. - m_uA = cA + m_rA - m_groundAnchorA; - m_uB = cB + m_rB - m_groundAnchorB; - - float32 lengthA = m_uA.Length(); - float32 lengthB = m_uB.Length(); - - if (lengthA > 10.0f * b2_linearSlop) - { - m_uA *= 1.0f / lengthA; - } - else - { - m_uA.SetZero(); - } - - if (lengthB > 10.0f * b2_linearSlop) - { - m_uB *= 1.0f / lengthB; - } - else - { - m_uB.SetZero(); - } - - // Compute effective mass. - float32 ruA = b2Cross(m_rA, m_uA); - float32 ruB = b2Cross(m_rB, m_uB); - - float32 mA = m_invMassA + m_invIA * ruA * ruA; - float32 mB = m_invMassB + m_invIB * ruB * ruB; - - m_mass = mA + m_ratio * m_ratio * mB; - - if (m_mass > 0.0f) - { - m_mass = 1.0f / m_mass; - } - - if (data.step.warmStarting) - { - // Scale impulses to support variable time steps. - m_impulse *= data.step.dtRatio; - - // Warm starting. - b2Vec2 PA = -(m_impulse) * m_uA; - b2Vec2 PB = (-m_ratio * m_impulse) * m_uB; - - vA += m_invMassA * PA; - wA += m_invIA * b2Cross(m_rA, PA); - vB += m_invMassB * PB; - wB += m_invIB * b2Cross(m_rB, PB); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + // Get the pulley axes. + m_uA = cA + m_rA - m_groundAnchorA; + m_uB = cB + m_rB - m_groundAnchorB; + + float32 lengthA = m_uA.Length(); + float32 lengthB = m_uB.Length(); + + if (lengthA > 10.0f * b2_linearSlop) { + m_uA *= 1.0f / lengthA; + } else { + m_uA.SetZero(); + } + + if (lengthB > 10.0f * b2_linearSlop) { + m_uB *= 1.0f / lengthB; + } else { + m_uB.SetZero(); + } + + // Compute effective mass. + float32 ruA = b2Cross(m_rA, m_uA); + float32 ruB = b2Cross(m_rB, m_uB); + + float32 mA = m_invMassA + m_invIA * ruA * ruA; + float32 mB = m_invMassB + m_invIB * ruB * ruB; + + m_mass = mA + m_ratio * m_ratio * mB; + + if (m_mass > 0.0f) { + m_mass = 1.0f / m_mass; + } + + if (data.step.warmStarting) { + // Scale impulses to support variable time steps. + m_impulse *= data.step.dtRatio; + + // Warm starting. + b2Vec2 PA = -(m_impulse)*m_uA; + b2Vec2 PB = (-m_ratio * m_impulse) * m_uB; + + vA += m_invMassA * PA; + wA += m_invIA * b2Cross(m_rA, PA); + vB += m_invMassB * PB; + wB += m_invIB * b2Cross(m_rB, PB); + } else { + m_impulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData & data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - - float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB); - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - b2Vec2 PA = -impulse * m_uA; - b2Vec2 PB = -m_ratio * impulse * m_uB; - vA += m_invMassA * PA; - wA += m_invIA * b2Cross(m_rA, PA); - vB += m_invMassB * PB; - wB += m_invIB * b2Cross(m_rB, PB); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + + float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB); + float32 impulse = -m_mass * Cdot; + m_impulse += impulse; + + b2Vec2 PA = -impulse * m_uA; + b2Vec2 PB = -m_ratio * impulse * m_uB; + vA += m_invMassA * PA; + wA += m_invIA * b2Cross(m_rA, PA); + vB += m_invMassB * PB; + wB += m_invIB * b2Cross(m_rB, PB); + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data) +bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData & data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // Get the pulley axes. - b2Vec2 uA = cA + rA - m_groundAnchorA; - b2Vec2 uB = cB + rB - m_groundAnchorB; - - float32 lengthA = uA.Length(); - float32 lengthB = uB.Length(); - - if (lengthA > 10.0f * b2_linearSlop) - { - uA *= 1.0f / lengthA; - } - else - { - uA.SetZero(); - } - - if (lengthB > 10.0f * b2_linearSlop) - { - uB *= 1.0f / lengthB; - } - else - { - uB.SetZero(); - } - - // Compute effective mass. - float32 ruA = b2Cross(rA, uA); - float32 ruB = b2Cross(rB, uB); - - float32 mA = m_invMassA + m_invIA * ruA * ruA; - float32 mB = m_invMassB + m_invIB * ruB * ruB; - - float32 mass = mA + m_ratio * m_ratio * mB; - - if (mass > 0.0f) - { - mass = 1.0f / mass; - } - - float32 C = m_constant - lengthA - m_ratio * lengthB; - float32 linearError = b2Abs(C); - - float32 impulse = -mass * C; - - b2Vec2 PA = -impulse * uA; - b2Vec2 PB = -m_ratio * impulse * uB; - - cA += m_invMassA * PA; - aA += m_invIA * b2Cross(rA, PA); - cB += m_invMassB * PB; - aB += m_invIB * b2Cross(rB, PB); - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return linearError < b2_linearSlop; -} + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; -b2Vec2 b2PulleyJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} + b2Rot qA(aA), qB(aB); -b2Vec2 b2PulleyJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + // Get the pulley axes. + b2Vec2 uA = cA + rA - m_groundAnchorA; + b2Vec2 uB = cB + rB - m_groundAnchorB; + + float32 lengthA = uA.Length(); + float32 lengthB = uB.Length(); + + if (lengthA > 10.0f * b2_linearSlop) { + uA *= 1.0f / lengthA; + } else { + uA.SetZero(); + } + + if (lengthB > 10.0f * b2_linearSlop) { + uB *= 1.0f / lengthB; + } else { + uB.SetZero(); + } + + // Compute effective mass. + float32 ruA = b2Cross(rA, uA); + float32 ruB = b2Cross(rB, uB); + + float32 mA = m_invMassA + m_invIA * ruA * ruA; + float32 mB = m_invMassB + m_invIB * ruB * ruB; + + float32 mass = mA + m_ratio * m_ratio * mB; + + if (mass > 0.0f) { + mass = 1.0f / mass; + } + + float32 C = m_constant - lengthA - m_ratio * lengthB; + float32 linearError = b2Abs(C); + + float32 impulse = -mass * C; + + b2Vec2 PA = -impulse * uA; + b2Vec2 PB = -m_ratio * impulse * uB; + + cA += m_invMassA * PA; + aA += m_invIA * b2Cross(rA, PA); + cB += m_invMassB * PB; + aB += m_invIB * b2Cross(rB, PB); + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + + return linearError < b2_linearSlop; } +b2Vec2 b2PulleyJoint::GetAnchorA() const { return m_bodyA->GetWorldPoint(m_localAnchorA); } + +b2Vec2 b2PulleyJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } + b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 P = m_impulse * m_uB; - return inv_dt * P; + b2Vec2 P = m_impulse * m_uB; + return inv_dt * P; } float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const { - B2_NOT_USED(inv_dt); - return 0.0f; + B2_NOT_USED(inv_dt); + return 0.0f; } -b2Vec2 b2PulleyJoint::GetGroundAnchorA() const -{ - return m_groundAnchorA; -} +b2Vec2 b2PulleyJoint::GetGroundAnchorA() const { return m_groundAnchorA; } -b2Vec2 b2PulleyJoint::GetGroundAnchorB() const -{ - return m_groundAnchorB; -} +b2Vec2 b2PulleyJoint::GetGroundAnchorB() const { return m_groundAnchorB; } -float32 b2PulleyJoint::GetLengthA() const -{ - return m_lengthA; -} +float32 b2PulleyJoint::GetLengthA() const { return m_lengthA; } -float32 b2PulleyJoint::GetLengthB() const -{ - return m_lengthB; -} +float32 b2PulleyJoint::GetLengthB() const { return m_lengthB; } -float32 b2PulleyJoint::GetRatio() const -{ - return m_ratio; -} +float32 b2PulleyJoint::GetRatio() const { return m_ratio; } float32 b2PulleyJoint::GetCurrentLengthA() const { - b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA); - b2Vec2 s = m_groundAnchorA; - b2Vec2 d = p - s; - return d.Length(); + b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA); + b2Vec2 s = m_groundAnchorA; + b2Vec2 d = p - s; + return d.Length(); } float32 b2PulleyJoint::GetCurrentLengthB() const { - b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB); - b2Vec2 s = m_groundAnchorB; - b2Vec2 d = p - s; - return d.Length(); + b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB); + b2Vec2 s = m_groundAnchorB; + b2Vec2 d = p - s; + return d.Length(); } void b2PulleyJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2PulleyJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y); - b2Log(" jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.lengthA = %.15lef;\n", m_lengthA); - b2Log(" jd.lengthB = %.15lef;\n", m_lengthB); - b2Log(" jd.ratio = %.15lef;\n", m_ratio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2PulleyJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y); + b2Log(" jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.lengthA = %.15lef;\n", m_lengthA); + b2Log(" jd.lengthB = %.15lef;\n", m_lengthB); + b2Log(" jd.ratio = %.15lef;\n", m_ratio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } -void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin) +void b2PulleyJoint::ShiftOrigin(const b2Vec2 & newOrigin) { - m_groundAnchorA -= newOrigin; - m_groundAnchorB -= newOrigin; + m_groundAnchorA -= newOrigin; + m_groundAnchorB -= newOrigin; } diff --git a/flatland_box2d/src/Dynamics/Joints/b2RevoluteJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2RevoluteJoint.cpp index a6c0d437..42abb4bc 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2RevoluteJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2RevoluteJoint.cpp @@ -1,22 +1,23 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2RevoluteJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" @@ -33,479 +34,411 @@ // J = [0 0 -1 0 0 1] // K = invI1 + invI2 -void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) +void b2RevoluteJointDef::Initialize(b2Body * bA, b2Body * bB, const b2Vec2 & anchor) { - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); + referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); } -b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def) -: b2Joint(def) +b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef * def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - - m_lowerAngle = def->lowerAngle; - m_upperAngle = def->upperAngle; - m_maxMotorTorque = def->maxMotorTorque; - m_motorSpeed = def->motorSpeed; - m_enableLimit = def->enableLimit; - m_enableMotor = def->enableMotor; - m_limitState = e_inactiveLimit; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_referenceAngle = def->referenceAngle; + + m_impulse.SetZero(); + m_motorImpulse = 0.0f; + + m_lowerAngle = def->lowerAngle; + m_upperAngle = def->upperAngle; + m_maxMotorTorque = def->maxMotorTorque; + m_motorSpeed = def->motorSpeed; + m_enableLimit = def->enableLimit; + m_enableMotor = def->enableMotor; + m_limitState = e_inactiveLimit; } -void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data) +void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - bool fixedRotation = (iA + iB == 0.0f); - - m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; - m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; - m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB; - m_mass.ex.y = m_mass.ey.x; - m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; - m_mass.ez.y = m_rA.x * iA + m_rB.x * iB; - m_mass.ex.z = m_mass.ez.x; - m_mass.ey.z = m_mass.ez.y; - m_mass.ez.z = iA + iB; - - m_motorMass = iA + iB; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - - if (m_enableMotor == false || fixedRotation) - { - m_motorImpulse = 0.0f; - } - - if (m_enableLimit && fixedRotation == false) - { - float32 jointAngle = aB - aA - m_referenceAngle; - if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) - { - m_limitState = e_equalLimits; - } - else if (jointAngle <= m_lowerAngle) - { - if (m_limitState != e_atLowerLimit) - { - m_impulse.z = 0.0f; - } - m_limitState = e_atLowerLimit; - } - else if (jointAngle >= m_upperAngle) - { - if (m_limitState != e_atUpperLimit) - { - m_impulse.z = 0.0f; - } - m_limitState = e_atUpperLimit; - } - else - { - m_limitState = e_inactiveLimit; - m_impulse.z = 0.0f; - } - } - else - { - m_limitState = e_inactiveLimit; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_impulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P(m_impulse.x, m_impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z); - } - else - { - m_impulse.SetZero(); - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] [ + // -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + bool fixedRotation = (iA + iB == 0.0f); + + m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; + m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; + m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB; + m_mass.ex.y = m_mass.ey.x; + m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; + m_mass.ez.y = m_rA.x * iA + m_rB.x * iB; + m_mass.ex.z = m_mass.ez.x; + m_mass.ey.z = m_mass.ez.y; + m_mass.ez.z = iA + iB; + + m_motorMass = iA + iB; + if (m_motorMass > 0.0f) { + m_motorMass = 1.0f / m_motorMass; + } + + if (m_enableMotor == false || fixedRotation) { + m_motorImpulse = 0.0f; + } + + if (m_enableLimit && fixedRotation == false) { + float32 jointAngle = aB - aA - m_referenceAngle; + if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop) { + m_limitState = e_equalLimits; + } else if (jointAngle <= m_lowerAngle) { + if (m_limitState != e_atLowerLimit) { + m_impulse.z = 0.0f; + } + m_limitState = e_atLowerLimit; + } else if (jointAngle >= m_upperAngle) { + if (m_limitState != e_atUpperLimit) { + m_impulse.z = 0.0f; + } + m_limitState = e_atUpperLimit; + } else { + m_limitState = e_inactiveLimit; + m_impulse.z = 0.0f; + } + } else { + m_limitState = e_inactiveLimit; + } + + if (data.step.warmStarting) { + // Scale impulses to support a variable time step. + m_impulse *= data.step.dtRatio; + m_motorImpulse *= data.step.dtRatio; + + b2Vec2 P(m_impulse.x, m_impulse.y); + + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z); + + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z); + } else { + m_impulse.SetZero(); + m_motorImpulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData & data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - bool fixedRotation = (iA + iB == 0.0f); - - // Solve motor constraint. - if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false) - { - float32 Cdot = wB - wA - m_motorSpeed; - float32 impulse = -m_motorMass * Cdot; - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorTorque; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) - { - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - float32 Cdot2 = wB - wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - - b2Vec3 impulse = -m_mass.Solve33(Cdot); - - if (m_limitState == e_equalLimits) - { - m_impulse += impulse; - } - else if (m_limitState == e_atLowerLimit) - { - float32 newImpulse = m_impulse.z + impulse.z; - if (newImpulse < 0.0f) - { - b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); - b2Vec2 reduced = m_mass.Solve22(rhs); - impulse.x = reduced.x; - impulse.y = reduced.y; - impulse.z = -m_impulse.z; - m_impulse.x += reduced.x; - m_impulse.y += reduced.y; - m_impulse.z = 0.0f; - } - else - { - m_impulse += impulse; - } - } - else if (m_limitState == e_atUpperLimit) - { - float32 newImpulse = m_impulse.z + impulse.z; - if (newImpulse > 0.0f) - { - b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); - b2Vec2 reduced = m_mass.Solve22(rhs); - impulse.x = reduced.x; - impulse.y = reduced.y; - impulse.z = -m_impulse.z; - m_impulse.x += reduced.x; - m_impulse.y += reduced.y; - m_impulse.z = 0.0f; - } - else - { - m_impulse += impulse; - } - } - - b2Vec2 P(impulse.x, impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + impulse.z); - } - else - { - // Solve point-to-point constraint - b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - b2Vec2 impulse = m_mass.Solve22(-Cdot); - - m_impulse.x += impulse.x; - m_impulse.y += impulse.y; - - vA -= mA * impulse; - wA -= iA * b2Cross(m_rA, impulse); - - vB += mB * impulse; - wB += iB * b2Cross(m_rB, impulse); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + bool fixedRotation = (iA + iB == 0.0f); + + // Solve motor constraint. + if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false) { + float32 Cdot = wB - wA - m_motorSpeed; + float32 impulse = -m_motorMass * Cdot; + float32 oldImpulse = m_motorImpulse; + float32 maxImpulse = data.step.dt * m_maxMotorTorque; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve limit constraint. + if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) { + b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + float32 Cdot2 = wB - wA; + b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); + + b2Vec3 impulse = -m_mass.Solve33(Cdot); + + if (m_limitState == e_equalLimits) { + m_impulse += impulse; + } else if (m_limitState == e_atLowerLimit) { + float32 newImpulse = m_impulse.z + impulse.z; + if (newImpulse < 0.0f) { + b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); + b2Vec2 reduced = m_mass.Solve22(rhs); + impulse.x = reduced.x; + impulse.y = reduced.y; + impulse.z = -m_impulse.z; + m_impulse.x += reduced.x; + m_impulse.y += reduced.y; + m_impulse.z = 0.0f; + } else { + m_impulse += impulse; + } + } else if (m_limitState == e_atUpperLimit) { + float32 newImpulse = m_impulse.z + impulse.z; + if (newImpulse > 0.0f) { + b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y); + b2Vec2 reduced = m_mass.Solve22(rhs); + impulse.x = reduced.x; + impulse.y = reduced.y; + impulse.z = -m_impulse.z; + m_impulse.x += reduced.x; + m_impulse.y += reduced.y; + m_impulse.z = 0.0f; + } else { + m_impulse += impulse; + } + } + + b2Vec2 P(impulse.x, impulse.y); + + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + impulse.z); + + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + impulse.z); + } else { + // Solve point-to-point constraint + b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + b2Vec2 impulse = m_mass.Solve22(-Cdot); + + m_impulse.x += impulse.x; + m_impulse.y += impulse.y; + + vA -= mA * impulse; + wA -= iA * b2Cross(m_rA, impulse); + + vB += mB * impulse; + wB += iB * b2Cross(m_rB, impulse); + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data) +bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData & data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 angularError = 0.0f; - float32 positionError = 0.0f; - - bool fixedRotation = (m_invIA + m_invIB == 0.0f); - - // Solve angular limit constraint. - if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) - { - float32 angle = aB - aA - m_referenceAngle; - float32 limitImpulse = 0.0f; - - if (m_limitState == e_equalLimits) - { - // Prevent large angular corrections - float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); - limitImpulse = -m_motorMass * C; - angularError = b2Abs(C); - } - else if (m_limitState == e_atLowerLimit) - { - float32 C = angle - m_lowerAngle; - angularError = -C; - - // Prevent large angular corrections and allow some slop. - C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); - limitImpulse = -m_motorMass * C; - } - else if (m_limitState == e_atUpperLimit) - { - float32 C = angle - m_upperAngle; - angularError = C; - - // Prevent large angular corrections and allow some slop. - C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection); - limitImpulse = -m_motorMass * C; - } - - aA -= m_invIA * limitImpulse; - aB += m_invIB * limitImpulse; - } - - // Solve point-to-point constraint. - { - qA.Set(aA); - qB.Set(aB); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - b2Vec2 C = cB + rB - cA - rA; - positionError = C.Length(); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat22 K; - K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; - K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; - K.ey.x = K.ex.y; - K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; - - b2Vec2 impulse = -K.Solve(C); - - cA -= mA * impulse; - aA -= iA * b2Cross(rA, impulse); - - cB += mB * impulse; - aB += iB * b2Cross(rB, impulse); - } - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return positionError <= b2_linearSlop && angularError <= b2_angularSlop; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + + b2Rot qA(aA), qB(aB); + + float32 angularError = 0.0f; + float32 positionError = 0.0f; + + bool fixedRotation = (m_invIA + m_invIB == 0.0f); + + // Solve angular limit constraint. + if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false) { + float32 angle = aB - aA - m_referenceAngle; + float32 limitImpulse = 0.0f; + + if (m_limitState == e_equalLimits) { + // Prevent large angular corrections + float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection); + limitImpulse = -m_motorMass * C; + angularError = b2Abs(C); + } else if (m_limitState == e_atLowerLimit) { + float32 C = angle - m_lowerAngle; + angularError = -C; + + // Prevent large angular corrections and allow some slop. + C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f); + limitImpulse = -m_motorMass * C; + } else if (m_limitState == e_atUpperLimit) { + float32 C = angle - m_upperAngle; + angularError = C; + + // Prevent large angular corrections and allow some slop. + C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection); + limitImpulse = -m_motorMass * C; + } + + aA -= m_invIA * limitImpulse; + aB += m_invIB * limitImpulse; + } + + // Solve point-to-point constraint. + { + qA.Set(aA); + qB.Set(aB); + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + b2Vec2 C = cB + rB - cA - rA; + positionError = C.Length(); + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Mat22 K; + K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y; + K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y; + K.ey.x = K.ex.y; + K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x; + + b2Vec2 impulse = -K.Solve(C); + + cA -= mA * impulse; + aA -= iA * b2Cross(rA, impulse); + + cB += mB * impulse; + aB += iB * b2Cross(rB, impulse); + } + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + + return positionError <= b2_linearSlop && angularError <= b2_angularSlop; } -b2Vec2 b2RevoluteJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} +b2Vec2 b2RevoluteJoint::GetAnchorA() const { return m_bodyA->GetWorldPoint(m_localAnchorA); } -b2Vec2 b2RevoluteJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} +b2Vec2 b2RevoluteJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 P(m_impulse.x, m_impulse.y); - return inv_dt * P; + b2Vec2 P(m_impulse.x, m_impulse.y); + return inv_dt * P; } -float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_impulse.z; -} +float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const { return inv_dt * m_impulse.z; } float32 b2RevoluteJoint::GetJointAngle() const { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; + b2Body * bA = m_bodyA; + b2Body * bB = m_bodyB; + return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle; } float32 b2RevoluteJoint::GetJointSpeed() const { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_angularVelocity - bA->m_angularVelocity; + b2Body * bA = m_bodyA; + b2Body * bB = m_bodyB; + return bB->m_angularVelocity - bA->m_angularVelocity; } -bool b2RevoluteJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} +bool b2RevoluteJoint::IsMotorEnabled() const { return m_enableMotor; } void b2RevoluteJoint::EnableMotor(bool flag) { - if (flag != m_enableMotor) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; - } + if (flag != m_enableMotor) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableMotor = flag; + } } -float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} +float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const { return inv_dt * m_motorImpulse; } void b2RevoluteJoint::SetMotorSpeed(float32 speed) { - if (speed != m_motorSpeed) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; - } + if (speed != m_motorSpeed) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; + } } void b2RevoluteJoint::SetMaxMotorTorque(float32 torque) { - if (torque != m_maxMotorTorque) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorTorque = torque; - } + if (torque != m_maxMotorTorque) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_maxMotorTorque = torque; + } } -bool b2RevoluteJoint::IsLimitEnabled() const -{ - return m_enableLimit; -} +bool b2RevoluteJoint::IsLimitEnabled() const { return m_enableLimit; } void b2RevoluteJoint::EnableLimit(bool flag) { - if (flag != m_enableLimit) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableLimit = flag; - m_impulse.z = 0.0f; - } + if (flag != m_enableLimit) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableLimit = flag; + m_impulse.z = 0.0f; + } } -float32 b2RevoluteJoint::GetLowerLimit() const -{ - return m_lowerAngle; -} +float32 b2RevoluteJoint::GetLowerLimit() const { return m_lowerAngle; } -float32 b2RevoluteJoint::GetUpperLimit() const -{ - return m_upperAngle; -} +float32 b2RevoluteJoint::GetUpperLimit() const { return m_upperAngle; } void b2RevoluteJoint::SetLimits(float32 lower, float32 upper) { - b2Assert(lower <= upper); - - if (lower != m_lowerAngle || upper != m_upperAngle) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_impulse.z = 0.0f; - m_lowerAngle = lower; - m_upperAngle = upper; - } + b2Assert(lower <= upper); + + if (lower != m_lowerAngle || upper != m_upperAngle) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_impulse.z = 0.0f; + m_lowerAngle = lower; + m_upperAngle = upper; + } } void b2RevoluteJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2RevoluteJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); - b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); - b2Log(" jd.lowerAngle = %.15lef;\n", m_lowerAngle); - b2Log(" jd.upperAngle = %.15lef;\n", m_upperAngle); - b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); - b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2RevoluteJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); + b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit); + b2Log(" jd.lowerAngle = %.15lef;\n", m_lowerAngle); + b2Log(" jd.upperAngle = %.15lef;\n", m_upperAngle); + b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); + b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); + b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2RopeJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2RopeJoint.cpp index 2cd5a1cc..2241c996 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2RopeJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2RopeJoint.cpp @@ -1,26 +1,26 @@ /* -* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2007-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2RopeJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" - // Limit: // C = norm(pB - pA) - L // u = (pB - pA) / norm(pB - pA) @@ -29,213 +29,190 @@ // K = J * invM * JT // = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2 -b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def) -: b2Joint(def) +b2RopeJoint::b2RopeJoint(const b2RopeJointDef * def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; - m_maxLength = def->maxLength; + m_maxLength = def->maxLength; - m_mass = 0.0f; - m_impulse = 0.0f; - m_state = e_inactiveLimit; - m_length = 0.0f; + m_mass = 0.0f; + m_impulse = 0.0f; + m_state = e_inactiveLimit; + m_length = 0.0f; } -void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data) +void b2RopeJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - m_u = cB + m_rB - cA - m_rA; - - m_length = m_u.Length(); - - float32 C = m_length - m_maxLength; - if (C > 0.0f) - { - m_state = e_atUpperLimit; - } - else - { - m_state = e_inactiveLimit; - } - - if (m_length > b2_linearSlop) - { - m_u *= 1.0f / m_length; - } - else - { - m_u.SetZero(); - m_mass = 0.0f; - m_impulse = 0.0f; - return; - } - - // Compute effective mass. - float32 crA = b2Cross(m_rA, m_u); - float32 crB = b2Cross(m_rB, m_u); - float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB; - - m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; - - if (data.step.warmStarting) - { - // Scale the impulse to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - } - else - { - m_impulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + m_u = cB + m_rB - cA - m_rA; + + m_length = m_u.Length(); + + float32 C = m_length - m_maxLength; + if (C > 0.0f) { + m_state = e_atUpperLimit; + } else { + m_state = e_inactiveLimit; + } + + if (m_length > b2_linearSlop) { + m_u *= 1.0f / m_length; + } else { + m_u.SetZero(); + m_mass = 0.0f; + m_impulse = 0.0f; + return; + } + + // Compute effective mass. + float32 crA = b2Cross(m_rA, m_u); + float32 crB = b2Cross(m_rB, m_u); + float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB; + + m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; + + if (data.step.warmStarting) { + // Scale the impulse to support a variable time step. + m_impulse *= data.step.dtRatio; + + b2Vec2 P = m_impulse * m_u; + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + } else { + m_impulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2RopeJoint::SolveVelocityConstraints(const b2SolverData & data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Cdot = dot(u, v + cross(w, r)) - b2Vec2 vpA = vA + b2Cross(wA, m_rA); - b2Vec2 vpB = vB + b2Cross(wB, m_rB); - float32 C = m_length - m_maxLength; - float32 Cdot = b2Dot(m_u, vpB - vpA); - - // Predictive constraint. - if (C < 0.0f) - { - Cdot += data.step.inv_dt * C; - } - - float32 impulse = -m_mass * Cdot; - float32 oldImpulse = m_impulse; - m_impulse = b2Min(0.0f, m_impulse + impulse); - impulse = m_impulse - oldImpulse; - - b2Vec2 P = impulse * m_u; - vA -= m_invMassA * P; - wA -= m_invIA * b2Cross(m_rA, P); - vB += m_invMassB * P; - wB += m_invIB * b2Cross(m_rB, P); - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vpA = vA + b2Cross(wA, m_rA); + b2Vec2 vpB = vB + b2Cross(wB, m_rB); + float32 C = m_length - m_maxLength; + float32 Cdot = b2Dot(m_u, vpB - vpA); + + // Predictive constraint. + if (C < 0.0f) { + Cdot += data.step.inv_dt * C; + } + + float32 impulse = -m_mass * Cdot; + float32 oldImpulse = m_impulse; + m_impulse = b2Min(0.0f, m_impulse + impulse); + impulse = m_impulse - oldImpulse; + + b2Vec2 P = impulse * m_u; + vA -= m_invMassA * P; + wA -= m_invIA * b2Cross(m_rA, P); + vB += m_invMassB * P; + wB += m_invIB * b2Cross(m_rB, P); + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data) +bool b2RopeJoint::SolvePositionConstraints(const b2SolverData & data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; - b2Rot qA(aA), qB(aB); + b2Rot qA(aA), qB(aB); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 u = cB + rB - cA - rA; + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 u = cB + rB - cA - rA; - float32 length = u.Normalize(); - float32 C = length - m_maxLength; + float32 length = u.Normalize(); + float32 C = length - m_maxLength; - C = b2Clamp(C, 0.0f, b2_maxLinearCorrection); + C = b2Clamp(C, 0.0f, b2_maxLinearCorrection); - float32 impulse = -m_mass * C; - b2Vec2 P = impulse * u; + float32 impulse = -m_mass * C; + b2Vec2 P = impulse * u; - cA -= m_invMassA * P; - aA -= m_invIA * b2Cross(rA, P); - cB += m_invMassB * P; - aB += m_invIB * b2Cross(rB, P); + cA -= m_invMassA * P; + aA -= m_invIA * b2Cross(rA, P); + cB += m_invMassB * P; + aB += m_invIB * b2Cross(rB, P); - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; - return length - m_maxLength < b2_linearSlop; + return length - m_maxLength < b2_linearSlop; } -b2Vec2 b2RopeJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} +b2Vec2 b2RopeJoint::GetAnchorA() const { return m_bodyA->GetWorldPoint(m_localAnchorA); } -b2Vec2 b2RopeJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} +b2Vec2 b2RopeJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 F = (inv_dt * m_impulse) * m_u; - return F; + b2Vec2 F = (inv_dt * m_impulse) * m_u; + return F; } float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const { - B2_NOT_USED(inv_dt); - return 0.0f; + B2_NOT_USED(inv_dt); + return 0.0f; } -float32 b2RopeJoint::GetMaxLength() const -{ - return m_maxLength; -} +float32 b2RopeJoint::GetMaxLength() const { return m_maxLength; } -b2LimitState b2RopeJoint::GetLimitState() const -{ - return m_state; -} +b2LimitState b2RopeJoint::GetLimitState() const { return m_state; } void b2RopeJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2RopeJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.maxLength = %.15lef;\n", m_maxLength); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2RopeJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.maxLength = %.15lef;\n", m_maxLength); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2WeldJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2WeldJoint.cpp index 450b9134..a3649666 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2WeldJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2WeldJoint.cpp @@ -1,22 +1,23 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2WeldJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" @@ -34,311 +35,284 @@ // J = [0 0 -1 0 0 1] // K = invI1 + invI2 -void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor) +void b2WeldJointDef::Initialize(b2Body * bA, b2Body * bB, const b2Vec2 & anchor) { - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); + referenceAngle = bodyB->GetAngle() - bodyA->GetAngle(); } -b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def) -: b2Joint(def) +b2WeldJoint::b2WeldJoint(const b2WeldJointDef * def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_referenceAngle = def->referenceAngle; - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_referenceAngle = def->referenceAngle; + m_frequencyHz = def->frequencyHz; + m_dampingRatio = def->dampingRatio; - m_impulse.SetZero(); + m_impulse.SetZero(); } -void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data) +void b2WeldJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - // J = [-I -r1_skew I r2_skew] - // [ 0 -1 0 1] - // r_skew = [-ry; rx] - - // Matlab - // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] - // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] - // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Mat33 K; - K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; - K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; - K.ez.x = -m_rA.y * iA - m_rB.y * iB; - K.ex.y = K.ey.x; - K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; - K.ez.y = m_rA.x * iA + m_rB.x * iB; - K.ex.z = K.ez.x; - K.ey.z = K.ez.y; - K.ez.z = iA + iB; - - if (m_frequencyHz > 0.0f) - { - K.GetInverse22(&m_mass); - - float32 invM = iA + iB; - float32 m = invM > 0.0f ? 1.0f / invM : 0.0f; - - float32 C = aB - aA - m_referenceAngle; - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 d = 2.0f * m * m_dampingRatio * omega; - - // Spring stiffness - float32 k = m * omega * omega; - - // magic formulas - float32 h = data.step.dt; - m_gamma = h * (d + h * k); - m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; - m_bias = C * h * k * m_gamma; - - invM += m_gamma; - m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; - } - else if (K.ez.z == 0.0f) - { - K.GetInverse22(&m_mass); - m_gamma = 0.0f; - m_bias = 0.0f; - } - else - { - K.GetSymInverse33(&m_mass); - m_gamma = 0.0f; - m_bias = 0.0f; - } - - if (data.step.warmStarting) - { - // Scale impulses to support a variable time step. - m_impulse *= data.step.dtRatio; - - b2Vec2 P(m_impulse.x, m_impulse.y); - - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + m_impulse.z); - - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + m_impulse.z); - } - else - { - m_impulse.SetZero(); - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + // J = [-I -r1_skew I r2_skew] + // [ 0 -1 0 1] + // r_skew = [-ry; rx] + + // Matlab + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] [ + // -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Mat33 K; + K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB; + K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB; + K.ez.x = -m_rA.y * iA - m_rB.y * iB; + K.ex.y = K.ey.x; + K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB; + K.ez.y = m_rA.x * iA + m_rB.x * iB; + K.ex.z = K.ez.x; + K.ey.z = K.ez.y; + K.ez.z = iA + iB; + + if (m_frequencyHz > 0.0f) { + K.GetInverse22(&m_mass); + + float32 invM = iA + iB; + float32 m = invM > 0.0f ? 1.0f / invM : 0.0f; + + float32 C = aB - aA - m_referenceAngle; + + // Frequency + float32 omega = 2.0f * b2_pi * m_frequencyHz; + + // Damping coefficient + float32 d = 2.0f * m * m_dampingRatio * omega; + + // Spring stiffness + float32 k = m * omega * omega; + + // magic formulas + float32 h = data.step.dt; + m_gamma = h * (d + h * k); + m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; + m_bias = C * h * k * m_gamma; + + invM += m_gamma; + m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f; + } else if (K.ez.z == 0.0f) { + K.GetInverse22(&m_mass); + m_gamma = 0.0f; + m_bias = 0.0f; + } else { + K.GetSymInverse33(&m_mass); + m_gamma = 0.0f; + m_bias = 0.0f; + } + + if (data.step.warmStarting) { + // Scale impulses to support a variable time step. + m_impulse *= data.step.dtRatio; + + b2Vec2 P(m_impulse.x, m_impulse.y); + + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + m_impulse.z); + + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + m_impulse.z); + } else { + m_impulse.SetZero(); + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2WeldJoint::SolveVelocityConstraints(const b2SolverData & data) { - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; - if (m_frequencyHz > 0.0f) - { - float32 Cdot2 = wB - wA; + if (m_frequencyHz > 0.0f) { + float32 Cdot2 = wB - wA; - float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); - m_impulse.z += impulse2; + float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); + m_impulse.z += impulse2; - wA -= iA * impulse2; - wB += iB * impulse2; + wA -= iA * impulse2; + wB += iB * impulse2; - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1); - m_impulse.x += impulse1.x; - m_impulse.y += impulse1.y; + b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1); + m_impulse.x += impulse1.x; + m_impulse.y += impulse1.y; - b2Vec2 P = impulse1; + b2Vec2 P = impulse1; - vA -= mA * P; - wA -= iA * b2Cross(m_rA, P); + vA -= mA * P; + wA -= iA * b2Cross(m_rA, P); - vB += mB * P; - wB += iB * b2Cross(m_rB, P); - } - else - { - b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); - float32 Cdot2 = wB - wA; - b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); + vB += mB * P; + wB += iB * b2Cross(m_rB, P); + } else { + b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA); + float32 Cdot2 = wB - wA; + b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2); - b2Vec3 impulse = -b2Mul(m_mass, Cdot); - m_impulse += impulse; + b2Vec3 impulse = -b2Mul(m_mass, Cdot); + m_impulse += impulse; - b2Vec2 P(impulse.x, impulse.y); + b2Vec2 P(impulse.x, impulse.y); - vA -= mA * P; - wA -= iA * (b2Cross(m_rA, P) + impulse.z); + vA -= mA * P; + wA -= iA * (b2Cross(m_rA, P) + impulse.z); - vB += mB * P; - wB += iB * (b2Cross(m_rB, P) + impulse.z); - } + vB += mB * P; + wB += iB * (b2Cross(m_rB, P) + impulse.z); + } - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data) +bool b2WeldJoint::SolvePositionConstraints(const b2SolverData & data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - - b2Rot qA(aA), qB(aB); - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - - float32 positionError, angularError; - - b2Mat33 K; - K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; - K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; - K.ez.x = -rA.y * iA - rB.y * iB; - K.ex.y = K.ey.x; - K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; - K.ez.y = rA.x * iA + rB.x * iB; - K.ex.z = K.ez.x; - K.ey.z = K.ez.y; - K.ez.z = iA + iB; - - if (m_frequencyHz > 0.0f) - { - b2Vec2 C1 = cB + rB - cA - rA; - - positionError = C1.Length(); - angularError = 0.0f; - - b2Vec2 P = -K.Solve22(C1); - - cA -= mA * P; - aA -= iA * b2Cross(rA, P); - - cB += mB * P; - aB += iB * b2Cross(rB, P); - } - else - { - b2Vec2 C1 = cB + rB - cA - rA; - float32 C2 = aB - aA - m_referenceAngle; - - positionError = C1.Length(); - angularError = b2Abs(C2); - - b2Vec3 C(C1.x, C1.y, C2); - - b2Vec3 impulse; - if (K.ez.z > 0.0f) - { - impulse = -K.Solve33(C); - } - else - { - b2Vec2 impulse2 = -K.Solve22(C1); - impulse.Set(impulse2.x, impulse2.y, 0.0f); - } - - b2Vec2 P(impulse.x, impulse.y); - - cA -= mA * P; - aA -= iA * (b2Cross(rA, P) + impulse.z); - - cB += mB * P; - aB += iB * (b2Cross(rB, P) + impulse.z); - } - - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; - - return positionError <= b2_linearSlop && angularError <= b2_angularSlop; -} + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; -b2Vec2 b2WeldJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} + b2Rot qA(aA), qB(aB); -b2Vec2 b2WeldJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + + float32 positionError, angularError; + + b2Mat33 K; + K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.ez.x = -rA.y * iA - rB.y * iB; + K.ex.y = K.ey.x; + K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + K.ez.y = rA.x * iA + rB.x * iB; + K.ex.z = K.ez.x; + K.ey.z = K.ez.y; + K.ez.z = iA + iB; + + if (m_frequencyHz > 0.0f) { + b2Vec2 C1 = cB + rB - cA - rA; + + positionError = C1.Length(); + angularError = 0.0f; + + b2Vec2 P = -K.Solve22(C1); + + cA -= mA * P; + aA -= iA * b2Cross(rA, P); + + cB += mB * P; + aB += iB * b2Cross(rB, P); + } else { + b2Vec2 C1 = cB + rB - cA - rA; + float32 C2 = aB - aA - m_referenceAngle; + + positionError = C1.Length(); + angularError = b2Abs(C2); + + b2Vec3 C(C1.x, C1.y, C2); + + b2Vec3 impulse; + if (K.ez.z > 0.0f) { + impulse = -K.Solve33(C); + } else { + b2Vec2 impulse2 = -K.Solve22(C1); + impulse.Set(impulse2.x, impulse2.y, 0.0f); + } + + b2Vec2 P(impulse.x, impulse.y); + + cA -= mA * P; + aA -= iA * (b2Cross(rA, P) + impulse.z); + + cB += mB * P; + aB += iB * (b2Cross(rB, P) + impulse.z); + } + + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; + + return positionError <= b2_linearSlop && angularError <= b2_angularSlop; } +b2Vec2 b2WeldJoint::GetAnchorA() const { return m_bodyA->GetWorldPoint(m_localAnchorA); } + +b2Vec2 b2WeldJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } + b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const { - b2Vec2 P(m_impulse.x, m_impulse.y); - return inv_dt * P; + b2Vec2 P(m_impulse.x, m_impulse.y); + return inv_dt * P; } -float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_impulse.z; -} +float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const { return inv_dt * m_impulse.z; } void b2WeldJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2WeldJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); - b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); - b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2WeldJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle); + b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); + b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/flatland_box2d/src/Dynamics/Joints/b2WheelJoint.cpp b/flatland_box2d/src/Dynamics/Joints/b2WheelJoint.cpp index 4941e21b..a672572e 100644 --- a/flatland_box2d/src/Dynamics/Joints/b2WheelJoint.cpp +++ b/flatland_box2d/src/Dynamics/Joints/b2WheelJoint.cpp @@ -1,456 +1,425 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/Joints/b2WheelJoint.h" + #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2TimeStep.h" // Linear constraint (point-to-line) // d = pB - pA = xB + rB - xA - rA // C = dot(ay, d) -// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) -// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) +// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, +// rA)) +// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + +// dot(cross(rB, ay), vB) // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] // Spring linear constraint // C = dot(ax, d) -// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) -// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] +// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + +// dot(cross(rB, ax), vB) J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] // Motor rotational constraint // Cdot = wB - wA // J = [0 0 -1 0 0 1] -void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis) +void b2WheelJointDef::Initialize( + b2Body * bA, b2Body * bB, const b2Vec2 & anchor, const b2Vec2 & axis) { - bodyA = bA; - bodyB = bB; - localAnchorA = bodyA->GetLocalPoint(anchor); - localAnchorB = bodyB->GetLocalPoint(anchor); - localAxisA = bodyA->GetLocalVector(axis); + bodyA = bA; + bodyB = bB; + localAnchorA = bodyA->GetLocalPoint(anchor); + localAnchorB = bodyB->GetLocalPoint(anchor); + localAxisA = bodyA->GetLocalVector(axis); } -b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def) -: b2Joint(def) +b2WheelJoint::b2WheelJoint(const b2WheelJointDef * def) : b2Joint(def) { - m_localAnchorA = def->localAnchorA; - m_localAnchorB = def->localAnchorB; - m_localXAxisA = def->localAxisA; - m_localYAxisA = b2Cross(1.0f, m_localXAxisA); - - m_mass = 0.0f; - m_impulse = 0.0f; - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - m_springMass = 0.0f; - m_springImpulse = 0.0f; - - m_maxMotorTorque = def->maxMotorTorque; - m_motorSpeed = def->motorSpeed; - m_enableMotor = def->enableMotor; - - m_frequencyHz = def->frequencyHz; - m_dampingRatio = def->dampingRatio; - - m_bias = 0.0f; - m_gamma = 0.0f; - - m_ax.SetZero(); - m_ay.SetZero(); + m_localAnchorA = def->localAnchorA; + m_localAnchorB = def->localAnchorB; + m_localXAxisA = def->localAxisA; + m_localYAxisA = b2Cross(1.0f, m_localXAxisA); + + m_mass = 0.0f; + m_impulse = 0.0f; + m_motorMass = 0.0f; + m_motorImpulse = 0.0f; + m_springMass = 0.0f; + m_springImpulse = 0.0f; + + m_maxMotorTorque = def->maxMotorTorque; + m_motorSpeed = def->motorSpeed; + m_enableMotor = def->enableMotor; + + m_frequencyHz = def->frequencyHz; + m_dampingRatio = def->dampingRatio; + + m_bias = 0.0f; + m_gamma = 0.0f; + + m_ax.SetZero(); + m_ay.SetZero(); } -void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data) +void b2WheelJoint::InitVelocityConstraints(const b2SolverData & data) { - m_indexA = m_bodyA->m_islandIndex; - m_indexB = m_bodyB->m_islandIndex; - m_localCenterA = m_bodyA->m_sweep.localCenter; - m_localCenterB = m_bodyB->m_sweep.localCenter; - m_invMassA = m_bodyA->m_invMass; - m_invMassB = m_bodyB->m_invMass; - m_invIA = m_bodyA->m_invI; - m_invIB = m_bodyB->m_invI; - - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - b2Rot qA(aA), qB(aB); - - // Compute the effective masses. - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = cB + rB - cA - rA; - - // Point to line constraint - { - m_ay = b2Mul(qA, m_localYAxisA); - m_sAy = b2Cross(d + rA, m_ay); - m_sBy = b2Cross(rB, m_ay); - - m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; - - if (m_mass > 0.0f) - { - m_mass = 1.0f / m_mass; - } - } - - // Spring constraint - m_springMass = 0.0f; - m_bias = 0.0f; - m_gamma = 0.0f; - if (m_frequencyHz > 0.0f) - { - m_ax = b2Mul(qA, m_localXAxisA); - m_sAx = b2Cross(d + rA, m_ax); - m_sBx = b2Cross(rB, m_ax); - - float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; - - if (invMass > 0.0f) - { - m_springMass = 1.0f / invMass; - - float32 C = b2Dot(d, m_ax); - - // Frequency - float32 omega = 2.0f * b2_pi * m_frequencyHz; - - // Damping coefficient - float32 damp = 2.0f * m_springMass * m_dampingRatio * omega; - - // Spring stiffness - float32 k = m_springMass * omega * omega; - - // magic formulas - float32 h = data.step.dt; - m_gamma = h * (damp + h * k); - if (m_gamma > 0.0f) - { - m_gamma = 1.0f / m_gamma; - } - - m_bias = C * h * k * m_gamma; - - m_springMass = invMass + m_gamma; - if (m_springMass > 0.0f) - { - m_springMass = 1.0f / m_springMass; - } - } - } - else - { - m_springImpulse = 0.0f; - } - - // Rotational motor - if (m_enableMotor) - { - m_motorMass = iA + iB; - if (m_motorMass > 0.0f) - { - m_motorMass = 1.0f / m_motorMass; - } - } - else - { - m_motorMass = 0.0f; - m_motorImpulse = 0.0f; - } - - if (data.step.warmStarting) - { - // Account for variable time step. - m_impulse *= data.step.dtRatio; - m_springImpulse *= data.step.dtRatio; - m_motorImpulse *= data.step.dtRatio; - - b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax; - float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; - float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; - - vA -= m_invMassA * P; - wA -= m_invIA * LA; - - vB += m_invMassB * P; - wB += m_invIB * LB; - } - else - { - m_impulse = 0.0f; - m_springImpulse = 0.0f; - m_motorImpulse = 0.0f; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + m_indexA = m_bodyA->m_islandIndex; + m_indexB = m_bodyB->m_islandIndex; + m_localCenterA = m_bodyA->m_sweep.localCenter; + m_localCenterB = m_bodyB->m_sweep.localCenter; + m_invMassA = m_bodyA->m_invMass; + m_invMassB = m_bodyB->m_invMass; + m_invIA = m_bodyA->m_invI; + m_invIB = m_bodyB->m_invI; + + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + b2Rot qA(aA), qB(aB); + + // Compute the effective masses. + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = cB + rB - cA - rA; + + // Point to line constraint + { + m_ay = b2Mul(qA, m_localYAxisA); + m_sAy = b2Cross(d + rA, m_ay); + m_sBy = b2Cross(rB, m_ay); + + m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; + + if (m_mass > 0.0f) { + m_mass = 1.0f / m_mass; + } + } + + // Spring constraint + m_springMass = 0.0f; + m_bias = 0.0f; + m_gamma = 0.0f; + if (m_frequencyHz > 0.0f) { + m_ax = b2Mul(qA, m_localXAxisA); + m_sAx = b2Cross(d + rA, m_ax); + m_sBx = b2Cross(rB, m_ax); + + float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; + + if (invMass > 0.0f) { + m_springMass = 1.0f / invMass; + + float32 C = b2Dot(d, m_ax); + + // Frequency + float32 omega = 2.0f * b2_pi * m_frequencyHz; + + // Damping coefficient + float32 damp = 2.0f * m_springMass * m_dampingRatio * omega; + + // Spring stiffness + float32 k = m_springMass * omega * omega; + + // magic formulas + float32 h = data.step.dt; + m_gamma = h * (damp + h * k); + if (m_gamma > 0.0f) { + m_gamma = 1.0f / m_gamma; + } + + m_bias = C * h * k * m_gamma; + + m_springMass = invMass + m_gamma; + if (m_springMass > 0.0f) { + m_springMass = 1.0f / m_springMass; + } + } + } else { + m_springImpulse = 0.0f; + } + + // Rotational motor + if (m_enableMotor) { + m_motorMass = iA + iB; + if (m_motorMass > 0.0f) { + m_motorMass = 1.0f / m_motorMass; + } + } else { + m_motorMass = 0.0f; + m_motorImpulse = 0.0f; + } + + if (data.step.warmStarting) { + // Account for variable time step. + m_impulse *= data.step.dtRatio; + m_springImpulse *= data.step.dtRatio; + m_motorImpulse *= data.step.dtRatio; + + b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax; + float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; + float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; + + vA -= m_invMassA * P; + wA -= m_invIA * LA; + + vB += m_invMassB * P; + wB += m_invIB * LB; + } else { + m_impulse = 0.0f; + m_springImpulse = 0.0f; + m_motorImpulse = 0.0f; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data) +void b2WheelJoint::SolveVelocityConstraints(const b2SolverData & data) { - float32 mA = m_invMassA, mB = m_invMassB; - float32 iA = m_invIA, iB = m_invIB; - - b2Vec2 vA = data.velocities[m_indexA].v; - float32 wA = data.velocities[m_indexA].w; - b2Vec2 vB = data.velocities[m_indexB].v; - float32 wB = data.velocities[m_indexB].w; - - // Solve spring constraint - { - float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; - float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse); - m_springImpulse += impulse; - - b2Vec2 P = impulse * m_ax; - float32 LA = impulse * m_sAx; - float32 LB = impulse * m_sBx; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - // Solve rotational motor constraint - { - float32 Cdot = wB - wA - m_motorSpeed; - float32 impulse = -m_motorMass * Cdot; - - float32 oldImpulse = m_motorImpulse; - float32 maxImpulse = data.step.dt * m_maxMotorTorque; - m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); - impulse = m_motorImpulse - oldImpulse; - - wA -= iA * impulse; - wB += iB * impulse; - } - - // Solve point to line constraint - { - float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA; - float32 impulse = -m_mass * Cdot; - m_impulse += impulse; - - b2Vec2 P = impulse * m_ay; - float32 LA = impulse * m_sAy; - float32 LB = impulse * m_sBy; - - vA -= mA * P; - wA -= iA * LA; - - vB += mB * P; - wB += iB * LB; - } - - data.velocities[m_indexA].v = vA; - data.velocities[m_indexA].w = wA; - data.velocities[m_indexB].v = vB; - data.velocities[m_indexB].w = wB; + float32 mA = m_invMassA, mB = m_invMassB; + float32 iA = m_invIA, iB = m_invIB; + + b2Vec2 vA = data.velocities[m_indexA].v; + float32 wA = data.velocities[m_indexA].w; + b2Vec2 vB = data.velocities[m_indexB].v; + float32 wB = data.velocities[m_indexB].w; + + // Solve spring constraint + { + float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA; + float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse); + m_springImpulse += impulse; + + b2Vec2 P = impulse * m_ax; + float32 LA = impulse * m_sAx; + float32 LB = impulse * m_sBx; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + // Solve rotational motor constraint + { + float32 Cdot = wB - wA - m_motorSpeed; + float32 impulse = -m_motorMass * Cdot; + + float32 oldImpulse = m_motorImpulse; + float32 maxImpulse = data.step.dt * m_maxMotorTorque; + m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse); + impulse = m_motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve point to line constraint + { + float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA; + float32 impulse = -m_mass * Cdot; + m_impulse += impulse; + + b2Vec2 P = impulse * m_ay; + float32 LA = impulse * m_sAy; + float32 LB = impulse * m_sBy; + + vA -= mA * P; + wA -= iA * LA; + + vB += mB * P; + wB += iB * LB; + } + + data.velocities[m_indexA].v = vA; + data.velocities[m_indexA].w = wA; + data.velocities[m_indexB].v = vB; + data.velocities[m_indexB].w = wB; } -bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data) +bool b2WheelJoint::SolvePositionConstraints(const b2SolverData & data) { - b2Vec2 cA = data.positions[m_indexA].c; - float32 aA = data.positions[m_indexA].a; - b2Vec2 cB = data.positions[m_indexB].c; - float32 aB = data.positions[m_indexB].a; + b2Vec2 cA = data.positions[m_indexA].c; + float32 aA = data.positions[m_indexA].a; + b2Vec2 cB = data.positions[m_indexB].c; + float32 aB = data.positions[m_indexB].a; - b2Rot qA(aA), qB(aB); + b2Rot qA(aA), qB(aB); - b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); - b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); - b2Vec2 d = (cB - cA) + rB - rA; + b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA); + b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB); + b2Vec2 d = (cB - cA) + rB - rA; - b2Vec2 ay = b2Mul(qA, m_localYAxisA); + b2Vec2 ay = b2Mul(qA, m_localYAxisA); - float32 sAy = b2Cross(d + rA, ay); - float32 sBy = b2Cross(rB, ay); + float32 sAy = b2Cross(d + rA, ay); + float32 sBy = b2Cross(rB, ay); - float32 C = b2Dot(d, ay); + float32 C = b2Dot(d, ay); - float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; + float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy; - float32 impulse; - if (k != 0.0f) - { - impulse = - C / k; - } - else - { - impulse = 0.0f; - } + float32 impulse; + if (k != 0.0f) { + impulse = -C / k; + } else { + impulse = 0.0f; + } - b2Vec2 P = impulse * ay; - float32 LA = impulse * sAy; - float32 LB = impulse * sBy; + b2Vec2 P = impulse * ay; + float32 LA = impulse * sAy; + float32 LB = impulse * sBy; - cA -= m_invMassA * P; - aA -= m_invIA * LA; - cB += m_invMassB * P; - aB += m_invIB * LB; + cA -= m_invMassA * P; + aA -= m_invIA * LA; + cB += m_invMassB * P; + aB += m_invIB * LB; - data.positions[m_indexA].c = cA; - data.positions[m_indexA].a = aA; - data.positions[m_indexB].c = cB; - data.positions[m_indexB].a = aB; + data.positions[m_indexA].c = cA; + data.positions[m_indexA].a = aA; + data.positions[m_indexB].c = cB; + data.positions[m_indexB].a = aB; - return b2Abs(C) <= b2_linearSlop; + return b2Abs(C) <= b2_linearSlop; } -b2Vec2 b2WheelJoint::GetAnchorA() const -{ - return m_bodyA->GetWorldPoint(m_localAnchorA); -} +b2Vec2 b2WheelJoint::GetAnchorA() const { return m_bodyA->GetWorldPoint(m_localAnchorA); } -b2Vec2 b2WheelJoint::GetAnchorB() const -{ - return m_bodyB->GetWorldPoint(m_localAnchorB); -} +b2Vec2 b2WheelJoint::GetAnchorB() const { return m_bodyB->GetWorldPoint(m_localAnchorB); } b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const { - return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax); + return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax); } -float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} +float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const { return inv_dt * m_motorImpulse; } float32 b2WheelJoint::GetJointTranslation() const { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; + b2Body * bA = m_bodyA; + b2Body * bB = m_bodyB; - b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA); - b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB); - b2Vec2 d = pB - pA; - b2Vec2 axis = bA->GetWorldVector(m_localXAxisA); + b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA); + b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB); + b2Vec2 d = pB - pA; + b2Vec2 axis = bA->GetWorldVector(m_localXAxisA); - float32 translation = b2Dot(d, axis); - return translation; + float32 translation = b2Dot(d, axis); + return translation; } float32 b2WheelJoint::GetJointLinearSpeed() const { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - - b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); - b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); - b2Vec2 p1 = bA->m_sweep.c + rA; - b2Vec2 p2 = bB->m_sweep.c + rB; - b2Vec2 d = p2 - p1; - b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); - - b2Vec2 vA = bA->m_linearVelocity; - b2Vec2 vB = bB->m_linearVelocity; - float32 wA = bA->m_angularVelocity; - float32 wB = bB->m_angularVelocity; - - float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); - return speed; + b2Body * bA = m_bodyA; + b2Body * bB = m_bodyB; + + b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter); + b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter); + b2Vec2 p1 = bA->m_sweep.c + rA; + b2Vec2 p2 = bB->m_sweep.c + rB; + b2Vec2 d = p2 - p1; + b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA); + + b2Vec2 vA = bA->m_linearVelocity; + b2Vec2 vB = bB->m_linearVelocity; + float32 wA = bA->m_angularVelocity; + float32 wB = bB->m_angularVelocity; + + float32 speed = + b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA)); + return speed; } float32 b2WheelJoint::GetJointAngle() const { - b2Body* bA = m_bodyA; - b2Body* bB = m_bodyB; - return bB->m_sweep.a - bA->m_sweep.a; + b2Body * bA = m_bodyA; + b2Body * bB = m_bodyB; + return bB->m_sweep.a - bA->m_sweep.a; } float32 b2WheelJoint::GetJointAngularSpeed() const { - float32 wA = m_bodyA->m_angularVelocity; - float32 wB = m_bodyB->m_angularVelocity; - return wB - wA; + float32 wA = m_bodyA->m_angularVelocity; + float32 wB = m_bodyB->m_angularVelocity; + return wB - wA; } -bool b2WheelJoint::IsMotorEnabled() const -{ - return m_enableMotor; -} +bool b2WheelJoint::IsMotorEnabled() const { return m_enableMotor; } void b2WheelJoint::EnableMotor(bool flag) { - if (flag != m_enableMotor) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_enableMotor = flag; - } + if (flag != m_enableMotor) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_enableMotor = flag; + } } void b2WheelJoint::SetMotorSpeed(float32 speed) { - if (speed != m_motorSpeed) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_motorSpeed = speed; - } + if (speed != m_motorSpeed) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_motorSpeed = speed; + } } void b2WheelJoint::SetMaxMotorTorque(float32 torque) { - if (torque != m_maxMotorTorque) - { - m_bodyA->SetAwake(true); - m_bodyB->SetAwake(true); - m_maxMotorTorque = torque; - } + if (torque != m_maxMotorTorque) { + m_bodyA->SetAwake(true); + m_bodyB->SetAwake(true); + m_maxMotorTorque = torque; + } } -float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const -{ - return inv_dt * m_motorImpulse; -} +float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const { return inv_dt * m_motorImpulse; } void b2WheelJoint::Dump() { - int32 indexA = m_bodyA->m_islandIndex; - int32 indexB = m_bodyB->m_islandIndex; - - b2Log(" b2WheelJointDef jd;\n"); - b2Log(" jd.bodyA = bodies[%d];\n", indexA); - b2Log(" jd.bodyB = bodies[%d];\n", indexB); - b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); - b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); - b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); - b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); - b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); - b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); - b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); - b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); - b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); - b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Log(" b2WheelJointDef jd;\n"); + b2Log(" jd.bodyA = bodies[%d];\n", indexA); + b2Log(" jd.bodyB = bodies[%d];\n", indexB); + b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y); + b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor); + b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed); + b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque); + b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz); + b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio); + b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); } diff --git a/flatland_box2d/src/Dynamics/b2Body.cpp b/flatland_box2d/src/Dynamics/b2Body.cpp index 3ed90a61..6453be85 100644 --- a/flatland_box2d/src/Dynamics/b2Body.cpp +++ b/flatland_box2d/src/Dynamics/b2Body.cpp @@ -1,554 +1,499 @@ /* -* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/b2Body.h" -#include "Box2D/Dynamics/b2Fixture.h" -#include "Box2D/Dynamics/b2World.h" + #include "Box2D/Dynamics/Contacts/b2Contact.h" #include "Box2D/Dynamics/Joints/b2Joint.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2World.h" -b2Body::b2Body(const b2BodyDef* bd, b2World* world) +b2Body::b2Body(const b2BodyDef * bd, b2World * world) { - b2Assert(bd->position.IsValid()); - b2Assert(bd->linearVelocity.IsValid()); - b2Assert(b2IsValid(bd->angle)); - b2Assert(b2IsValid(bd->angularVelocity)); - b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); - b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); - - m_flags = 0; - - if (bd->bullet) - { - m_flags |= e_bulletFlag; - } - if (bd->fixedRotation) - { - m_flags |= e_fixedRotationFlag; - } - if (bd->allowSleep) - { - m_flags |= e_autoSleepFlag; - } - if (bd->awake) - { - m_flags |= e_awakeFlag; - } - if (bd->active) - { - m_flags |= e_activeFlag; - } - - m_world = world; - - m_xf.p = bd->position; - m_xf.q.Set(bd->angle); - - m_sweep.localCenter.SetZero(); - m_sweep.c0 = m_xf.p; - m_sweep.c = m_xf.p; - m_sweep.a0 = bd->angle; - m_sweep.a = bd->angle; - m_sweep.alpha0 = 0.0f; - - m_jointList = nullptr; - m_contactList = nullptr; - m_prev = nullptr; - m_next = nullptr; - - m_linearVelocity = bd->linearVelocity; - m_angularVelocity = bd->angularVelocity; - - m_linearDamping = bd->linearDamping; - m_angularDamping = bd->angularDamping; - m_gravityScale = bd->gravityScale; - - m_force.SetZero(); - m_torque = 0.0f; - - m_sleepTime = 0.0f; - - m_type = bd->type; - - if (m_type == b2_dynamicBody) - { - m_mass = 1.0f; - m_invMass = 1.0f; - } - else - { - m_mass = 0.0f; - m_invMass = 0.0f; - } - - m_I = 0.0f; - m_invI = 0.0f; - - m_userData = bd->userData; - - m_fixtureList = nullptr; - m_fixtureCount = 0; + b2Assert(bd->position.IsValid()); + b2Assert(bd->linearVelocity.IsValid()); + b2Assert(b2IsValid(bd->angle)); + b2Assert(b2IsValid(bd->angularVelocity)); + b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); + b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); + + m_flags = 0; + + if (bd->bullet) { + m_flags |= e_bulletFlag; + } + if (bd->fixedRotation) { + m_flags |= e_fixedRotationFlag; + } + if (bd->allowSleep) { + m_flags |= e_autoSleepFlag; + } + if (bd->awake) { + m_flags |= e_awakeFlag; + } + if (bd->active) { + m_flags |= e_activeFlag; + } + + m_world = world; + + m_xf.p = bd->position; + m_xf.q.Set(bd->angle); + + m_sweep.localCenter.SetZero(); + m_sweep.c0 = m_xf.p; + m_sweep.c = m_xf.p; + m_sweep.a0 = bd->angle; + m_sweep.a = bd->angle; + m_sweep.alpha0 = 0.0f; + + m_jointList = nullptr; + m_contactList = nullptr; + m_prev = nullptr; + m_next = nullptr; + + m_linearVelocity = bd->linearVelocity; + m_angularVelocity = bd->angularVelocity; + + m_linearDamping = bd->linearDamping; + m_angularDamping = bd->angularDamping; + m_gravityScale = bd->gravityScale; + + m_force.SetZero(); + m_torque = 0.0f; + + m_sleepTime = 0.0f; + + m_type = bd->type; + + if (m_type == b2_dynamicBody) { + m_mass = 1.0f; + m_invMass = 1.0f; + } else { + m_mass = 0.0f; + m_invMass = 0.0f; + } + + m_I = 0.0f; + m_invI = 0.0f; + + m_userData = bd->userData; + + m_fixtureList = nullptr; + m_fixtureCount = 0; } b2Body::~b2Body() { - // shapes and joints are destroyed in b2World::Destroy + // shapes and joints are destroyed in b2World::Destroy } void b2Body::SetType(b2BodyType type) { - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - if (m_type == type) - { - return; - } - - m_type = type; - - ResetMassData(); - - if (m_type == b2_staticBody) - { - m_linearVelocity.SetZero(); - m_angularVelocity = 0.0f; - m_sweep.a0 = m_sweep.a; - m_sweep.c0 = m_sweep.c; - SynchronizeFixtures(); - } - - SetAwake(true); - - m_force.SetZero(); - m_torque = 0.0f; - - // Delete the attached contacts. - b2ContactEdge* ce = m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_world->m_contactManager.Destroy(ce0->contact); - } - m_contactList = nullptr; - - // Touch the proxies so that new contacts will be created (when appropriate) - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - int32 proxyCount = f->m_proxyCount; - for (int32 i = 0; i < proxyCount; ++i) - { - broadPhase->TouchProxy(f->m_proxies[i].proxyId); - } - } + b2Assert(m_world->IsLocked() == false); + if (m_world->IsLocked() == true) { + return; + } + + if (m_type == type) { + return; + } + + m_type = type; + + ResetMassData(); + + if (m_type == b2_staticBody) { + m_linearVelocity.SetZero(); + m_angularVelocity = 0.0f; + m_sweep.a0 = m_sweep.a; + m_sweep.c0 = m_sweep.c; + SynchronizeFixtures(); + } + + SetAwake(true); + + m_force.SetZero(); + m_torque = 0.0f; + + // Delete the attached contacts. + b2ContactEdge * ce = m_contactList; + while (ce) { + b2ContactEdge * ce0 = ce; + ce = ce->next; + m_world->m_contactManager.Destroy(ce0->contact); + } + m_contactList = nullptr; + + // Touch the proxies so that new contacts will be created (when appropriate) + b2BroadPhase * broadPhase = &m_world->m_contactManager.m_broadPhase; + for (b2Fixture * f = m_fixtureList; f; f = f->m_next) { + int32 proxyCount = f->m_proxyCount; + for (int32 i = 0; i < proxyCount; ++i) { + broadPhase->TouchProxy(f->m_proxies[i].proxyId); + } + } } -b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) +b2Fixture * b2Body::CreateFixture(const b2FixtureDef * def) { - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return nullptr; - } + b2Assert(m_world->IsLocked() == false); + if (m_world->IsLocked() == true) { + return nullptr; + } - b2BlockAllocator* allocator = &m_world->m_blockAllocator; + b2BlockAllocator * allocator = &m_world->m_blockAllocator; - void* memory = allocator->Allocate(sizeof(b2Fixture)); - b2Fixture* fixture = new (memory) b2Fixture; - fixture->Create(allocator, this, def); + void * memory = allocator->Allocate(sizeof(b2Fixture)); + b2Fixture * fixture = new (memory) b2Fixture; + fixture->Create(allocator, this, def); - if (m_flags & e_activeFlag) - { - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->CreateProxies(broadPhase, m_xf); - } + if (m_flags & e_activeFlag) { + b2BroadPhase * broadPhase = &m_world->m_contactManager.m_broadPhase; + fixture->CreateProxies(broadPhase, m_xf); + } - fixture->m_next = m_fixtureList; - m_fixtureList = fixture; - ++m_fixtureCount; + fixture->m_next = m_fixtureList; + m_fixtureList = fixture; + ++m_fixtureCount; - fixture->m_body = this; + fixture->m_body = this; - // Adjust mass properties if needed. - if (fixture->m_density > 0.0f) - { - ResetMassData(); - } + // Adjust mass properties if needed. + if (fixture->m_density > 0.0f) { + ResetMassData(); + } - // Let the world know we have a new fixture. This will cause new contacts - // to be created at the beginning of the next time step. - m_world->m_flags |= b2World::e_newFixture; + // Let the world know we have a new fixture. This will cause new contacts + // to be created at the beginning of the next time step. + m_world->m_flags |= b2World::e_newFixture; - return fixture; + return fixture; } -b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density) +b2Fixture * b2Body::CreateFixture(const b2Shape * shape, float32 density) { - b2FixtureDef def; - def.shape = shape; - def.density = density; + b2FixtureDef def; + def.shape = shape; + def.density = density; - return CreateFixture(&def); + return CreateFixture(&def); } -void b2Body::DestroyFixture(b2Fixture* fixture) +void b2Body::DestroyFixture(b2Fixture * fixture) { - if (fixture == NULL) - { - return; - } - - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - b2Assert(fixture->m_body == this); - - // Remove the fixture from this body's singly linked list. - b2Assert(m_fixtureCount > 0); - b2Fixture** node = &m_fixtureList; - bool found = false; - while (*node != nullptr) - { - if (*node == fixture) - { - *node = fixture->m_next; - found = true; - break; - } - - node = &(*node)->m_next; - } - - // You tried to remove a shape that is not attached to this body. - b2Assert(found); - - // Destroy any contacts associated with the fixture. - b2ContactEdge* edge = m_contactList; - while (edge) - { - b2Contact* c = edge->contact; - edge = edge->next; - - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - - if (fixture == fixtureA || fixture == fixtureB) - { - // This destroys the contact and removes it from - // this body's contact list. - m_world->m_contactManager.Destroy(c); - } - } - - b2BlockAllocator* allocator = &m_world->m_blockAllocator; - - if (m_flags & e_activeFlag) - { - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - fixture->DestroyProxies(broadPhase); - } - - fixture->m_body = nullptr; - fixture->m_next = nullptr; - fixture->Destroy(allocator); - fixture->~b2Fixture(); - allocator->Free(fixture, sizeof(b2Fixture)); - - --m_fixtureCount; - - // Reset the mass data. - ResetMassData(); + if (fixture == NULL) { + return; + } + + b2Assert(m_world->IsLocked() == false); + if (m_world->IsLocked() == true) { + return; + } + + b2Assert(fixture->m_body == this); + + // Remove the fixture from this body's singly linked list. + b2Assert(m_fixtureCount > 0); + b2Fixture ** node = &m_fixtureList; + bool found = false; + while (*node != nullptr) { + if (*node == fixture) { + *node = fixture->m_next; + found = true; + break; + } + + node = &(*node)->m_next; + } + + // You tried to remove a shape that is not attached to this body. + b2Assert(found); + + // Destroy any contacts associated with the fixture. + b2ContactEdge * edge = m_contactList; + while (edge) { + b2Contact * c = edge->contact; + edge = edge->next; + + b2Fixture * fixtureA = c->GetFixtureA(); + b2Fixture * fixtureB = c->GetFixtureB(); + + if (fixture == fixtureA || fixture == fixtureB) { + // This destroys the contact and removes it from + // this body's contact list. + m_world->m_contactManager.Destroy(c); + } + } + + b2BlockAllocator * allocator = &m_world->m_blockAllocator; + + if (m_flags & e_activeFlag) { + b2BroadPhase * broadPhase = &m_world->m_contactManager.m_broadPhase; + fixture->DestroyProxies(broadPhase); + } + + fixture->m_body = nullptr; + fixture->m_next = nullptr; + fixture->Destroy(allocator); + fixture->~b2Fixture(); + allocator->Free(fixture, sizeof(b2Fixture)); + + --m_fixtureCount; + + // Reset the mass data. + ResetMassData(); } void b2Body::ResetMassData() { - // Compute mass data from shapes. Each shape has its own density. - m_mass = 0.0f; - m_invMass = 0.0f; - m_I = 0.0f; - m_invI = 0.0f; - m_sweep.localCenter.SetZero(); - - // Static and kinematic bodies have zero mass. - if (m_type == b2_staticBody || m_type == b2_kinematicBody) - { - m_sweep.c0 = m_xf.p; - m_sweep.c = m_xf.p; - m_sweep.a0 = m_sweep.a; - return; - } - - b2Assert(m_type == b2_dynamicBody); - - // Accumulate mass over all fixtures. - b2Vec2 localCenter = b2Vec2_zero; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - if (f->m_density == 0.0f) - { - continue; - } - - b2MassData massData; - f->GetMassData(&massData); - m_mass += massData.mass; - localCenter += massData.mass * massData.center; - m_I += massData.I; - } - - // Compute center of mass. - if (m_mass > 0.0f) - { - m_invMass = 1.0f / m_mass; - localCenter *= m_invMass; - } - else - { - // Force all dynamic bodies to have a positive mass. - m_mass = 1.0f; - m_invMass = 1.0f; - } - - if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) - { - // Center the inertia about the center of mass. - m_I -= m_mass * b2Dot(localCenter, localCenter); - b2Assert(m_I > 0.0f); - m_invI = 1.0f / m_I; - - } - else - { - m_I = 0.0f; - m_invI = 0.0f; - } - - // Move center of mass. - b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = localCenter; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - - // Update center of mass velocity. - m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); + // Compute mass data from shapes. Each shape has its own density. + m_mass = 0.0f; + m_invMass = 0.0f; + m_I = 0.0f; + m_invI = 0.0f; + m_sweep.localCenter.SetZero(); + + // Static and kinematic bodies have zero mass. + if (m_type == b2_staticBody || m_type == b2_kinematicBody) { + m_sweep.c0 = m_xf.p; + m_sweep.c = m_xf.p; + m_sweep.a0 = m_sweep.a; + return; + } + + b2Assert(m_type == b2_dynamicBody); + + // Accumulate mass over all fixtures. + b2Vec2 localCenter = b2Vec2_zero; + for (b2Fixture * f = m_fixtureList; f; f = f->m_next) { + if (f->m_density == 0.0f) { + continue; + } + + b2MassData massData; + f->GetMassData(&massData); + m_mass += massData.mass; + localCenter += massData.mass * massData.center; + m_I += massData.I; + } + + // Compute center of mass. + if (m_mass > 0.0f) { + m_invMass = 1.0f / m_mass; + localCenter *= m_invMass; + } else { + // Force all dynamic bodies to have a positive mass. + m_mass = 1.0f; + m_invMass = 1.0f; + } + + if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) { + // Center the inertia about the center of mass. + m_I -= m_mass * b2Dot(localCenter, localCenter); + b2Assert(m_I > 0.0f); + m_invI = 1.0f / m_I; + + } else { + m_I = 0.0f; + m_invI = 0.0f; + } + + // Move center of mass. + b2Vec2 oldCenter = m_sweep.c; + m_sweep.localCenter = localCenter; + m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); + + // Update center of mass velocity. + m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); } -void b2Body::SetMassData(const b2MassData* massData) +void b2Body::SetMassData(const b2MassData * massData) { - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - if (m_type != b2_dynamicBody) - { - return; - } - - m_invMass = 0.0f; - m_I = 0.0f; - m_invI = 0.0f; - - m_mass = massData->mass; - if (m_mass <= 0.0f) - { - m_mass = 1.0f; - } - - m_invMass = 1.0f / m_mass; - - if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0) - { - m_I = massData->I - m_mass * b2Dot(massData->center, massData->center); - b2Assert(m_I > 0.0f); - m_invI = 1.0f / m_I; - } - - // Move center of mass. - b2Vec2 oldCenter = m_sweep.c; - m_sweep.localCenter = massData->center; - m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - - // Update center of mass velocity. - m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); + b2Assert(m_world->IsLocked() == false); + if (m_world->IsLocked() == true) { + return; + } + + if (m_type != b2_dynamicBody) { + return; + } + + m_invMass = 0.0f; + m_I = 0.0f; + m_invI = 0.0f; + + m_mass = massData->mass; + if (m_mass <= 0.0f) { + m_mass = 1.0f; + } + + m_invMass = 1.0f / m_mass; + + if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0) { + m_I = massData->I - m_mass * b2Dot(massData->center, massData->center); + b2Assert(m_I > 0.0f); + m_invI = 1.0f / m_I; + } + + // Move center of mass. + b2Vec2 oldCenter = m_sweep.c; + m_sweep.localCenter = massData->center; + m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); + + // Update center of mass velocity. + m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); } -bool b2Body::ShouldCollide(const b2Body* other) const +bool b2Body::ShouldCollide(const b2Body * other) const { - // At least one body should be dynamic. - if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody) - { - return false; - } - - // Does a joint prevent collision? - for (b2JointEdge* jn = m_jointList; jn; jn = jn->next) - { - if (jn->other == other) - { - if (jn->joint->m_collideConnected == false) - { - return false; - } - } - } - - return true; + // At least one body should be dynamic. + if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody) { + return false; + } + + // Does a joint prevent collision? + for (b2JointEdge * jn = m_jointList; jn; jn = jn->next) { + if (jn->other == other) { + if (jn->joint->m_collideConnected == false) { + return false; + } + } + } + + return true; } -void b2Body::SetTransform(const b2Vec2& position, float32 angle) +void b2Body::SetTransform(const b2Vec2 & position, float32 angle) { - b2Assert(m_world->IsLocked() == false); - if (m_world->IsLocked() == true) - { - return; - } - - m_xf.q.Set(angle); - m_xf.p = position; - - m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); - m_sweep.a = angle; - - m_sweep.c0 = m_sweep.c; - m_sweep.a0 = angle; - - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->Synchronize(broadPhase, m_xf, m_xf); - } + b2Assert(m_world->IsLocked() == false); + if (m_world->IsLocked() == true) { + return; + } + + m_xf.q.Set(angle); + m_xf.p = position; + + m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); + m_sweep.a = angle; + + m_sweep.c0 = m_sweep.c; + m_sweep.a0 = angle; + + b2BroadPhase * broadPhase = &m_world->m_contactManager.m_broadPhase; + for (b2Fixture * f = m_fixtureList; f; f = f->m_next) { + f->Synchronize(broadPhase, m_xf, m_xf); + } } void b2Body::SynchronizeFixtures() { - b2Transform xf1; - xf1.q.Set(m_sweep.a0); - xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter); - - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->Synchronize(broadPhase, xf1, m_xf); - } + b2Transform xf1; + xf1.q.Set(m_sweep.a0); + xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter); + + b2BroadPhase * broadPhase = &m_world->m_contactManager.m_broadPhase; + for (b2Fixture * f = m_fixtureList; f; f = f->m_next) { + f->Synchronize(broadPhase, xf1, m_xf); + } } void b2Body::SetActive(bool flag) { - b2Assert(m_world->IsLocked() == false); - - if (flag == IsActive()) - { - return; - } - - if (flag) - { - m_flags |= e_activeFlag; - - // Create all proxies. - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->CreateProxies(broadPhase, m_xf); - } - - // Contacts are created the next time step. - } - else - { - m_flags &= ~e_activeFlag; - - // Destroy all proxies. - b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - f->DestroyProxies(broadPhase); - } - - // Destroy the attached contacts. - b2ContactEdge* ce = m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_world->m_contactManager.Destroy(ce0->contact); - } - m_contactList = nullptr; - } + b2Assert(m_world->IsLocked() == false); + + if (flag == IsActive()) { + return; + } + + if (flag) { + m_flags |= e_activeFlag; + + // Create all proxies. + b2BroadPhase * broadPhase = &m_world->m_contactManager.m_broadPhase; + for (b2Fixture * f = m_fixtureList; f; f = f->m_next) { + f->CreateProxies(broadPhase, m_xf); + } + + // Contacts are created the next time step. + } else { + m_flags &= ~e_activeFlag; + + // Destroy all proxies. + b2BroadPhase * broadPhase = &m_world->m_contactManager.m_broadPhase; + for (b2Fixture * f = m_fixtureList; f; f = f->m_next) { + f->DestroyProxies(broadPhase); + } + + // Destroy the attached contacts. + b2ContactEdge * ce = m_contactList; + while (ce) { + b2ContactEdge * ce0 = ce; + ce = ce->next; + m_world->m_contactManager.Destroy(ce0->contact); + } + m_contactList = nullptr; + } } void b2Body::SetFixedRotation(bool flag) { - bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; - if (status == flag) - { - return; - } - - if (flag) - { - m_flags |= e_fixedRotationFlag; - } - else - { - m_flags &= ~e_fixedRotationFlag; - } - - m_angularVelocity = 0.0f; - - ResetMassData(); + bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; + if (status == flag) { + return; + } + + if (flag) { + m_flags |= e_fixedRotationFlag; + } else { + m_flags &= ~e_fixedRotationFlag; + } + + m_angularVelocity = 0.0f; + + ResetMassData(); } void b2Body::Dump() { - int32 bodyIndex = m_islandIndex; - - b2Log("{\n"); - b2Log(" b2BodyDef bd;\n"); - b2Log(" bd.type = b2BodyType(%d);\n", m_type); - b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y); - b2Log(" bd.angle = %.15lef;\n", m_sweep.a); - b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y); - b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity); - b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping); - b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping); - b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag); - b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag); - b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag); - b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag); - b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag); - b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale); - b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex); - b2Log("\n"); - for (b2Fixture* f = m_fixtureList; f; f = f->m_next) - { - b2Log(" {\n"); - f->Dump(bodyIndex); - b2Log(" }\n"); - } - b2Log("}\n"); + int32 bodyIndex = m_islandIndex; + + b2Log("{\n"); + b2Log(" b2BodyDef bd;\n"); + b2Log(" bd.type = b2BodyType(%d);\n", m_type); + b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y); + b2Log(" bd.angle = %.15lef;\n", m_sweep.a); + b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y); + b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity); + b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping); + b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping); + b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag); + b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag); + b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag); + b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag); + b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag); + b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale); + b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex); + b2Log("\n"); + for (b2Fixture * f = m_fixtureList; f; f = f->m_next) { + b2Log(" {\n"); + f->Dump(bodyIndex); + b2Log(" }\n"); + } + b2Log("}\n"); } diff --git a/flatland_box2d/src/Dynamics/b2ContactManager.cpp b/flatland_box2d/src/Dynamics/b2ContactManager.cpp index 2e58da88..d5beb60e 100644 --- a/flatland_box2d/src/Dynamics/b2ContactManager.cpp +++ b/flatland_box2d/src/Dynamics/b2ContactManager.cpp @@ -1,102 +1,93 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/b2ContactManager.h" + +#include "Box2D/Dynamics/Contacts/b2Contact.h" #include "Box2D/Dynamics/b2Body.h" #include "Box2D/Dynamics/b2Fixture.h" #include "Box2D/Dynamics/b2WorldCallbacks.h" -#include "Box2D/Dynamics/Contacts/b2Contact.h" b2ContactFilter b2_defaultFilter; b2ContactListener b2_defaultListener; b2ContactManager::b2ContactManager() { - m_contactList = nullptr; - m_contactCount = 0; - m_contactFilter = &b2_defaultFilter; - m_contactListener = &b2_defaultListener; - m_allocator = nullptr; + m_contactList = nullptr; + m_contactCount = 0; + m_contactFilter = &b2_defaultFilter; + m_contactListener = &b2_defaultListener; + m_allocator = nullptr; } -void b2ContactManager::Destroy(b2Contact* c) +void b2ContactManager::Destroy(b2Contact * c) { - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - if (m_contactListener && c->IsTouching()) - { - m_contactListener->EndContact(c); - } - - // Remove from the world. - if (c->m_prev) - { - c->m_prev->m_next = c->m_next; - } - - if (c->m_next) - { - c->m_next->m_prev = c->m_prev; - } - - if (c == m_contactList) - { - m_contactList = c->m_next; - } - - // Remove from body 1 - if (c->m_nodeA.prev) - { - c->m_nodeA.prev->next = c->m_nodeA.next; - } - - if (c->m_nodeA.next) - { - c->m_nodeA.next->prev = c->m_nodeA.prev; - } - - if (&c->m_nodeA == bodyA->m_contactList) - { - bodyA->m_contactList = c->m_nodeA.next; - } - - // Remove from body 2 - if (c->m_nodeB.prev) - { - c->m_nodeB.prev->next = c->m_nodeB.next; - } - - if (c->m_nodeB.next) - { - c->m_nodeB.next->prev = c->m_nodeB.prev; - } - - if (&c->m_nodeB == bodyB->m_contactList) - { - bodyB->m_contactList = c->m_nodeB.next; - } - - // Call the factory. - b2Contact::Destroy(c, m_allocator); - --m_contactCount; + b2Fixture * fixtureA = c->GetFixtureA(); + b2Fixture * fixtureB = c->GetFixtureB(); + b2Body * bodyA = fixtureA->GetBody(); + b2Body * bodyB = fixtureB->GetBody(); + + if (m_contactListener && c->IsTouching()) { + m_contactListener->EndContact(c); + } + + // Remove from the world. + if (c->m_prev) { + c->m_prev->m_next = c->m_next; + } + + if (c->m_next) { + c->m_next->m_prev = c->m_prev; + } + + if (c == m_contactList) { + m_contactList = c->m_next; + } + + // Remove from body 1 + if (c->m_nodeA.prev) { + c->m_nodeA.prev->next = c->m_nodeA.next; + } + + if (c->m_nodeA.next) { + c->m_nodeA.next->prev = c->m_nodeA.prev; + } + + if (&c->m_nodeA == bodyA->m_contactList) { + bodyA->m_contactList = c->m_nodeA.next; + } + + // Remove from body 2 + if (c->m_nodeB.prev) { + c->m_nodeB.prev->next = c->m_nodeB.next; + } + + if (c->m_nodeB.next) { + c->m_nodeB.next->prev = c->m_nodeB.prev; + } + + if (&c->m_nodeB == bodyB->m_contactList) { + bodyB->m_contactList = c->m_nodeB.next; + } + + // Call the factory. + b2Contact::Destroy(c, m_allocator); + --m_contactCount; } // This is the top level collision call for the time step. Here @@ -104,193 +95,172 @@ void b2ContactManager::Destroy(b2Contact* c) // contact list. void b2ContactManager::Collide() { - // Update awake contacts. - b2Contact* c = m_contactList; - while (c) - { - b2Fixture* fixtureA = c->GetFixtureA(); - b2Fixture* fixtureB = c->GetFixtureB(); - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - // Is this contact flagged for filtering? - if (c->m_flags & b2Contact::e_filterFlag) - { - // Should these bodies collide? - if (bodyB->ShouldCollide(bodyA) == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // Check user filtering. - if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // Clear the filtering flag. - c->m_flags &= ~b2Contact::e_filterFlag; - } - - bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody; - bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody; - - // At least one body must be awake and it must be dynamic or kinematic. - if (activeA == false && activeB == false) - { - c = c->GetNext(); - continue; - } - - int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId; - int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId; - bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); - - // Here we destroy contacts that cease to overlap in the broad-phase. - if (overlap == false) - { - b2Contact* cNuke = c; - c = cNuke->GetNext(); - Destroy(cNuke); - continue; - } - - // The contact persists. - c->Update(m_contactListener); - c = c->GetNext(); - } + // Update awake contacts. + b2Contact * c = m_contactList; + while (c) { + b2Fixture * fixtureA = c->GetFixtureA(); + b2Fixture * fixtureB = c->GetFixtureB(); + int32 indexA = c->GetChildIndexA(); + int32 indexB = c->GetChildIndexB(); + b2Body * bodyA = fixtureA->GetBody(); + b2Body * bodyB = fixtureB->GetBody(); + + // Is this contact flagged for filtering? + if (c->m_flags & b2Contact::e_filterFlag) { + // Should these bodies collide? + if (bodyB->ShouldCollide(bodyA) == false) { + b2Contact * cNuke = c; + c = cNuke->GetNext(); + Destroy(cNuke); + continue; + } + + // Check user filtering. + if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) { + b2Contact * cNuke = c; + c = cNuke->GetNext(); + Destroy(cNuke); + continue; + } + + // Clear the filtering flag. + c->m_flags &= ~b2Contact::e_filterFlag; + } + + bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody; + bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody; + + // At least one body must be awake and it must be dynamic or kinematic. + if (activeA == false && activeB == false) { + c = c->GetNext(); + continue; + } + + int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId; + int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId; + bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB); + + // Here we destroy contacts that cease to overlap in the broad-phase. + if (overlap == false) { + b2Contact * cNuke = c; + c = cNuke->GetNext(); + Destroy(cNuke); + continue; + } + + // The contact persists. + c->Update(m_contactListener); + c = c->GetNext(); + } } -void b2ContactManager::FindNewContacts() -{ - m_broadPhase.UpdatePairs(this); -} +void b2ContactManager::FindNewContacts() { m_broadPhase.UpdatePairs(this); } -void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB) +void b2ContactManager::AddPair(void * proxyUserDataA, void * proxyUserDataB) { - b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA; - b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB; - - b2Fixture* fixtureA = proxyA->fixture; - b2Fixture* fixtureB = proxyB->fixture; - - int32 indexA = proxyA->childIndex; - int32 indexB = proxyB->childIndex; - - b2Body* bodyA = fixtureA->GetBody(); - b2Body* bodyB = fixtureB->GetBody(); - - // Are the fixtures on the same body? - if (bodyA == bodyB) - { - return; - } - - // TODO_ERIN use a hash table to remove a potential bottleneck when both - // bodies have a lot of contacts. - // Does a contact already exist? - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - b2Fixture* fA = edge->contact->GetFixtureA(); - b2Fixture* fB = edge->contact->GetFixtureB(); - int32 iA = edge->contact->GetChildIndexA(); - int32 iB = edge->contact->GetChildIndexB(); - - if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) - { - // A contact already exists. - return; - } - - if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) - { - // A contact already exists. - return; - } - } - - edge = edge->next; - } - - // Does a joint override collision? Is at least one body dynamic? - if (bodyB->ShouldCollide(bodyA) == false) - { - return; - } - - // Check user filtering. - if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) - { - return; - } - - // Call the factory. - b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator); - if (c == nullptr) - { - return; - } - - // Contact creation may swap fixtures. - fixtureA = c->GetFixtureA(); - fixtureB = c->GetFixtureB(); - indexA = c->GetChildIndexA(); - indexB = c->GetChildIndexB(); - bodyA = fixtureA->GetBody(); - bodyB = fixtureB->GetBody(); - - // Insert into the world. - c->m_prev = nullptr; - c->m_next = m_contactList; - if (m_contactList != nullptr) - { - m_contactList->m_prev = c; - } - m_contactList = c; - - // Connect to island graph. - - // Connect to body A - c->m_nodeA.contact = c; - c->m_nodeA.other = bodyB; - - c->m_nodeA.prev = nullptr; - c->m_nodeA.next = bodyA->m_contactList; - if (bodyA->m_contactList != nullptr) - { - bodyA->m_contactList->prev = &c->m_nodeA; - } - bodyA->m_contactList = &c->m_nodeA; - - // Connect to body B - c->m_nodeB.contact = c; - c->m_nodeB.other = bodyA; - - c->m_nodeB.prev = nullptr; - c->m_nodeB.next = bodyB->m_contactList; - if (bodyB->m_contactList != nullptr) - { - bodyB->m_contactList->prev = &c->m_nodeB; - } - bodyB->m_contactList = &c->m_nodeB; - - // Wake up the bodies - if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false) - { - bodyA->SetAwake(true); - bodyB->SetAwake(true); - } - - ++m_contactCount; + b2FixtureProxy * proxyA = (b2FixtureProxy *)proxyUserDataA; + b2FixtureProxy * proxyB = (b2FixtureProxy *)proxyUserDataB; + + b2Fixture * fixtureA = proxyA->fixture; + b2Fixture * fixtureB = proxyB->fixture; + + int32 indexA = proxyA->childIndex; + int32 indexB = proxyB->childIndex; + + b2Body * bodyA = fixtureA->GetBody(); + b2Body * bodyB = fixtureB->GetBody(); + + // Are the fixtures on the same body? + if (bodyA == bodyB) { + return; + } + + // TODO_ERIN use a hash table to remove a potential bottleneck when both + // bodies have a lot of contacts. + // Does a contact already exist? + b2ContactEdge * edge = bodyB->GetContactList(); + while (edge) { + if (edge->other == bodyA) { + b2Fixture * fA = edge->contact->GetFixtureA(); + b2Fixture * fB = edge->contact->GetFixtureB(); + int32 iA = edge->contact->GetChildIndexA(); + int32 iB = edge->contact->GetChildIndexB(); + + if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) { + // A contact already exists. + return; + } + + if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) { + // A contact already exists. + return; + } + } + + edge = edge->next; + } + + // Does a joint override collision? Is at least one body dynamic? + if (bodyB->ShouldCollide(bodyA) == false) { + return; + } + + // Check user filtering. + if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false) { + return; + } + + // Call the factory. + b2Contact * c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator); + if (c == nullptr) { + return; + } + + // Contact creation may swap fixtures. + fixtureA = c->GetFixtureA(); + fixtureB = c->GetFixtureB(); + indexA = c->GetChildIndexA(); + indexB = c->GetChildIndexB(); + bodyA = fixtureA->GetBody(); + bodyB = fixtureB->GetBody(); + + // Insert into the world. + c->m_prev = nullptr; + c->m_next = m_contactList; + if (m_contactList != nullptr) { + m_contactList->m_prev = c; + } + m_contactList = c; + + // Connect to island graph. + + // Connect to body A + c->m_nodeA.contact = c; + c->m_nodeA.other = bodyB; + + c->m_nodeA.prev = nullptr; + c->m_nodeA.next = bodyA->m_contactList; + if (bodyA->m_contactList != nullptr) { + bodyA->m_contactList->prev = &c->m_nodeA; + } + bodyA->m_contactList = &c->m_nodeA; + + // Connect to body B + c->m_nodeB.contact = c; + c->m_nodeB.other = bodyA; + + c->m_nodeB.prev = nullptr; + c->m_nodeB.next = bodyB->m_contactList; + if (bodyB->m_contactList != nullptr) { + bodyB->m_contactList->prev = &c->m_nodeB; + } + bodyB->m_contactList = &c->m_nodeB; + + // Wake up the bodies + if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false) { + bodyA->SetAwake(true); + bodyB->SetAwake(true); + } + + ++m_contactCount; } diff --git a/flatland_box2d/src/Dynamics/b2Fixture.cpp b/flatland_box2d/src/Dynamics/b2Fixture.cpp index a5c1ba9d..63202d2f 100644 --- a/flatland_box2d/src/Dynamics/b2Fixture.cpp +++ b/flatland_box2d/src/Dynamics/b2Fixture.cpp @@ -1,303 +1,277 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/b2Fixture.h" -#include "Box2D/Dynamics/Contacts/b2Contact.h" -#include "Box2D/Dynamics/b2World.h" + +#include "Box2D/Collision/Shapes/b2ChainShape.h" #include "Box2D/Collision/Shapes/b2CircleShape.h" #include "Box2D/Collision/Shapes/b2EdgeShape.h" #include "Box2D/Collision/Shapes/b2PolygonShape.h" -#include "Box2D/Collision/Shapes/b2ChainShape.h" #include "Box2D/Collision/b2BroadPhase.h" #include "Box2D/Collision/b2Collision.h" #include "Box2D/Common/b2BlockAllocator.h" +#include "Box2D/Dynamics/Contacts/b2Contact.h" +#include "Box2D/Dynamics/b2World.h" b2Fixture::b2Fixture() { - m_userData = nullptr; - m_body = nullptr; - m_next = nullptr; - m_proxies = nullptr; - m_proxyCount = 0; - m_shape = nullptr; - m_density = 0.0f; + m_userData = nullptr; + m_body = nullptr; + m_next = nullptr; + m_proxies = nullptr; + m_proxyCount = 0; + m_shape = nullptr; + m_density = 0.0f; } -void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def) +void b2Fixture::Create(b2BlockAllocator * allocator, b2Body * body, const b2FixtureDef * def) { - m_userData = def->userData; - m_friction = def->friction; - m_restitution = def->restitution; + m_userData = def->userData; + m_friction = def->friction; + m_restitution = def->restitution; - m_body = body; - m_next = nullptr; + m_body = body; + m_next = nullptr; - m_filter = def->filter; + m_filter = def->filter; - m_isSensor = def->isSensor; + m_isSensor = def->isSensor; - m_shape = def->shape->Clone(allocator); + m_shape = def->shape->Clone(allocator); - // Reserve proxy space - int32 childCount = m_shape->GetChildCount(); - m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy)); - for (int32 i = 0; i < childCount; ++i) - { - m_proxies[i].fixture = nullptr; - m_proxies[i].proxyId = b2BroadPhase::e_nullProxy; - } - m_proxyCount = 0; + // Reserve proxy space + int32 childCount = m_shape->GetChildCount(); + m_proxies = (b2FixtureProxy *)allocator->Allocate(childCount * sizeof(b2FixtureProxy)); + for (int32 i = 0; i < childCount; ++i) { + m_proxies[i].fixture = nullptr; + m_proxies[i].proxyId = b2BroadPhase::e_nullProxy; + } + m_proxyCount = 0; - m_density = def->density; + m_density = def->density; } -void b2Fixture::Destroy(b2BlockAllocator* allocator) +void b2Fixture::Destroy(b2BlockAllocator * allocator) { - // The proxies must be destroyed before calling this. - b2Assert(m_proxyCount == 0); - - // Free the proxy array. - int32 childCount = m_shape->GetChildCount(); - allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy)); - m_proxies = nullptr; - - // Free the child shape. - switch (m_shape->m_type) - { - case b2Shape::e_circle: - { - b2CircleShape* s = (b2CircleShape*)m_shape; - s->~b2CircleShape(); - allocator->Free(s, sizeof(b2CircleShape)); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* s = (b2EdgeShape*)m_shape; - s->~b2EdgeShape(); - allocator->Free(s, sizeof(b2EdgeShape)); - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* s = (b2PolygonShape*)m_shape; - s->~b2PolygonShape(); - allocator->Free(s, sizeof(b2PolygonShape)); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* s = (b2ChainShape*)m_shape; - s->~b2ChainShape(); - allocator->Free(s, sizeof(b2ChainShape)); - } - break; - - default: - b2Assert(false); - break; - } - - m_shape = nullptr; + // The proxies must be destroyed before calling this. + b2Assert(m_proxyCount == 0); + + // Free the proxy array. + int32 childCount = m_shape->GetChildCount(); + allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy)); + m_proxies = nullptr; + + // Free the child shape. + switch (m_shape->m_type) { + case b2Shape::e_circle: { + b2CircleShape * s = (b2CircleShape *)m_shape; + s->~b2CircleShape(); + allocator->Free(s, sizeof(b2CircleShape)); + } break; + + case b2Shape::e_edge: { + b2EdgeShape * s = (b2EdgeShape *)m_shape; + s->~b2EdgeShape(); + allocator->Free(s, sizeof(b2EdgeShape)); + } break; + + case b2Shape::e_polygon: { + b2PolygonShape * s = (b2PolygonShape *)m_shape; + s->~b2PolygonShape(); + allocator->Free(s, sizeof(b2PolygonShape)); + } break; + + case b2Shape::e_chain: { + b2ChainShape * s = (b2ChainShape *)m_shape; + s->~b2ChainShape(); + allocator->Free(s, sizeof(b2ChainShape)); + } break; + + default: + b2Assert(false); + break; + } + + m_shape = nullptr; } -void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf) +void b2Fixture::CreateProxies(b2BroadPhase * broadPhase, const b2Transform & xf) { - b2Assert(m_proxyCount == 0); - - // Create proxies in the broad-phase. - m_proxyCount = m_shape->GetChildCount(); - - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - m_shape->ComputeAABB(&proxy->aabb, xf, i); - proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy); - proxy->fixture = this; - proxy->childIndex = i; - } + b2Assert(m_proxyCount == 0); + + // Create proxies in the broad-phase. + m_proxyCount = m_shape->GetChildCount(); + + for (int32 i = 0; i < m_proxyCount; ++i) { + b2FixtureProxy * proxy = m_proxies + i; + m_shape->ComputeAABB(&proxy->aabb, xf, i); + proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy); + proxy->fixture = this; + proxy->childIndex = i; + } } -void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase) +void b2Fixture::DestroyProxies(b2BroadPhase * broadPhase) { - // Destroy proxies in the broad-phase. - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - broadPhase->DestroyProxy(proxy->proxyId); - proxy->proxyId = b2BroadPhase::e_nullProxy; - } - - m_proxyCount = 0; + // Destroy proxies in the broad-phase. + for (int32 i = 0; i < m_proxyCount; ++i) { + b2FixtureProxy * proxy = m_proxies + i; + broadPhase->DestroyProxy(proxy->proxyId); + proxy->proxyId = b2BroadPhase::e_nullProxy; + } + + m_proxyCount = 0; } -void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2) +void b2Fixture::Synchronize( + b2BroadPhase * broadPhase, const b2Transform & transform1, const b2Transform & transform2) { - if (m_proxyCount == 0) - { - return; - } - - for (int32 i = 0; i < m_proxyCount; ++i) - { - b2FixtureProxy* proxy = m_proxies + i; - - // Compute an AABB that covers the swept shape (may miss some rotation effect). - b2AABB aabb1, aabb2; - m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex); - m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex); - - proxy->aabb.Combine(aabb1, aabb2); - - b2Vec2 displacement = transform2.p - transform1.p; - - broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement); - } + if (m_proxyCount == 0) { + return; + } + + for (int32 i = 0; i < m_proxyCount; ++i) { + b2FixtureProxy * proxy = m_proxies + i; + + // Compute an AABB that covers the swept shape (may miss some rotation + // effect). + b2AABB aabb1, aabb2; + m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex); + m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex); + + proxy->aabb.Combine(aabb1, aabb2); + + b2Vec2 displacement = transform2.p - transform1.p; + + broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement); + } } -void b2Fixture::SetFilterData(const b2Filter& filter) +void b2Fixture::SetFilterData(const b2Filter & filter) { - m_filter = filter; + m_filter = filter; - Refilter(); + Refilter(); } void b2Fixture::Refilter() { - if (m_body == nullptr) - { - return; - } - - // Flag associated contacts for filtering. - b2ContactEdge* edge = m_body->GetContactList(); - while (edge) - { - b2Contact* contact = edge->contact; - b2Fixture* fixtureA = contact->GetFixtureA(); - b2Fixture* fixtureB = contact->GetFixtureB(); - if (fixtureA == this || fixtureB == this) - { - contact->FlagForFiltering(); - } - - edge = edge->next; - } - - b2World* world = m_body->GetWorld(); - - if (world == nullptr) - { - return; - } - - // Touch each proxy so that new pairs may be created - b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase; - for (int32 i = 0; i < m_proxyCount; ++i) - { - broadPhase->TouchProxy(m_proxies[i].proxyId); - } + if (m_body == nullptr) { + return; + } + + // Flag associated contacts for filtering. + b2ContactEdge * edge = m_body->GetContactList(); + while (edge) { + b2Contact * contact = edge->contact; + b2Fixture * fixtureA = contact->GetFixtureA(); + b2Fixture * fixtureB = contact->GetFixtureB(); + if (fixtureA == this || fixtureB == this) { + contact->FlagForFiltering(); + } + + edge = edge->next; + } + + b2World * world = m_body->GetWorld(); + + if (world == nullptr) { + return; + } + + // Touch each proxy so that new pairs may be created + b2BroadPhase * broadPhase = &world->m_contactManager.m_broadPhase; + for (int32 i = 0; i < m_proxyCount; ++i) { + broadPhase->TouchProxy(m_proxies[i].proxyId); + } } void b2Fixture::SetSensor(bool sensor) { - if (sensor != m_isSensor) - { - m_body->SetAwake(true); - m_isSensor = sensor; - } + if (sensor != m_isSensor) { + m_body->SetAwake(true); + m_isSensor = sensor; + } } void b2Fixture::Dump(int32 bodyIndex) { - b2Log(" b2FixtureDef fd;\n"); - b2Log(" fd.friction = %.15lef;\n", m_friction); - b2Log(" fd.restitution = %.15lef;\n", m_restitution); - b2Log(" fd.density = %.15lef;\n", m_density); - b2Log(" fd.isSensor = bool(%d);\n", m_isSensor); - b2Log(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits); - b2Log(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits); - b2Log(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex); - - switch (m_shape->m_type) - { - case b2Shape::e_circle: - { - b2CircleShape* s = (b2CircleShape*)m_shape; - b2Log(" b2CircleShape shape;\n"); - b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); - b2Log(" shape.m_p.Set(%.15lef, %.15lef);\n", s->m_p.x, s->m_p.y); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* s = (b2EdgeShape*)m_shape; - b2Log(" b2EdgeShape shape;\n"); - b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); - b2Log(" shape.m_vertex0.Set(%.15lef, %.15lef);\n", s->m_vertex0.x, s->m_vertex0.y); - b2Log(" shape.m_vertex1.Set(%.15lef, %.15lef);\n", s->m_vertex1.x, s->m_vertex1.y); - b2Log(" shape.m_vertex2.Set(%.15lef, %.15lef);\n", s->m_vertex2.x, s->m_vertex2.y); - b2Log(" shape.m_vertex3.Set(%.15lef, %.15lef);\n", s->m_vertex3.x, s->m_vertex3.y); - b2Log(" shape.m_hasVertex0 = bool(%d);\n", s->m_hasVertex0); - b2Log(" shape.m_hasVertex3 = bool(%d);\n", s->m_hasVertex3); - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* s = (b2PolygonShape*)m_shape; - b2Log(" b2PolygonShape shape;\n"); - b2Log(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices); - for (int32 i = 0; i < s->m_count; ++i) - { - b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); - } - b2Log(" shape.Set(vs, %d);\n", s->m_count); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* s = (b2ChainShape*)m_shape; - b2Log(" b2ChainShape shape;\n"); - b2Log(" b2Vec2 vs[%d];\n", s->m_count); - for (int32 i = 0; i < s->m_count; ++i) - { - b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); - } - b2Log(" shape.CreateChain(vs, %d);\n", s->m_count); - b2Log(" shape.m_prevVertex.Set(%.15lef, %.15lef);\n", s->m_prevVertex.x, s->m_prevVertex.y); - b2Log(" shape.m_nextVertex.Set(%.15lef, %.15lef);\n", s->m_nextVertex.x, s->m_nextVertex.y); - b2Log(" shape.m_hasPrevVertex = bool(%d);\n", s->m_hasPrevVertex); - b2Log(" shape.m_hasNextVertex = bool(%d);\n", s->m_hasNextVertex); - } - break; - - default: - return; - } - - b2Log("\n"); - b2Log(" fd.shape = &shape;\n"); - b2Log("\n"); - b2Log(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex); + b2Log(" b2FixtureDef fd;\n"); + b2Log(" fd.friction = %.15lef;\n", m_friction); + b2Log(" fd.restitution = %.15lef;\n", m_restitution); + b2Log(" fd.density = %.15lef;\n", m_density); + b2Log(" fd.isSensor = bool(%d);\n", m_isSensor); + b2Log(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits); + b2Log(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits); + b2Log(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex); + + switch (m_shape->m_type) { + case b2Shape::e_circle: { + b2CircleShape * s = (b2CircleShape *)m_shape; + b2Log(" b2CircleShape shape;\n"); + b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); + b2Log(" shape.m_p.Set(%.15lef, %.15lef);\n", s->m_p.x, s->m_p.y); + } break; + + case b2Shape::e_edge: { + b2EdgeShape * s = (b2EdgeShape *)m_shape; + b2Log(" b2EdgeShape shape;\n"); + b2Log(" shape.m_radius = %.15lef;\n", s->m_radius); + b2Log(" shape.m_vertex0.Set(%.15lef, %.15lef);\n", s->m_vertex0.x, s->m_vertex0.y); + b2Log(" shape.m_vertex1.Set(%.15lef, %.15lef);\n", s->m_vertex1.x, s->m_vertex1.y); + b2Log(" shape.m_vertex2.Set(%.15lef, %.15lef);\n", s->m_vertex2.x, s->m_vertex2.y); + b2Log(" shape.m_vertex3.Set(%.15lef, %.15lef);\n", s->m_vertex3.x, s->m_vertex3.y); + b2Log(" shape.m_hasVertex0 = bool(%d);\n", s->m_hasVertex0); + b2Log(" shape.m_hasVertex3 = bool(%d);\n", s->m_hasVertex3); + } break; + + case b2Shape::e_polygon: { + b2PolygonShape * s = (b2PolygonShape *)m_shape; + b2Log(" b2PolygonShape shape;\n"); + b2Log(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices); + for (int32 i = 0; i < s->m_count; ++i) { + b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); + } + b2Log(" shape.Set(vs, %d);\n", s->m_count); + } break; + + case b2Shape::e_chain: { + b2ChainShape * s = (b2ChainShape *)m_shape; + b2Log(" b2ChainShape shape;\n"); + b2Log(" b2Vec2 vs[%d];\n", s->m_count); + for (int32 i = 0; i < s->m_count; ++i) { + b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y); + } + b2Log(" shape.CreateChain(vs, %d);\n", s->m_count); + b2Log( + " shape.m_prevVertex.Set(%.15lef, %.15lef);\n", s->m_prevVertex.x, s->m_prevVertex.y); + b2Log( + " shape.m_nextVertex.Set(%.15lef, %.15lef);\n", s->m_nextVertex.x, s->m_nextVertex.y); + b2Log(" shape.m_hasPrevVertex = bool(%d);\n", s->m_hasPrevVertex); + b2Log(" shape.m_hasNextVertex = bool(%d);\n", s->m_hasNextVertex); + } break; + + default: + return; + } + + b2Log("\n"); + b2Log(" fd.shape = &shape;\n"); + b2Log("\n"); + b2Log(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex); } diff --git a/flatland_box2d/src/Dynamics/b2Island.cpp b/flatland_box2d/src/Dynamics/b2Island.cpp index 8f57a4fb..155f8f0a 100644 --- a/flatland_box2d/src/Dynamics/b2Island.cpp +++ b/flatland_box2d/src/Dynamics/b2Island.cpp @@ -1,45 +1,47 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ -#include "Box2D/Collision/b2Distance.h" #include "Box2D/Dynamics/b2Island.h" -#include "Box2D/Dynamics/b2Body.h" -#include "Box2D/Dynamics/b2Fixture.h" -#include "Box2D/Dynamics/b2World.h" + +#include "Box2D/Collision/b2Distance.h" +#include "Box2D/Common/b2StackAllocator.h" +#include "Box2D/Common/b2Timer.h" #include "Box2D/Dynamics/Contacts/b2Contact.h" #include "Box2D/Dynamics/Contacts/b2ContactSolver.h" #include "Box2D/Dynamics/Joints/b2Joint.h" -#include "Box2D/Common/b2StackAllocator.h" -#include "Box2D/Common/b2Timer.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2World.h" /* Position Correction Notes ========================= I tried the several algorithms for position correction of the 2D revolute joint. I looked at these systems: -- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s. +- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular +velocity of 100 rad/s. - suspension bridge with 30 1m long planks of length 1m. - multi-link chain with 30 1m long links. Here are the algorithms: -Baumgarte - A fraction of the position error is added to the velocity error. There is no -separate position solver. +Baumgarte - A fraction of the position error is added to the velocity error. +There is no separate position solver. Pseudo Velocities - After the velocity solver and position integration, the position error, Jacobian, and effective mass are recomputed. Then @@ -53,27 +55,28 @@ Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the position error is re-computed for each constraint and the positions are updated after the constraint is solved. The radius vectors (aka Jacobians) are re-computed too (otherwise the algorithm has horrible instability). The pseudo -velocity states are not needed because they are effectively zero at the beginning -of each iteration. Since we have the current position error, we allow the -iterations to terminate early if the error becomes smaller than b2_linearSlop. +velocity states are not needed because they are effectively zero at the +beginning of each iteration. Since we have the current position error, we allow +the iterations to terminate early if the error becomes smaller than +b2_linearSlop. -Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed -each time a constraint is solved. +Full NGS or just NGS - Like Modified NGS except the effective mass are +re-computed each time a constraint is solved. Here are the results: Baumgarte - this is the cheapest algorithm but it has some stability problems, especially with the bridge. The chain links separate easily close to the root -and they jitter as they struggle to pull together. This is one of the most common -methods in the field. The big drawback is that the position correction artificially -affects the momentum, thus leading to instabilities and false bounce. I used a -bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller -factor makes joints and contacts more spongy. +and they jitter as they struggle to pull together. This is one of the most +common methods in the field. The big drawback is that the position correction +artificially affects the momentum, thus leading to instabilities and false +bounce. I used a bias factor of 0.2. A larger bias factor makes the bridge less +stable, a smaller factor makes joints and contacts more spongy. Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is stable. However, joints still separate with large angular velocities. Drag the -simple pendulum in a circle quickly and the joint will separate. The chain separates -easily and does not recover. I used a bias factor of 0.2. A larger value lead to -the bridge collapsing when a heavy cube drops on it. +simple pendulum in a circle quickly and the joint will separate. The chain +separates easily and does not recover. I used a bias factor of 0.2. A larger +value lead to the bridge collapsing when a heavy cube drops on it. Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo Velocities, but in other ways it is worse. The bridge and chain are much more @@ -84,26 +87,29 @@ still sags, but this is better than infinite forces. Recommendations Pseudo Velocities are not really worthwhile because the bridge and chain cannot -recover from joint separation. In other cases the benefit over Baumgarte is small. +recover from joint separation. In other cases the benefit over Baumgarte is +small. Modified NGS is not a robust method for the revolute joint due to the violent -instability seen in the simple pendulum. Perhaps it is viable with other constraint -types, especially scalar constraints where the effective mass is a scalar. +instability seen in the simple pendulum. Perhaps it is viable with other +constraint types, especially scalar constraints where the effective mass is a +scalar. -This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities -and is very fast. I don't think we can escape Baumgarte, especially in highly -demanding cases where high constraint fidelity is not needed. +This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable +instabilities and is very fast. I don't think we can escape Baumgarte, +especially in highly demanding cases where high constraint fidelity is not +needed. Full NGS is robust and easy on the eyes. I recommend this as an option for higher fidelity simulation and certainly for suspension bridges and long chains. -Full NGS might be a good choice for ragdolls, especially motorized ragdolls where -joint separation can be problematic. The number of NGS iterations can be reduced -for better performance without harming robustness much. - -Each joint in a can be handled differently in the position solver. So I recommend -a system where the user can select the algorithm on a per joint basis. I would -probably default to the slower Full NGS and let the user select the faster -Baumgarte method in performance critical scenarios. +Full NGS might be a good choice for ragdolls, especially motorized ragdolls +where joint separation can be problematic. The number of NGS iterations can be +reduced for better performance without harming robustness much. + +Each joint in a can be handled differently in the position solver. So I +recommend a system where the user can select the algorithm on a per joint basis. +I would probably default to the slower Full NGS and let the user select the +faster Baumgarte method in performance critical scenarios. */ /* @@ -115,11 +121,12 @@ to body data. The constraint structures are iterated over linearly, which leads to few cache misses. The bodies are not accessed during iteration. Instead read only data, such as -the mass values are stored with the constraints. The mutable data are the constraint -impulses and the bodies velocities/positions. The impulses are held inside the -constraint structures. The body velocities/positions are held in compact, temporary -arrays to increase the number of cache hits. Linear and angular velocity are -stored in a single array since multiple arrays lead to multiple misses. +the mass values are stored with the constraints. The mutable data are the +constraint impulses and the bodies velocities/positions. The impulses are held +inside the constraint structures. The body velocities/positions are held in +compact, temporary arrays to increase the number of cache hits. Linear and +angular velocity are stored in a single array since multiple arrays lead to +multiple misses. */ /* @@ -146,274 +153,248 @@ However, we can compute sin+cos of the same angle fast. */ b2Island::b2Island( - int32 bodyCapacity, - int32 contactCapacity, - int32 jointCapacity, - b2StackAllocator* allocator, - b2ContactListener* listener) + int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity, b2StackAllocator * allocator, + b2ContactListener * listener) { - m_bodyCapacity = bodyCapacity; - m_contactCapacity = contactCapacity; - m_jointCapacity = jointCapacity; - m_bodyCount = 0; - m_contactCount = 0; - m_jointCount = 0; - - m_allocator = allocator; - m_listener = listener; - - m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*)); - m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*)); - m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*)); - - m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity)); - m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position)); + m_bodyCapacity = bodyCapacity; + m_contactCapacity = contactCapacity; + m_jointCapacity = jointCapacity; + m_bodyCount = 0; + m_contactCount = 0; + m_jointCount = 0; + + m_allocator = allocator; + m_listener = listener; + + m_bodies = (b2Body **)m_allocator->Allocate(bodyCapacity * sizeof(b2Body *)); + m_contacts = (b2Contact **)m_allocator->Allocate(contactCapacity * sizeof(b2Contact *)); + m_joints = (b2Joint **)m_allocator->Allocate(jointCapacity * sizeof(b2Joint *)); + + m_velocities = (b2Velocity *)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity)); + m_positions = (b2Position *)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position)); } b2Island::~b2Island() { - // Warning: the order should reverse the constructor order. - m_allocator->Free(m_positions); - m_allocator->Free(m_velocities); - m_allocator->Free(m_joints); - m_allocator->Free(m_contacts); - m_allocator->Free(m_bodies); + // Warning: the order should reverse the constructor order. + m_allocator->Free(m_positions); + m_allocator->Free(m_velocities); + m_allocator->Free(m_joints); + m_allocator->Free(m_contacts); + m_allocator->Free(m_bodies); } -void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep) +void b2Island::Solve( + b2Profile * profile, const b2TimeStep & step, const b2Vec2 & gravity, bool allowSleep) { - b2Timer timer; - - float32 h = step.dt; - - // Integrate velocities and apply damping. Initialize the body state. - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - - b2Vec2 c = b->m_sweep.c; - float32 a = b->m_sweep.a; - b2Vec2 v = b->m_linearVelocity; - float32 w = b->m_angularVelocity; - - // Store positions for continuous collision. - b->m_sweep.c0 = b->m_sweep.c; - b->m_sweep.a0 = b->m_sweep.a; - - if (b->m_type == b2_dynamicBody) - { - // Integrate velocities. - v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force); - w += h * b->m_invI * b->m_torque; - - // Apply damping. - // ODE: dv/dt + c * v = 0 - // Solution: v(t) = v0 * exp(-c * t) - // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) - // v2 = exp(-c * dt) * v1 - // Pade approximation: - // v2 = v1 * 1 / (1 + c * dt) - v *= 1.0f / (1.0f + h * b->m_linearDamping); - w *= 1.0f / (1.0f + h * b->m_angularDamping); - } - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - } - - timer.Reset(); - - // Solver data - b2SolverData solverData; - solverData.step = step; - solverData.positions = m_positions; - solverData.velocities = m_velocities; - - // Initialize velocity constraints. - b2ContactSolverDef contactSolverDef; - contactSolverDef.step = step; - contactSolverDef.contacts = m_contacts; - contactSolverDef.count = m_contactCount; - contactSolverDef.positions = m_positions; - contactSolverDef.velocities = m_velocities; - contactSolverDef.allocator = m_allocator; - - b2ContactSolver contactSolver(&contactSolverDef); - contactSolver.InitializeVelocityConstraints(); - - if (step.warmStarting) - { - contactSolver.WarmStart(); - } - - for (int32 i = 0; i < m_jointCount; ++i) - { - m_joints[i]->InitVelocityConstraints(solverData); - } - - profile->solveInit = timer.GetMilliseconds(); - - // Solve velocity constraints - timer.Reset(); - for (int32 i = 0; i < step.velocityIterations; ++i) - { - for (int32 j = 0; j < m_jointCount; ++j) - { - m_joints[j]->SolveVelocityConstraints(solverData); - } - - contactSolver.SolveVelocityConstraints(); - } - - // Store impulses for warm starting - contactSolver.StoreImpulses(); - profile->solveVelocity = timer.GetMilliseconds(); - - // Integrate positions - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Vec2 c = m_positions[i].c; - float32 a = m_positions[i].a; - b2Vec2 v = m_velocities[i].v; - float32 w = m_velocities[i].w; - - // Check for large velocities - b2Vec2 translation = h * v; - if (b2Dot(translation, translation) > b2_maxTranslationSquared) - { - float32 ratio = b2_maxTranslation / translation.Length(); - v *= ratio; - } - - float32 rotation = h * w; - if (rotation * rotation > b2_maxRotationSquared) - { - float32 ratio = b2_maxRotation / b2Abs(rotation); - w *= ratio; - } - - // Integrate - c += h * v; - a += h * w; - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - } - - // Solve position constraints - timer.Reset(); - bool positionSolved = false; - for (int32 i = 0; i < step.positionIterations; ++i) - { - bool contactsOkay = contactSolver.SolvePositionConstraints(); - - bool jointsOkay = true; - for (int32 j = 0; j < m_jointCount; ++j) - { - bool jointOkay = m_joints[j]->SolvePositionConstraints(solverData); - jointsOkay = jointsOkay && jointOkay; - } - - if (contactsOkay && jointsOkay) - { - // Exit early if the position errors are small. - positionSolved = true; - break; - } - } - - // Copy state buffers back to the bodies - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* body = m_bodies[i]; - body->m_sweep.c = m_positions[i].c; - body->m_sweep.a = m_positions[i].a; - body->m_linearVelocity = m_velocities[i].v; - body->m_angularVelocity = m_velocities[i].w; - body->SynchronizeTransform(); - } - - profile->solvePosition = timer.GetMilliseconds(); - - Report(contactSolver.m_velocityConstraints); - - if (allowSleep) - { - float32 minSleepTime = b2_maxFloat; - - const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance; - const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance; - - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - if (b->GetType() == b2_staticBody) - { - continue; - } - - if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 || - b->m_angularVelocity * b->m_angularVelocity > angTolSqr || - b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr) - { - b->m_sleepTime = 0.0f; - minSleepTime = 0.0f; - } - else - { - b->m_sleepTime += h; - minSleepTime = b2Min(minSleepTime, b->m_sleepTime); - } - } - - if (minSleepTime >= b2_timeToSleep && positionSolved) - { - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - b->SetAwake(false); - } - } - } + b2Timer timer; + + float32 h = step.dt; + + // Integrate velocities and apply damping. Initialize the body state. + for (int32 i = 0; i < m_bodyCount; ++i) { + b2Body * b = m_bodies[i]; + + b2Vec2 c = b->m_sweep.c; + float32 a = b->m_sweep.a; + b2Vec2 v = b->m_linearVelocity; + float32 w = b->m_angularVelocity; + + // Store positions for continuous collision. + b->m_sweep.c0 = b->m_sweep.c; + b->m_sweep.a0 = b->m_sweep.a; + + if (b->m_type == b2_dynamicBody) { + // Integrate velocities. + v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force); + w += h * b->m_invI * b->m_torque; + + // Apply damping. + // ODE: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * + // exp(-c * dt) = v * exp(-c * dt) v2 = exp(-c * dt) * v1 Pade + // approximation: v2 = v1 * 1 / (1 + c * dt) + v *= 1.0f / (1.0f + h * b->m_linearDamping); + w *= 1.0f / (1.0f + h * b->m_angularDamping); + } + + m_positions[i].c = c; + m_positions[i].a = a; + m_velocities[i].v = v; + m_velocities[i].w = w; + } + + timer.Reset(); + + // Solver data + b2SolverData solverData; + solverData.step = step; + solverData.positions = m_positions; + solverData.velocities = m_velocities; + + // Initialize velocity constraints. + b2ContactSolverDef contactSolverDef; + contactSolverDef.step = step; + contactSolverDef.contacts = m_contacts; + contactSolverDef.count = m_contactCount; + contactSolverDef.positions = m_positions; + contactSolverDef.velocities = m_velocities; + contactSolverDef.allocator = m_allocator; + + b2ContactSolver contactSolver(&contactSolverDef); + contactSolver.InitializeVelocityConstraints(); + + if (step.warmStarting) { + contactSolver.WarmStart(); + } + + for (int32 i = 0; i < m_jointCount; ++i) { + m_joints[i]->InitVelocityConstraints(solverData); + } + + profile->solveInit = timer.GetMilliseconds(); + + // Solve velocity constraints + timer.Reset(); + for (int32 i = 0; i < step.velocityIterations; ++i) { + for (int32 j = 0; j < m_jointCount; ++j) { + m_joints[j]->SolveVelocityConstraints(solverData); + } + + contactSolver.SolveVelocityConstraints(); + } + + // Store impulses for warm starting + contactSolver.StoreImpulses(); + profile->solveVelocity = timer.GetMilliseconds(); + + // Integrate positions + for (int32 i = 0; i < m_bodyCount; ++i) { + b2Vec2 c = m_positions[i].c; + float32 a = m_positions[i].a; + b2Vec2 v = m_velocities[i].v; + float32 w = m_velocities[i].w; + + // Check for large velocities + b2Vec2 translation = h * v; + if (b2Dot(translation, translation) > b2_maxTranslationSquared) { + float32 ratio = b2_maxTranslation / translation.Length(); + v *= ratio; + } + + float32 rotation = h * w; + if (rotation * rotation > b2_maxRotationSquared) { + float32 ratio = b2_maxRotation / b2Abs(rotation); + w *= ratio; + } + + // Integrate + c += h * v; + a += h * w; + + m_positions[i].c = c; + m_positions[i].a = a; + m_velocities[i].v = v; + m_velocities[i].w = w; + } + + // Solve position constraints + timer.Reset(); + bool positionSolved = false; + for (int32 i = 0; i < step.positionIterations; ++i) { + bool contactsOkay = contactSolver.SolvePositionConstraints(); + + bool jointsOkay = true; + for (int32 j = 0; j < m_jointCount; ++j) { + bool jointOkay = m_joints[j]->SolvePositionConstraints(solverData); + jointsOkay = jointsOkay && jointOkay; + } + + if (contactsOkay && jointsOkay) { + // Exit early if the position errors are small. + positionSolved = true; + break; + } + } + + // Copy state buffers back to the bodies + for (int32 i = 0; i < m_bodyCount; ++i) { + b2Body * body = m_bodies[i]; + body->m_sweep.c = m_positions[i].c; + body->m_sweep.a = m_positions[i].a; + body->m_linearVelocity = m_velocities[i].v; + body->m_angularVelocity = m_velocities[i].w; + body->SynchronizeTransform(); + } + + profile->solvePosition = timer.GetMilliseconds(); + + Report(contactSolver.m_velocityConstraints); + + if (allowSleep) { + float32 minSleepTime = b2_maxFloat; + + const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance; + const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance; + + for (int32 i = 0; i < m_bodyCount; ++i) { + b2Body * b = m_bodies[i]; + if (b->GetType() == b2_staticBody) { + continue; + } + + if ( + (b->m_flags & b2Body::e_autoSleepFlag) == 0 || + b->m_angularVelocity * b->m_angularVelocity > angTolSqr || + b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr) { + b->m_sleepTime = 0.0f; + minSleepTime = 0.0f; + } else { + b->m_sleepTime += h; + minSleepTime = b2Min(minSleepTime, b->m_sleepTime); + } + } + + if (minSleepTime >= b2_timeToSleep && positionSolved) { + for (int32 i = 0; i < m_bodyCount; ++i) { + b2Body * b = m_bodies[i]; + b->SetAwake(false); + } + } + } } -void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB) +void b2Island::SolveTOI(const b2TimeStep & subStep, int32 toiIndexA, int32 toiIndexB) { - b2Assert(toiIndexA < m_bodyCount); - b2Assert(toiIndexB < m_bodyCount); - - // Initialize the body state. - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Body* b = m_bodies[i]; - m_positions[i].c = b->m_sweep.c; - m_positions[i].a = b->m_sweep.a; - m_velocities[i].v = b->m_linearVelocity; - m_velocities[i].w = b->m_angularVelocity; - } - - b2ContactSolverDef contactSolverDef; - contactSolverDef.contacts = m_contacts; - contactSolverDef.count = m_contactCount; - contactSolverDef.allocator = m_allocator; - contactSolverDef.step = subStep; - contactSolverDef.positions = m_positions; - contactSolverDef.velocities = m_velocities; - b2ContactSolver contactSolver(&contactSolverDef); - - // Solve position constraints. - for (int32 i = 0; i < subStep.positionIterations; ++i) - { - bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); - if (contactsOkay) - { - break; - } - } + b2Assert(toiIndexA < m_bodyCount); + b2Assert(toiIndexB < m_bodyCount); + + // Initialize the body state. + for (int32 i = 0; i < m_bodyCount; ++i) { + b2Body * b = m_bodies[i]; + m_positions[i].c = b->m_sweep.c; + m_positions[i].a = b->m_sweep.a; + m_velocities[i].v = b->m_linearVelocity; + m_velocities[i].w = b->m_angularVelocity; + } + + b2ContactSolverDef contactSolverDef; + contactSolverDef.contacts = m_contacts; + contactSolverDef.count = m_contactCount; + contactSolverDef.allocator = m_allocator; + contactSolverDef.step = subStep; + contactSolverDef.positions = m_positions; + contactSolverDef.velocities = m_velocities; + b2ContactSolver contactSolver(&contactSolverDef); + + // Solve position constraints. + for (int32 i = 0; i < subStep.positionIterations; ++i) { + bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); + if (contactsOkay) { + break; + } + } #if 0 // Is the new position really safe? @@ -448,92 +429,85 @@ void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiInd } #endif - // Leap of faith to new safe state. - m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c; - m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a; - m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c; - m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a; - - // No warm starting is needed for TOI events because warm - // starting impulses were applied in the discrete solver. - contactSolver.InitializeVelocityConstraints(); - - // Solve velocity constraints. - for (int32 i = 0; i < subStep.velocityIterations; ++i) - { - contactSolver.SolveVelocityConstraints(); - } - - // Don't store the TOI contact forces for warm starting - // because they can be quite large. - - float32 h = subStep.dt; - - // Integrate positions - for (int32 i = 0; i < m_bodyCount; ++i) - { - b2Vec2 c = m_positions[i].c; - float32 a = m_positions[i].a; - b2Vec2 v = m_velocities[i].v; - float32 w = m_velocities[i].w; - - // Check for large velocities - b2Vec2 translation = h * v; - if (b2Dot(translation, translation) > b2_maxTranslationSquared) - { - float32 ratio = b2_maxTranslation / translation.Length(); - v *= ratio; - } - - float32 rotation = h * w; - if (rotation * rotation > b2_maxRotationSquared) - { - float32 ratio = b2_maxRotation / b2Abs(rotation); - w *= ratio; - } - - // Integrate - c += h * v; - a += h * w; - - m_positions[i].c = c; - m_positions[i].a = a; - m_velocities[i].v = v; - m_velocities[i].w = w; - - // Sync bodies - b2Body* body = m_bodies[i]; - body->m_sweep.c = c; - body->m_sweep.a = a; - body->m_linearVelocity = v; - body->m_angularVelocity = w; - body->SynchronizeTransform(); - } - - Report(contactSolver.m_velocityConstraints); + // Leap of faith to new safe state. + m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c; + m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a; + m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c; + m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a; + + // No warm starting is needed for TOI events because warm + // starting impulses were applied in the discrete solver. + contactSolver.InitializeVelocityConstraints(); + + // Solve velocity constraints. + for (int32 i = 0; i < subStep.velocityIterations; ++i) { + contactSolver.SolveVelocityConstraints(); + } + + // Don't store the TOI contact forces for warm starting + // because they can be quite large. + + float32 h = subStep.dt; + + // Integrate positions + for (int32 i = 0; i < m_bodyCount; ++i) { + b2Vec2 c = m_positions[i].c; + float32 a = m_positions[i].a; + b2Vec2 v = m_velocities[i].v; + float32 w = m_velocities[i].w; + + // Check for large velocities + b2Vec2 translation = h * v; + if (b2Dot(translation, translation) > b2_maxTranslationSquared) { + float32 ratio = b2_maxTranslation / translation.Length(); + v *= ratio; + } + + float32 rotation = h * w; + if (rotation * rotation > b2_maxRotationSquared) { + float32 ratio = b2_maxRotation / b2Abs(rotation); + w *= ratio; + } + + // Integrate + c += h * v; + a += h * w; + + m_positions[i].c = c; + m_positions[i].a = a; + m_velocities[i].v = v; + m_velocities[i].w = w; + + // Sync bodies + b2Body * body = m_bodies[i]; + body->m_sweep.c = c; + body->m_sweep.a = a; + body->m_linearVelocity = v; + body->m_angularVelocity = w; + body->SynchronizeTransform(); + } + + Report(contactSolver.m_velocityConstraints); } -void b2Island::Report(const b2ContactVelocityConstraint* constraints) +void b2Island::Report(const b2ContactVelocityConstraint * constraints) { - if (m_listener == nullptr) - { - return; - } + if (m_listener == nullptr) { + return; + } - for (int32 i = 0; i < m_contactCount; ++i) - { - b2Contact* c = m_contacts[i]; + for (int32 i = 0; i < m_contactCount; ++i) { + b2Contact * c = m_contacts[i]; - const b2ContactVelocityConstraint* vc = constraints + i; - - b2ContactImpulse impulse; - impulse.count = vc->pointCount; - for (int32 j = 0; j < vc->pointCount; ++j) - { - impulse.normalImpulses[j] = vc->points[j].normalImpulse; - impulse.tangentImpulses[j] = vc->points[j].tangentImpulse; - } + const b2ContactVelocityConstraint * vc = constraints + i; - m_listener->PostSolve(c, &impulse); - } + b2ContactImpulse impulse; + impulse.count = vc->pointCount; + for (int32 j = 0; j < vc->pointCount; ++j) { + impulse.normalImpulses[j] = vc->points[j].normalImpulse; + impulse.tangentImpulses[j] = vc->points[j].tangentImpulse; + } + + m_listener->PostSolve(c, &impulse); + } } diff --git a/flatland_box2d/src/Dynamics/b2World.cpp b/flatland_box2d/src/Dynamics/b2World.cpp index 201d5160..4b9be2a9 100644 --- a/flatland_box2d/src/Dynamics/b2World.cpp +++ b/flatland_box2d/src/Dynamics/b2World.cpp @@ -1,1357 +1,1189 @@ /* -* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2011 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/b2World.h" -#include "Box2D/Dynamics/b2Body.h" -#include "Box2D/Dynamics/b2Fixture.h" -#include "Box2D/Dynamics/b2Island.h" -#include "Box2D/Dynamics/Joints/b2PulleyJoint.h" -#include "Box2D/Dynamics/Contacts/b2Contact.h" -#include "Box2D/Dynamics/Contacts/b2ContactSolver.h" -#include "Box2D/Collision/b2Collision.h" -#include "Box2D/Collision/b2BroadPhase.h" + +#include + +#include "Box2D/Collision/Shapes/b2ChainShape.h" #include "Box2D/Collision/Shapes/b2CircleShape.h" #include "Box2D/Collision/Shapes/b2EdgeShape.h" -#include "Box2D/Collision/Shapes/b2ChainShape.h" #include "Box2D/Collision/Shapes/b2PolygonShape.h" +#include "Box2D/Collision/b2BroadPhase.h" +#include "Box2D/Collision/b2Collision.h" #include "Box2D/Collision/b2TimeOfImpact.h" #include "Box2D/Common/b2Draw.h" #include "Box2D/Common/b2Timer.h" -#include +#include "Box2D/Dynamics/Contacts/b2Contact.h" +#include "Box2D/Dynamics/Contacts/b2ContactSolver.h" +#include "Box2D/Dynamics/Joints/b2PulleyJoint.h" +#include "Box2D/Dynamics/b2Body.h" +#include "Box2D/Dynamics/b2Fixture.h" +#include "Box2D/Dynamics/b2Island.h" -b2World::b2World(const b2Vec2& gravity) +b2World::b2World(const b2Vec2 & gravity) { - m_destructionListener = nullptr; - g_debugDraw = nullptr; + m_destructionListener = nullptr; + g_debugDraw = nullptr; - m_bodyList = nullptr; - m_jointList = nullptr; + m_bodyList = nullptr; + m_jointList = nullptr; - m_bodyCount = 0; - m_jointCount = 0; + m_bodyCount = 0; + m_jointCount = 0; - m_warmStarting = true; - m_continuousPhysics = true; - m_subStepping = false; + m_warmStarting = true; + m_continuousPhysics = true; + m_subStepping = false; - m_stepComplete = true; + m_stepComplete = true; - m_allowSleep = true; - m_gravity = gravity; + m_allowSleep = true; + m_gravity = gravity; - m_flags = e_clearForces; + m_flags = e_clearForces; - m_inv_dt0 = 0.0f; + m_inv_dt0 = 0.0f; - m_contactManager.m_allocator = &m_blockAllocator; + m_contactManager.m_allocator = &m_blockAllocator; - memset(&m_profile, 0, sizeof(b2Profile)); + memset(&m_profile, 0, sizeof(b2Profile)); } b2World::~b2World() { - // Some shapes allocate using b2Alloc. - b2Body* b = m_bodyList; - while (b) - { - b2Body* bNext = b->m_next; - - b2Fixture* f = b->m_fixtureList; - while (f) - { - b2Fixture* fNext = f->m_next; - f->m_proxyCount = 0; - f->Destroy(&m_blockAllocator); - f = fNext; - } - - b = bNext; - } + // Some shapes allocate using b2Alloc. + b2Body * b = m_bodyList; + while (b) { + b2Body * bNext = b->m_next; + + b2Fixture * f = b->m_fixtureList; + while (f) { + b2Fixture * fNext = f->m_next; + f->m_proxyCount = 0; + f->Destroy(&m_blockAllocator); + f = fNext; + } + + b = bNext; + } } -void b2World::SetDestructionListener(b2DestructionListener* listener) +void b2World::SetDestructionListener(b2DestructionListener * listener) { - m_destructionListener = listener; + m_destructionListener = listener; } -void b2World::SetContactFilter(b2ContactFilter* filter) +void b2World::SetContactFilter(b2ContactFilter * filter) { - m_contactManager.m_contactFilter = filter; + m_contactManager.m_contactFilter = filter; } -void b2World::SetContactListener(b2ContactListener* listener) +void b2World::SetContactListener(b2ContactListener * listener) { - m_contactManager.m_contactListener = listener; + m_contactManager.m_contactListener = listener; } -void b2World::SetDebugDraw(b2Draw* debugDraw) -{ - g_debugDraw = debugDraw; -} +void b2World::SetDebugDraw(b2Draw * debugDraw) { g_debugDraw = debugDraw; } -b2Body* b2World::CreateBody(const b2BodyDef* def) +b2Body * b2World::CreateBody(const b2BodyDef * def) { - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return nullptr; - } - - void* mem = m_blockAllocator.Allocate(sizeof(b2Body)); - b2Body* b = new (mem) b2Body(def, this); - - // Add to world doubly linked list. - b->m_prev = nullptr; - b->m_next = m_bodyList; - if (m_bodyList) - { - m_bodyList->m_prev = b; - } - m_bodyList = b; - ++m_bodyCount; - - return b; + b2Assert(IsLocked() == false); + if (IsLocked()) { + return nullptr; + } + + void * mem = m_blockAllocator.Allocate(sizeof(b2Body)); + b2Body * b = new (mem) b2Body(def, this); + + // Add to world doubly linked list. + b->m_prev = nullptr; + b->m_next = m_bodyList; + if (m_bodyList) { + m_bodyList->m_prev = b; + } + m_bodyList = b; + ++m_bodyCount; + + return b; } -void b2World::DestroyBody(b2Body* b) +void b2World::DestroyBody(b2Body * b) { - b2Assert(m_bodyCount > 0); - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return; - } - - // Delete the attached joints. - b2JointEdge* je = b->m_jointList; - while (je) - { - b2JointEdge* je0 = je; - je = je->next; - - if (m_destructionListener) - { - m_destructionListener->SayGoodbye(je0->joint); - } - - DestroyJoint(je0->joint); - - b->m_jointList = je; - } - b->m_jointList = nullptr; - - // Delete the attached contacts. - b2ContactEdge* ce = b->m_contactList; - while (ce) - { - b2ContactEdge* ce0 = ce; - ce = ce->next; - m_contactManager.Destroy(ce0->contact); - } - b->m_contactList = nullptr; - - // Delete the attached fixtures. This destroys broad-phase proxies. - b2Fixture* f = b->m_fixtureList; - while (f) - { - b2Fixture* f0 = f; - f = f->m_next; - - if (m_destructionListener) - { - m_destructionListener->SayGoodbye(f0); - } - - f0->DestroyProxies(&m_contactManager.m_broadPhase); - f0->Destroy(&m_blockAllocator); - f0->~b2Fixture(); - m_blockAllocator.Free(f0, sizeof(b2Fixture)); - - b->m_fixtureList = f; - b->m_fixtureCount -= 1; - } - b->m_fixtureList = nullptr; - b->m_fixtureCount = 0; - - // Remove world body list. - if (b->m_prev) - { - b->m_prev->m_next = b->m_next; - } - - if (b->m_next) - { - b->m_next->m_prev = b->m_prev; - } - - if (b == m_bodyList) - { - m_bodyList = b->m_next; - } - - --m_bodyCount; - b->~b2Body(); - m_blockAllocator.Free(b, sizeof(b2Body)); + b2Assert(m_bodyCount > 0); + b2Assert(IsLocked() == false); + if (IsLocked()) { + return; + } + + // Delete the attached joints. + b2JointEdge * je = b->m_jointList; + while (je) { + b2JointEdge * je0 = je; + je = je->next; + + if (m_destructionListener) { + m_destructionListener->SayGoodbye(je0->joint); + } + + DestroyJoint(je0->joint); + + b->m_jointList = je; + } + b->m_jointList = nullptr; + + // Delete the attached contacts. + b2ContactEdge * ce = b->m_contactList; + while (ce) { + b2ContactEdge * ce0 = ce; + ce = ce->next; + m_contactManager.Destroy(ce0->contact); + } + b->m_contactList = nullptr; + + // Delete the attached fixtures. This destroys broad-phase proxies. + b2Fixture * f = b->m_fixtureList; + while (f) { + b2Fixture * f0 = f; + f = f->m_next; + + if (m_destructionListener) { + m_destructionListener->SayGoodbye(f0); + } + + f0->DestroyProxies(&m_contactManager.m_broadPhase); + f0->Destroy(&m_blockAllocator); + f0->~b2Fixture(); + m_blockAllocator.Free(f0, sizeof(b2Fixture)); + + b->m_fixtureList = f; + b->m_fixtureCount -= 1; + } + b->m_fixtureList = nullptr; + b->m_fixtureCount = 0; + + // Remove world body list. + if (b->m_prev) { + b->m_prev->m_next = b->m_next; + } + + if (b->m_next) { + b->m_next->m_prev = b->m_prev; + } + + if (b == m_bodyList) { + m_bodyList = b->m_next; + } + + --m_bodyCount; + b->~b2Body(); + m_blockAllocator.Free(b, sizeof(b2Body)); } -b2Joint* b2World::CreateJoint(const b2JointDef* def) +b2Joint * b2World::CreateJoint(const b2JointDef * def) { - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return nullptr; - } - - b2Joint* j = b2Joint::Create(def, &m_blockAllocator); - - // Connect to the world list. - j->m_prev = nullptr; - j->m_next = m_jointList; - if (m_jointList) - { - m_jointList->m_prev = j; - } - m_jointList = j; - ++m_jointCount; - - // Connect to the bodies' doubly linked lists. - j->m_edgeA.joint = j; - j->m_edgeA.other = j->m_bodyB; - j->m_edgeA.prev = nullptr; - j->m_edgeA.next = j->m_bodyA->m_jointList; - if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA; - j->m_bodyA->m_jointList = &j->m_edgeA; - - j->m_edgeB.joint = j; - j->m_edgeB.other = j->m_bodyA; - j->m_edgeB.prev = nullptr; - j->m_edgeB.next = j->m_bodyB->m_jointList; - if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB; - j->m_bodyB->m_jointList = &j->m_edgeB; - - b2Body* bodyA = def->bodyA; - b2Body* bodyB = def->bodyB; - - // If the joint prevents collisions, then flag any contacts for filtering. - if (def->collideConnected == false) - { - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - // Flag the contact for filtering at the next time step (where either - // body is awake). - edge->contact->FlagForFiltering(); - } - - edge = edge->next; - } - } - - // Note: creating a joint doesn't wake the bodies. - - return j; + b2Assert(IsLocked() == false); + if (IsLocked()) { + return nullptr; + } + + b2Joint * j = b2Joint::Create(def, &m_blockAllocator); + + // Connect to the world list. + j->m_prev = nullptr; + j->m_next = m_jointList; + if (m_jointList) { + m_jointList->m_prev = j; + } + m_jointList = j; + ++m_jointCount; + + // Connect to the bodies' doubly linked lists. + j->m_edgeA.joint = j; + j->m_edgeA.other = j->m_bodyB; + j->m_edgeA.prev = nullptr; + j->m_edgeA.next = j->m_bodyA->m_jointList; + if (j->m_bodyA->m_jointList) j->m_bodyA->m_jointList->prev = &j->m_edgeA; + j->m_bodyA->m_jointList = &j->m_edgeA; + + j->m_edgeB.joint = j; + j->m_edgeB.other = j->m_bodyA; + j->m_edgeB.prev = nullptr; + j->m_edgeB.next = j->m_bodyB->m_jointList; + if (j->m_bodyB->m_jointList) j->m_bodyB->m_jointList->prev = &j->m_edgeB; + j->m_bodyB->m_jointList = &j->m_edgeB; + + b2Body * bodyA = def->bodyA; + b2Body * bodyB = def->bodyB; + + // If the joint prevents collisions, then flag any contacts for filtering. + if (def->collideConnected == false) { + b2ContactEdge * edge = bodyB->GetContactList(); + while (edge) { + if (edge->other == bodyA) { + // Flag the contact for filtering at the next time step (where either + // body is awake). + edge->contact->FlagForFiltering(); + } + + edge = edge->next; + } + } + + // Note: creating a joint doesn't wake the bodies. + + return j; } -void b2World::DestroyJoint(b2Joint* j) +void b2World::DestroyJoint(b2Joint * j) { - b2Assert(IsLocked() == false); - if (IsLocked()) - { - return; - } - - bool collideConnected = j->m_collideConnected; - - // Remove from the doubly linked list. - if (j->m_prev) - { - j->m_prev->m_next = j->m_next; - } - - if (j->m_next) - { - j->m_next->m_prev = j->m_prev; - } - - if (j == m_jointList) - { - m_jointList = j->m_next; - } - - // Disconnect from island graph. - b2Body* bodyA = j->m_bodyA; - b2Body* bodyB = j->m_bodyB; - - // Wake up connected bodies. - bodyA->SetAwake(true); - bodyB->SetAwake(true); - - // Remove from body 1. - if (j->m_edgeA.prev) - { - j->m_edgeA.prev->next = j->m_edgeA.next; - } - - if (j->m_edgeA.next) - { - j->m_edgeA.next->prev = j->m_edgeA.prev; - } - - if (&j->m_edgeA == bodyA->m_jointList) - { - bodyA->m_jointList = j->m_edgeA.next; - } - - j->m_edgeA.prev = nullptr; - j->m_edgeA.next = nullptr; - - // Remove from body 2 - if (j->m_edgeB.prev) - { - j->m_edgeB.prev->next = j->m_edgeB.next; - } - - if (j->m_edgeB.next) - { - j->m_edgeB.next->prev = j->m_edgeB.prev; - } - - if (&j->m_edgeB == bodyB->m_jointList) - { - bodyB->m_jointList = j->m_edgeB.next; - } - - j->m_edgeB.prev = nullptr; - j->m_edgeB.next = nullptr; - - b2Joint::Destroy(j, &m_blockAllocator); - - b2Assert(m_jointCount > 0); - --m_jointCount; - - // If the joint prevents collisions, then flag any contacts for filtering. - if (collideConnected == false) - { - b2ContactEdge* edge = bodyB->GetContactList(); - while (edge) - { - if (edge->other == bodyA) - { - // Flag the contact for filtering at the next time step (where either - // body is awake). - edge->contact->FlagForFiltering(); - } - - edge = edge->next; - } - } + b2Assert(IsLocked() == false); + if (IsLocked()) { + return; + } + + bool collideConnected = j->m_collideConnected; + + // Remove from the doubly linked list. + if (j->m_prev) { + j->m_prev->m_next = j->m_next; + } + + if (j->m_next) { + j->m_next->m_prev = j->m_prev; + } + + if (j == m_jointList) { + m_jointList = j->m_next; + } + + // Disconnect from island graph. + b2Body * bodyA = j->m_bodyA; + b2Body * bodyB = j->m_bodyB; + + // Wake up connected bodies. + bodyA->SetAwake(true); + bodyB->SetAwake(true); + + // Remove from body 1. + if (j->m_edgeA.prev) { + j->m_edgeA.prev->next = j->m_edgeA.next; + } + + if (j->m_edgeA.next) { + j->m_edgeA.next->prev = j->m_edgeA.prev; + } + + if (&j->m_edgeA == bodyA->m_jointList) { + bodyA->m_jointList = j->m_edgeA.next; + } + + j->m_edgeA.prev = nullptr; + j->m_edgeA.next = nullptr; + + // Remove from body 2 + if (j->m_edgeB.prev) { + j->m_edgeB.prev->next = j->m_edgeB.next; + } + + if (j->m_edgeB.next) { + j->m_edgeB.next->prev = j->m_edgeB.prev; + } + + if (&j->m_edgeB == bodyB->m_jointList) { + bodyB->m_jointList = j->m_edgeB.next; + } + + j->m_edgeB.prev = nullptr; + j->m_edgeB.next = nullptr; + + b2Joint::Destroy(j, &m_blockAllocator); + + b2Assert(m_jointCount > 0); + --m_jointCount; + + // If the joint prevents collisions, then flag any contacts for filtering. + if (collideConnected == false) { + b2ContactEdge * edge = bodyB->GetContactList(); + while (edge) { + if (edge->other == bodyA) { + // Flag the contact for filtering at the next time step (where either + // body is awake). + edge->contact->FlagForFiltering(); + } + + edge = edge->next; + } + } } // void b2World::SetAllowSleeping(bool flag) { - if (flag == m_allowSleep) - { - return; - } - - m_allowSleep = flag; - if (m_allowSleep == false) - { - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->SetAwake(true); - } - } + if (flag == m_allowSleep) { + return; + } + + m_allowSleep = flag; + if (m_allowSleep == false) { + for (b2Body * b = m_bodyList; b; b = b->m_next) { + b->SetAwake(true); + } + } } // Find islands, integrate and solve constraints, solve position constraints -void b2World::Solve(const b2TimeStep& step) +void b2World::Solve(const b2TimeStep & step) { - m_profile.solveInit = 0.0f; - m_profile.solveVelocity = 0.0f; - m_profile.solvePosition = 0.0f; - - // Size the island for the worst case. - b2Island island(m_bodyCount, - m_contactManager.m_contactCount, - m_jointCount, - &m_stackAllocator, - m_contactManager.m_contactListener); - - // Clear all the island flags. - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_flags &= ~b2Body::e_islandFlag; - } - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - c->m_flags &= ~b2Contact::e_islandFlag; - } - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->m_islandFlag = false; - } - - // Build and simulate all awake islands. - int32 stackSize = m_bodyCount; - b2Body** stack = (b2Body**)m_stackAllocator.Allocate(stackSize * sizeof(b2Body*)); - for (b2Body* seed = m_bodyList; seed; seed = seed->m_next) - { - if (seed->m_flags & b2Body::e_islandFlag) - { - continue; - } - - if (seed->IsAwake() == false || seed->IsActive() == false) - { - continue; - } - - // The seed can be dynamic or kinematic. - if (seed->GetType() == b2_staticBody) - { - continue; - } - - // Reset island and stack. - island.Clear(); - int32 stackCount = 0; - stack[stackCount++] = seed; - seed->m_flags |= b2Body::e_islandFlag; - - // Perform a depth first search (DFS) on the constraint graph. - while (stackCount > 0) - { - // Grab the next body off the stack and add it to the island. - b2Body* b = stack[--stackCount]; - b2Assert(b->IsActive() == true); - island.Add(b); - - // Make sure the body is awake (without resetting sleep timer). - b->m_flags |= b2Body::e_awakeFlag; - - // To keep islands as small as possible, we don't - // propagate islands across static bodies. - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Search all contacts connected to this body. - for (b2ContactEdge* ce = b->m_contactList; ce; ce = ce->next) - { - b2Contact* contact = ce->contact; - - // Has this contact already been added to an island? - if (contact->m_flags & b2Contact::e_islandFlag) - { - continue; - } - - // Is this contact solid and touching? - if (contact->IsEnabled() == false || - contact->IsTouching() == false) - { - continue; - } - - // Skip sensors. - bool sensorA = contact->m_fixtureA->m_isSensor; - bool sensorB = contact->m_fixtureB->m_isSensor; - if (sensorA || sensorB) - { - continue; - } - - island.Add(contact); - contact->m_flags |= b2Contact::e_islandFlag; - - b2Body* other = ce->other; - - // Was the other body already added to this island? - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - b2Assert(stackCount < stackSize); - stack[stackCount++] = other; - other->m_flags |= b2Body::e_islandFlag; - } - - // Search all joints connect to this body. - for (b2JointEdge* je = b->m_jointList; je; je = je->next) - { - if (je->joint->m_islandFlag == true) - { - continue; - } - - b2Body* other = je->other; - - // Don't simulate joints connected to inactive bodies. - if (other->IsActive() == false) - { - continue; - } - - island.Add(je->joint); - je->joint->m_islandFlag = true; - - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - b2Assert(stackCount < stackSize); - stack[stackCount++] = other; - other->m_flags |= b2Body::e_islandFlag; - } - } - - b2Profile profile; - island.Solve(&profile, step, m_gravity, m_allowSleep); - m_profile.solveInit += profile.solveInit; - m_profile.solveVelocity += profile.solveVelocity; - m_profile.solvePosition += profile.solvePosition; - - // Post solve cleanup. - for (int32 i = 0; i < island.m_bodyCount; ++i) - { - // Allow static bodies to participate in other islands. - b2Body* b = island.m_bodies[i]; - if (b->GetType() == b2_staticBody) - { - b->m_flags &= ~b2Body::e_islandFlag; - } - } - } - - m_stackAllocator.Free(stack); - - { - b2Timer timer; - // Synchronize fixtures, check for out of range bodies. - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - // If a body was not in an island then it did not move. - if ((b->m_flags & b2Body::e_islandFlag) == 0) - { - continue; - } - - if (b->GetType() == b2_staticBody) - { - continue; - } - - // Update fixtures (for broad-phase). - b->SynchronizeFixtures(); - } - - // Look for new contacts. - m_contactManager.FindNewContacts(); - m_profile.broadphase = timer.GetMilliseconds(); - } + m_profile.solveInit = 0.0f; + m_profile.solveVelocity = 0.0f; + m_profile.solvePosition = 0.0f; + + // Size the island for the worst case. + b2Island island( + m_bodyCount, m_contactManager.m_contactCount, m_jointCount, &m_stackAllocator, + m_contactManager.m_contactListener); + + // Clear all the island flags. + for (b2Body * b = m_bodyList; b; b = b->m_next) { + b->m_flags &= ~b2Body::e_islandFlag; + } + for (b2Contact * c = m_contactManager.m_contactList; c; c = c->m_next) { + c->m_flags &= ~b2Contact::e_islandFlag; + } + for (b2Joint * j = m_jointList; j; j = j->m_next) { + j->m_islandFlag = false; + } + + // Build and simulate all awake islands. + int32 stackSize = m_bodyCount; + b2Body ** stack = (b2Body **)m_stackAllocator.Allocate(stackSize * sizeof(b2Body *)); + for (b2Body * seed = m_bodyList; seed; seed = seed->m_next) { + if (seed->m_flags & b2Body::e_islandFlag) { + continue; + } + + if (seed->IsAwake() == false || seed->IsActive() == false) { + continue; + } + + // The seed can be dynamic or kinematic. + if (seed->GetType() == b2_staticBody) { + continue; + } + + // Reset island and stack. + island.Clear(); + int32 stackCount = 0; + stack[stackCount++] = seed; + seed->m_flags |= b2Body::e_islandFlag; + + // Perform a depth first search (DFS) on the constraint graph. + while (stackCount > 0) { + // Grab the next body off the stack and add it to the island. + b2Body * b = stack[--stackCount]; + b2Assert(b->IsActive() == true); + island.Add(b); + + // Make sure the body is awake (without resetting sleep timer). + b->m_flags |= b2Body::e_awakeFlag; + + // To keep islands as small as possible, we don't + // propagate islands across static bodies. + if (b->GetType() == b2_staticBody) { + continue; + } + + // Search all contacts connected to this body. + for (b2ContactEdge * ce = b->m_contactList; ce; ce = ce->next) { + b2Contact * contact = ce->contact; + + // Has this contact already been added to an island? + if (contact->m_flags & b2Contact::e_islandFlag) { + continue; + } + + // Is this contact solid and touching? + if (contact->IsEnabled() == false || contact->IsTouching() == false) { + continue; + } + + // Skip sensors. + bool sensorA = contact->m_fixtureA->m_isSensor; + bool sensorB = contact->m_fixtureB->m_isSensor; + if (sensorA || sensorB) { + continue; + } + + island.Add(contact); + contact->m_flags |= b2Contact::e_islandFlag; + + b2Body * other = ce->other; + + // Was the other body already added to this island? + if (other->m_flags & b2Body::e_islandFlag) { + continue; + } + + b2Assert(stackCount < stackSize); + stack[stackCount++] = other; + other->m_flags |= b2Body::e_islandFlag; + } + + // Search all joints connect to this body. + for (b2JointEdge * je = b->m_jointList; je; je = je->next) { + if (je->joint->m_islandFlag == true) { + continue; + } + + b2Body * other = je->other; + + // Don't simulate joints connected to inactive bodies. + if (other->IsActive() == false) { + continue; + } + + island.Add(je->joint); + je->joint->m_islandFlag = true; + + if (other->m_flags & b2Body::e_islandFlag) { + continue; + } + + b2Assert(stackCount < stackSize); + stack[stackCount++] = other; + other->m_flags |= b2Body::e_islandFlag; + } + } + + b2Profile profile; + island.Solve(&profile, step, m_gravity, m_allowSleep); + m_profile.solveInit += profile.solveInit; + m_profile.solveVelocity += profile.solveVelocity; + m_profile.solvePosition += profile.solvePosition; + + // Post solve cleanup. + for (int32 i = 0; i < island.m_bodyCount; ++i) { + // Allow static bodies to participate in other islands. + b2Body * b = island.m_bodies[i]; + if (b->GetType() == b2_staticBody) { + b->m_flags &= ~b2Body::e_islandFlag; + } + } + } + + m_stackAllocator.Free(stack); + + { + b2Timer timer; + // Synchronize fixtures, check for out of range bodies. + for (b2Body * b = m_bodyList; b; b = b->GetNext()) { + // If a body was not in an island then it did not move. + if ((b->m_flags & b2Body::e_islandFlag) == 0) { + continue; + } + + if (b->GetType() == b2_staticBody) { + continue; + } + + // Update fixtures (for broad-phase). + b->SynchronizeFixtures(); + } + + // Look for new contacts. + m_contactManager.FindNewContacts(); + m_profile.broadphase = timer.GetMilliseconds(); + } } // Find TOI contacts and solve them. -void b2World::SolveTOI(const b2TimeStep& step) +void b2World::SolveTOI(const b2TimeStep & step) { - b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener); - - if (m_stepComplete) - { - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_flags &= ~b2Body::e_islandFlag; - b->m_sweep.alpha0 = 0.0f; - } - - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Invalidate TOI - c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); - c->m_toiCount = 0; - c->m_toi = 1.0f; - } - } - - // Find TOI events and solve them. - for (;;) - { - // Find the first TOI. - b2Contact* minContact = nullptr; - float32 minAlpha = 1.0f; - - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next) - { - // Is this contact disabled? - if (c->IsEnabled() == false) - { - continue; - } - - // Prevent excessive sub-stepping. - if (c->m_toiCount > b2_maxSubSteps) - { - continue; - } - - float32 alpha = 1.0f; - if (c->m_flags & b2Contact::e_toiFlag) - { - // This contact has a valid cached TOI. - alpha = c->m_toi; - } - else - { - b2Fixture* fA = c->GetFixtureA(); - b2Fixture* fB = c->GetFixtureB(); - - // Is there a sensor? - if (fA->IsSensor() || fB->IsSensor()) - { - continue; - } - - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - b2BodyType typeA = bA->m_type; - b2BodyType typeB = bB->m_type; - b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody); - - bool activeA = bA->IsAwake() && typeA != b2_staticBody; - bool activeB = bB->IsAwake() && typeB != b2_staticBody; - - // Is at least one body active (awake and dynamic or kinematic)? - if (activeA == false && activeB == false) - { - continue; - } - - bool collideA = bA->IsBullet() || typeA != b2_dynamicBody; - bool collideB = bB->IsBullet() || typeB != b2_dynamicBody; - - // Are these two non-bullet dynamic bodies? - if (collideA == false && collideB == false) - { - continue; - } - - // Compute the TOI for this contact. - // Put the sweeps onto the same time interval. - float32 alpha0 = bA->m_sweep.alpha0; - - if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) - { - alpha0 = bB->m_sweep.alpha0; - bA->m_sweep.Advance(alpha0); - } - else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) - { - alpha0 = bA->m_sweep.alpha0; - bB->m_sweep.Advance(alpha0); - } - - b2Assert(alpha0 < 1.0f); - - int32 indexA = c->GetChildIndexA(); - int32 indexB = c->GetChildIndexB(); - - // Compute the time of impact in interval [0, minTOI] - b2TOIInput input; - input.proxyA.Set(fA->GetShape(), indexA); - input.proxyB.Set(fB->GetShape(), indexB); - input.sweepA = bA->m_sweep; - input.sweepB = bB->m_sweep; - input.tMax = 1.0f; - - b2TOIOutput output; - b2TimeOfImpact(&output, &input); - - // Beta is the fraction of the remaining portion of the . - float32 beta = output.t; - if (output.state == b2TOIOutput::e_touching) - { - alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); - } - else - { - alpha = 1.0f; - } - - c->m_toi = alpha; - c->m_flags |= b2Contact::e_toiFlag; - } - - if (alpha < minAlpha) - { - // This is the minimum TOI found so far. - minContact = c; - minAlpha = alpha; - } - } - - if (minContact == nullptr || 1.0f - 10.0f * b2_epsilon < minAlpha) - { - // No more TOI events. Done! - m_stepComplete = true; - break; - } - - // Advance the bodies to the TOI. - b2Fixture* fA = minContact->GetFixtureA(); - b2Fixture* fB = minContact->GetFixtureB(); - b2Body* bA = fA->GetBody(); - b2Body* bB = fB->GetBody(); - - b2Sweep backup1 = bA->m_sweep; - b2Sweep backup2 = bB->m_sweep; - - bA->Advance(minAlpha); - bB->Advance(minAlpha); - - // The TOI contact likely has some new contact points. - minContact->Update(m_contactManager.m_contactListener); - minContact->m_flags &= ~b2Contact::e_toiFlag; - ++minContact->m_toiCount; - - // Is the contact solid? - if (minContact->IsEnabled() == false || minContact->IsTouching() == false) - { - // Restore the sweeps. - minContact->SetEnabled(false); - bA->m_sweep = backup1; - bB->m_sweep = backup2; - bA->SynchronizeTransform(); - bB->SynchronizeTransform(); - continue; - } - - bA->SetAwake(true); - bB->SetAwake(true); - - // Build the island - island.Clear(); - island.Add(bA); - island.Add(bB); - island.Add(minContact); - - bA->m_flags |= b2Body::e_islandFlag; - bB->m_flags |= b2Body::e_islandFlag; - minContact->m_flags |= b2Contact::e_islandFlag; - - // Get contacts on bodyA and bodyB. - b2Body* bodies[2] = {bA, bB}; - for (int32 i = 0; i < 2; ++i) - { - b2Body* body = bodies[i]; - if (body->m_type == b2_dynamicBody) - { - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) - { - if (island.m_bodyCount == island.m_bodyCapacity) - { - break; - } - - if (island.m_contactCount == island.m_contactCapacity) - { - break; - } - - b2Contact* contact = ce->contact; - - // Has this contact already been added to the island? - if (contact->m_flags & b2Contact::e_islandFlag) - { - continue; - } - - // Only add static, kinematic, or bullet bodies. - b2Body* other = ce->other; - if (other->m_type == b2_dynamicBody && - body->IsBullet() == false && other->IsBullet() == false) - { - continue; - } - - // Skip sensors. - bool sensorA = contact->m_fixtureA->m_isSensor; - bool sensorB = contact->m_fixtureB->m_isSensor; - if (sensorA || sensorB) - { - continue; - } - - // Tentatively advance the body to the TOI. - b2Sweep backup = other->m_sweep; - if ((other->m_flags & b2Body::e_islandFlag) == 0) - { - other->Advance(minAlpha); - } - - // Update the contact points - contact->Update(m_contactManager.m_contactListener); - - // Was the contact disabled by the user? - if (contact->IsEnabled() == false) - { - other->m_sweep = backup; - other->SynchronizeTransform(); - continue; - } - - // Are there contact points? - if (contact->IsTouching() == false) - { - other->m_sweep = backup; - other->SynchronizeTransform(); - continue; - } - - // Add the contact to the island - contact->m_flags |= b2Contact::e_islandFlag; - island.Add(contact); - - // Has the other body already been added to the island? - if (other->m_flags & b2Body::e_islandFlag) - { - continue; - } - - // Add the other body to the island. - other->m_flags |= b2Body::e_islandFlag; - - if (other->m_type != b2_staticBody) - { - other->SetAwake(true); - } - - island.Add(other); - } - } - } - - b2TimeStep subStep; - subStep.dt = (1.0f - minAlpha) * step.dt; - subStep.inv_dt = 1.0f / subStep.dt; - subStep.dtRatio = 1.0f; - subStep.positionIterations = 20; - subStep.velocityIterations = step.velocityIterations; - subStep.warmStarting = false; - island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex); - - // Reset island flags and synchronize broad-phase proxies. - for (int32 i = 0; i < island.m_bodyCount; ++i) - { - b2Body* body = island.m_bodies[i]; - body->m_flags &= ~b2Body::e_islandFlag; - - if (body->m_type != b2_dynamicBody) - { - continue; - } - - body->SynchronizeFixtures(); - - // Invalidate all contact TOIs on this displaced body. - for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next) - { - ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); - } - } - - // Commit fixture proxy movements to the broad-phase so that new contacts are created. - // Also, some contacts can be destroyed. - m_contactManager.FindNewContacts(); - - if (m_subStepping) - { - m_stepComplete = false; - break; - } - } + b2Island island( + 2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, + m_contactManager.m_contactListener); + + if (m_stepComplete) { + for (b2Body * b = m_bodyList; b; b = b->m_next) { + b->m_flags &= ~b2Body::e_islandFlag; + b->m_sweep.alpha0 = 0.0f; + } + + for (b2Contact * c = m_contactManager.m_contactList; c; c = c->m_next) { + // Invalidate TOI + c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); + c->m_toiCount = 0; + c->m_toi = 1.0f; + } + } + + // Find TOI events and solve them. + for (;;) { + // Find the first TOI. + b2Contact * minContact = nullptr; + float32 minAlpha = 1.0f; + + for (b2Contact * c = m_contactManager.m_contactList; c; c = c->m_next) { + // Is this contact disabled? + if (c->IsEnabled() == false) { + continue; + } + + // Prevent excessive sub-stepping. + if (c->m_toiCount > b2_maxSubSteps) { + continue; + } + + float32 alpha = 1.0f; + if (c->m_flags & b2Contact::e_toiFlag) { + // This contact has a valid cached TOI. + alpha = c->m_toi; + } else { + b2Fixture * fA = c->GetFixtureA(); + b2Fixture * fB = c->GetFixtureB(); + + // Is there a sensor? + if (fA->IsSensor() || fB->IsSensor()) { + continue; + } + + b2Body * bA = fA->GetBody(); + b2Body * bB = fB->GetBody(); + + b2BodyType typeA = bA->m_type; + b2BodyType typeB = bB->m_type; + b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody); + + bool activeA = bA->IsAwake() && typeA != b2_staticBody; + bool activeB = bB->IsAwake() && typeB != b2_staticBody; + + // Is at least one body active (awake and dynamic or kinematic)? + if (activeA == false && activeB == false) { + continue; + } + + bool collideA = bA->IsBullet() || typeA != b2_dynamicBody; + bool collideB = bB->IsBullet() || typeB != b2_dynamicBody; + + // Are these two non-bullet dynamic bodies? + if (collideA == false && collideB == false) { + continue; + } + + // Compute the TOI for this contact. + // Put the sweeps onto the same time interval. + float32 alpha0 = bA->m_sweep.alpha0; + + if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0) { + alpha0 = bB->m_sweep.alpha0; + bA->m_sweep.Advance(alpha0); + } else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0) { + alpha0 = bA->m_sweep.alpha0; + bB->m_sweep.Advance(alpha0); + } + + b2Assert(alpha0 < 1.0f); + + int32 indexA = c->GetChildIndexA(); + int32 indexB = c->GetChildIndexB(); + + // Compute the time of impact in interval [0, minTOI] + b2TOIInput input; + input.proxyA.Set(fA->GetShape(), indexA); + input.proxyB.Set(fB->GetShape(), indexB); + input.sweepA = bA->m_sweep; + input.sweepB = bB->m_sweep; + input.tMax = 1.0f; + + b2TOIOutput output; + b2TimeOfImpact(&output, &input); + + // Beta is the fraction of the remaining portion of the . + float32 beta = output.t; + if (output.state == b2TOIOutput::e_touching) { + alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); + } else { + alpha = 1.0f; + } + + c->m_toi = alpha; + c->m_flags |= b2Contact::e_toiFlag; + } + + if (alpha < minAlpha) { + // This is the minimum TOI found so far. + minContact = c; + minAlpha = alpha; + } + } + + if (minContact == nullptr || 1.0f - 10.0f * b2_epsilon < minAlpha) { + // No more TOI events. Done! + m_stepComplete = true; + break; + } + + // Advance the bodies to the TOI. + b2Fixture * fA = minContact->GetFixtureA(); + b2Fixture * fB = minContact->GetFixtureB(); + b2Body * bA = fA->GetBody(); + b2Body * bB = fB->GetBody(); + + b2Sweep backup1 = bA->m_sweep; + b2Sweep backup2 = bB->m_sweep; + + bA->Advance(minAlpha); + bB->Advance(minAlpha); + + // The TOI contact likely has some new contact points. + minContact->Update(m_contactManager.m_contactListener); + minContact->m_flags &= ~b2Contact::e_toiFlag; + ++minContact->m_toiCount; + + // Is the contact solid? + if (minContact->IsEnabled() == false || minContact->IsTouching() == false) { + // Restore the sweeps. + minContact->SetEnabled(false); + bA->m_sweep = backup1; + bB->m_sweep = backup2; + bA->SynchronizeTransform(); + bB->SynchronizeTransform(); + continue; + } + + bA->SetAwake(true); + bB->SetAwake(true); + + // Build the island + island.Clear(); + island.Add(bA); + island.Add(bB); + island.Add(minContact); + + bA->m_flags |= b2Body::e_islandFlag; + bB->m_flags |= b2Body::e_islandFlag; + minContact->m_flags |= b2Contact::e_islandFlag; + + // Get contacts on bodyA and bodyB. + b2Body * bodies[2] = {bA, bB}; + for (int32 i = 0; i < 2; ++i) { + b2Body * body = bodies[i]; + if (body->m_type == b2_dynamicBody) { + for (b2ContactEdge * ce = body->m_contactList; ce; ce = ce->next) { + if (island.m_bodyCount == island.m_bodyCapacity) { + break; + } + + if (island.m_contactCount == island.m_contactCapacity) { + break; + } + + b2Contact * contact = ce->contact; + + // Has this contact already been added to the island? + if (contact->m_flags & b2Contact::e_islandFlag) { + continue; + } + + // Only add static, kinematic, or bullet bodies. + b2Body * other = ce->other; + if ( + other->m_type == b2_dynamicBody && body->IsBullet() == false && + other->IsBullet() == false) { + continue; + } + + // Skip sensors. + bool sensorA = contact->m_fixtureA->m_isSensor; + bool sensorB = contact->m_fixtureB->m_isSensor; + if (sensorA || sensorB) { + continue; + } + + // Tentatively advance the body to the TOI. + b2Sweep backup = other->m_sweep; + if ((other->m_flags & b2Body::e_islandFlag) == 0) { + other->Advance(minAlpha); + } + + // Update the contact points + contact->Update(m_contactManager.m_contactListener); + + // Was the contact disabled by the user? + if (contact->IsEnabled() == false) { + other->m_sweep = backup; + other->SynchronizeTransform(); + continue; + } + + // Are there contact points? + if (contact->IsTouching() == false) { + other->m_sweep = backup; + other->SynchronizeTransform(); + continue; + } + + // Add the contact to the island + contact->m_flags |= b2Contact::e_islandFlag; + island.Add(contact); + + // Has the other body already been added to the island? + if (other->m_flags & b2Body::e_islandFlag) { + continue; + } + + // Add the other body to the island. + other->m_flags |= b2Body::e_islandFlag; + + if (other->m_type != b2_staticBody) { + other->SetAwake(true); + } + + island.Add(other); + } + } + } + + b2TimeStep subStep; + subStep.dt = (1.0f - minAlpha) * step.dt; + subStep.inv_dt = 1.0f / subStep.dt; + subStep.dtRatio = 1.0f; + subStep.positionIterations = 20; + subStep.velocityIterations = step.velocityIterations; + subStep.warmStarting = false; + island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex); + + // Reset island flags and synchronize broad-phase proxies. + for (int32 i = 0; i < island.m_bodyCount; ++i) { + b2Body * body = island.m_bodies[i]; + body->m_flags &= ~b2Body::e_islandFlag; + + if (body->m_type != b2_dynamicBody) { + continue; + } + + body->SynchronizeFixtures(); + + // Invalidate all contact TOIs on this displaced body. + for (b2ContactEdge * ce = body->m_contactList; ce; ce = ce->next) { + ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag); + } + } + + // Commit fixture proxy movements to the broad-phase so that new contacts + // are created. Also, some contacts can be destroyed. + m_contactManager.FindNewContacts(); + + if (m_subStepping) { + m_stepComplete = false; + break; + } + } } void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations) { - b2Timer stepTimer; - - // If new fixtures were added, we need to find the new contacts. - if (m_flags & e_newFixture) - { - m_contactManager.FindNewContacts(); - m_flags &= ~e_newFixture; - } - - m_flags |= e_locked; - - b2TimeStep step; - step.dt = dt; - step.velocityIterations = velocityIterations; - step.positionIterations = positionIterations; - if (dt > 0.0f) - { - step.inv_dt = 1.0f / dt; - } - else - { - step.inv_dt = 0.0f; - } - - step.dtRatio = m_inv_dt0 * dt; - - step.warmStarting = m_warmStarting; - - // Update contacts. This is where some contacts are destroyed. - { - b2Timer timer; - m_contactManager.Collide(); - m_profile.collide = timer.GetMilliseconds(); - } - - // Integrate velocities, solve velocity constraints, and integrate positions. - if (m_stepComplete && step.dt > 0.0f) - { - b2Timer timer; - Solve(step); - m_profile.solve = timer.GetMilliseconds(); - } - - // Handle TOI events. - if (m_continuousPhysics && step.dt > 0.0f) - { - b2Timer timer; - SolveTOI(step); - m_profile.solveTOI = timer.GetMilliseconds(); - } - - if (step.dt > 0.0f) - { - m_inv_dt0 = step.inv_dt; - } - - if (m_flags & e_clearForces) - { - ClearForces(); - } - - m_flags &= ~e_locked; - - m_profile.step = stepTimer.GetMilliseconds(); + b2Timer stepTimer; + + // If new fixtures were added, we need to find the new contacts. + if (m_flags & e_newFixture) { + m_contactManager.FindNewContacts(); + m_flags &= ~e_newFixture; + } + + m_flags |= e_locked; + + b2TimeStep step; + step.dt = dt; + step.velocityIterations = velocityIterations; + step.positionIterations = positionIterations; + if (dt > 0.0f) { + step.inv_dt = 1.0f / dt; + } else { + step.inv_dt = 0.0f; + } + + step.dtRatio = m_inv_dt0 * dt; + + step.warmStarting = m_warmStarting; + + // Update contacts. This is where some contacts are destroyed. + { + b2Timer timer; + m_contactManager.Collide(); + m_profile.collide = timer.GetMilliseconds(); + } + + // Integrate velocities, solve velocity constraints, and integrate positions. + if (m_stepComplete && step.dt > 0.0f) { + b2Timer timer; + Solve(step); + m_profile.solve = timer.GetMilliseconds(); + } + + // Handle TOI events. + if (m_continuousPhysics && step.dt > 0.0f) { + b2Timer timer; + SolveTOI(step); + m_profile.solveTOI = timer.GetMilliseconds(); + } + + if (step.dt > 0.0f) { + m_inv_dt0 = step.inv_dt; + } + + if (m_flags & e_clearForces) { + ClearForces(); + } + + m_flags &= ~e_locked; + + m_profile.step = stepTimer.GetMilliseconds(); } void b2World::ClearForces() { - for (b2Body* body = m_bodyList; body; body = body->GetNext()) - { - body->m_force.SetZero(); - body->m_torque = 0.0f; - } + for (b2Body * body = m_bodyList; body; body = body->GetNext()) { + body->m_force.SetZero(); + body->m_torque = 0.0f; + } } struct b2WorldQueryWrapper { - bool QueryCallback(int32 proxyId) - { - b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId); - return callback->ReportFixture(proxy->fixture); - } - - const b2BroadPhase* broadPhase; - b2QueryCallback* callback; + bool QueryCallback(int32 proxyId) + { + b2FixtureProxy * proxy = (b2FixtureProxy *)broadPhase->GetUserData(proxyId); + return callback->ReportFixture(proxy->fixture); + } + + const b2BroadPhase * broadPhase; + b2QueryCallback * callback; }; -void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const +void b2World::QueryAABB(b2QueryCallback * callback, const b2AABB & aabb) const { - b2WorldQueryWrapper wrapper; - wrapper.broadPhase = &m_contactManager.m_broadPhase; - wrapper.callback = callback; - m_contactManager.m_broadPhase.Query(&wrapper, aabb); + b2WorldQueryWrapper wrapper; + wrapper.broadPhase = &m_contactManager.m_broadPhase; + wrapper.callback = callback; + m_contactManager.m_broadPhase.Query(&wrapper, aabb); } struct b2WorldRayCastWrapper { - float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId) - { - void* userData = broadPhase->GetUserData(proxyId); - b2FixtureProxy* proxy = (b2FixtureProxy*)userData; - b2Fixture* fixture = proxy->fixture; - int32 index = proxy->childIndex; - b2RayCastOutput output; - bool hit = fixture->RayCast(&output, input, index); - - if (hit) - { - float32 fraction = output.fraction; - b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; - return callback->ReportFixture(fixture, point, output.normal, fraction); - } - - return input.maxFraction; - } - - const b2BroadPhase* broadPhase; - b2RayCastCallback* callback; + float32 RayCastCallback(const b2RayCastInput & input, int32 proxyId) + { + void * userData = broadPhase->GetUserData(proxyId); + b2FixtureProxy * proxy = (b2FixtureProxy *)userData; + b2Fixture * fixture = proxy->fixture; + int32 index = proxy->childIndex; + b2RayCastOutput output; + bool hit = fixture->RayCast(&output, input, index); + + if (hit) { + float32 fraction = output.fraction; + b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2; + return callback->ReportFixture(fixture, point, output.normal, fraction); + } + + return input.maxFraction; + } + + const b2BroadPhase * broadPhase; + b2RayCastCallback * callback; }; -void b2World::RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const +void b2World::RayCast( + b2RayCastCallback * callback, const b2Vec2 & point1, const b2Vec2 & point2) const { - b2WorldRayCastWrapper wrapper; - wrapper.broadPhase = &m_contactManager.m_broadPhase; - wrapper.callback = callback; - b2RayCastInput input; - input.maxFraction = 1.0f; - input.p1 = point1; - input.p2 = point2; - m_contactManager.m_broadPhase.RayCast(&wrapper, input); + b2WorldRayCastWrapper wrapper; + wrapper.broadPhase = &m_contactManager.m_broadPhase; + wrapper.callback = callback; + b2RayCastInput input; + input.maxFraction = 1.0f; + input.p1 = point1; + input.p2 = point2; + m_contactManager.m_broadPhase.RayCast(&wrapper, input); } -void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color& color) +void b2World::DrawShape(b2Fixture * fixture, const b2Transform & xf, const b2Color & color) { - switch (fixture->GetType()) - { - case b2Shape::e_circle: - { - b2CircleShape* circle = (b2CircleShape*)fixture->GetShape(); - - b2Vec2 center = b2Mul(xf, circle->m_p); - float32 radius = circle->m_radius; - b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); - - g_debugDraw->DrawSolidCircle(center, radius, axis, color); - } - break; - - case b2Shape::e_edge: - { - b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); - b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); - b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); - g_debugDraw->DrawSegment(v1, v2, color); - } - break; - - case b2Shape::e_chain: - { - b2ChainShape* chain = (b2ChainShape*)fixture->GetShape(); - int32 count = chain->m_count; - const b2Vec2* vertices = chain->m_vertices; - - b2Color ghostColor(0.75f * color.r, 0.75f * color.g, 0.75f * color.b, color.a); - - b2Vec2 v1 = b2Mul(xf, vertices[0]); - g_debugDraw->DrawPoint(v1, 4.0f, color); - - if (chain->m_hasPrevVertex) - { - b2Vec2 vp = b2Mul(xf, chain->m_prevVertex); - g_debugDraw->DrawSegment(vp, v1, ghostColor); - g_debugDraw->DrawCircle(vp, 0.1f, ghostColor); - } - - for (int32 i = 1; i < count; ++i) - { - b2Vec2 v2 = b2Mul(xf, vertices[i]); - g_debugDraw->DrawSegment(v1, v2, color); - g_debugDraw->DrawPoint(v2, 4.0f, color); - v1 = v2; - } - - if (chain->m_hasNextVertex) - { - b2Vec2 vn = b2Mul(xf, chain->m_nextVertex); - g_debugDraw->DrawSegment(v1, vn, ghostColor); - g_debugDraw->DrawCircle(vn, 0.1f, ghostColor); - } - } - break; - - case b2Shape::e_polygon: - { - b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); - int32 vertexCount = poly->m_count; - b2Assert(vertexCount <= b2_maxPolygonVertices); - b2Vec2 vertices[b2_maxPolygonVertices]; - - for (int32 i = 0; i < vertexCount; ++i) - { - vertices[i] = b2Mul(xf, poly->m_vertices[i]); - } - - g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); - } - break; - + switch (fixture->GetType()) { + case b2Shape::e_circle: { + b2CircleShape * circle = (b2CircleShape *)fixture->GetShape(); + + b2Vec2 center = b2Mul(xf, circle->m_p); + float32 radius = circle->m_radius; + b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f)); + + g_debugDraw->DrawSolidCircle(center, radius, axis, color); + } break; + + case b2Shape::e_edge: { + b2EdgeShape * edge = (b2EdgeShape *)fixture->GetShape(); + b2Vec2 v1 = b2Mul(xf, edge->m_vertex1); + b2Vec2 v2 = b2Mul(xf, edge->m_vertex2); + g_debugDraw->DrawSegment(v1, v2, color); + } break; + + case b2Shape::e_chain: { + b2ChainShape * chain = (b2ChainShape *)fixture->GetShape(); + int32 count = chain->m_count; + const b2Vec2 * vertices = chain->m_vertices; + + b2Color ghostColor(0.75f * color.r, 0.75f * color.g, 0.75f * color.b, color.a); + + b2Vec2 v1 = b2Mul(xf, vertices[0]); + g_debugDraw->DrawPoint(v1, 4.0f, color); + + if (chain->m_hasPrevVertex) { + b2Vec2 vp = b2Mul(xf, chain->m_prevVertex); + g_debugDraw->DrawSegment(vp, v1, ghostColor); + g_debugDraw->DrawCircle(vp, 0.1f, ghostColor); + } + + for (int32 i = 1; i < count; ++i) { + b2Vec2 v2 = b2Mul(xf, vertices[i]); + g_debugDraw->DrawSegment(v1, v2, color); + g_debugDraw->DrawPoint(v2, 4.0f, color); + v1 = v2; + } + + if (chain->m_hasNextVertex) { + b2Vec2 vn = b2Mul(xf, chain->m_nextVertex); + g_debugDraw->DrawSegment(v1, vn, ghostColor); + g_debugDraw->DrawCircle(vn, 0.1f, ghostColor); + } + } break; + + case b2Shape::e_polygon: { + b2PolygonShape * poly = (b2PolygonShape *)fixture->GetShape(); + int32 vertexCount = poly->m_count; + b2Assert(vertexCount <= b2_maxPolygonVertices); + b2Vec2 vertices[b2_maxPolygonVertices]; + + for (int32 i = 0; i < vertexCount; ++i) { + vertices[i] = b2Mul(xf, poly->m_vertices[i]); + } + + g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color); + } break; + default: - break; - } + break; + } } -void b2World::DrawJoint(b2Joint* joint) +void b2World::DrawJoint(b2Joint * joint) { - b2Body* bodyA = joint->GetBodyA(); - b2Body* bodyB = joint->GetBodyB(); - const b2Transform& xf1 = bodyA->GetTransform(); - const b2Transform& xf2 = bodyB->GetTransform(); - b2Vec2 x1 = xf1.p; - b2Vec2 x2 = xf2.p; - b2Vec2 p1 = joint->GetAnchorA(); - b2Vec2 p2 = joint->GetAnchorB(); - - b2Color color(0.5f, 0.8f, 0.8f); - - switch (joint->GetType()) - { - case e_distanceJoint: - g_debugDraw->DrawSegment(p1, p2, color); - break; - - case e_pulleyJoint: - { - b2PulleyJoint* pulley = (b2PulleyJoint*)joint; - b2Vec2 s1 = pulley->GetGroundAnchorA(); - b2Vec2 s2 = pulley->GetGroundAnchorB(); - g_debugDraw->DrawSegment(s1, p1, color); - g_debugDraw->DrawSegment(s2, p2, color); - g_debugDraw->DrawSegment(s1, s2, color); - } - break; - - case e_mouseJoint: - // don't draw this - break; - - default: - g_debugDraw->DrawSegment(x1, p1, color); - g_debugDraw->DrawSegment(p1, p2, color); - g_debugDraw->DrawSegment(x2, p2, color); - } + b2Body * bodyA = joint->GetBodyA(); + b2Body * bodyB = joint->GetBodyB(); + const b2Transform & xf1 = bodyA->GetTransform(); + const b2Transform & xf2 = bodyB->GetTransform(); + b2Vec2 x1 = xf1.p; + b2Vec2 x2 = xf2.p; + b2Vec2 p1 = joint->GetAnchorA(); + b2Vec2 p2 = joint->GetAnchorB(); + + b2Color color(0.5f, 0.8f, 0.8f); + + switch (joint->GetType()) { + case e_distanceJoint: + g_debugDraw->DrawSegment(p1, p2, color); + break; + + case e_pulleyJoint: { + b2PulleyJoint * pulley = (b2PulleyJoint *)joint; + b2Vec2 s1 = pulley->GetGroundAnchorA(); + b2Vec2 s2 = pulley->GetGroundAnchorB(); + g_debugDraw->DrawSegment(s1, p1, color); + g_debugDraw->DrawSegment(s2, p2, color); + g_debugDraw->DrawSegment(s1, s2, color); + } break; + + case e_mouseJoint: + // don't draw this + break; + + default: + g_debugDraw->DrawSegment(x1, p1, color); + g_debugDraw->DrawSegment(p1, p2, color); + g_debugDraw->DrawSegment(x2, p2, color); + } } void b2World::DrawDebugData() { - if (g_debugDraw == nullptr) - { - return; - } - - uint32 flags = g_debugDraw->GetFlags(); - - if (flags & b2Draw::e_shapeBit) - { - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - const b2Transform& xf = b->GetTransform(); - for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) - { - if (b->IsActive() == false) - { - DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f)); - } - else if (b->GetType() == b2_staticBody) - { - DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f)); - } - else if (b->GetType() == b2_kinematicBody) - { - DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f)); - } - else if (b->IsAwake() == false) - { - DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f)); - } - else - { - DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f)); - } - } - } - } - - if (flags & b2Draw::e_jointBit) - { - for (b2Joint* j = m_jointList; j; j = j->GetNext()) - { - DrawJoint(j); - } - } - - if (flags & b2Draw::e_pairBit) - { - b2Color color(0.3f, 0.9f, 0.9f); - for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext()) - { - //b2Fixture* fixtureA = c->GetFixtureA(); - //b2Fixture* fixtureB = c->GetFixtureB(); - - //b2Vec2 cA = fixtureA->GetAABB().GetCenter(); - //b2Vec2 cB = fixtureB->GetAABB().GetCenter(); - - //g_debugDraw->DrawSegment(cA, cB, color); - } - } - - if (flags & b2Draw::e_aabbBit) - { - b2Color color(0.9f, 0.3f, 0.9f); - b2BroadPhase* bp = &m_contactManager.m_broadPhase; - - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - if (b->IsActive() == false) - { - continue; - } - - for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext()) - { - for (int32 i = 0; i < f->m_proxyCount; ++i) - { - b2FixtureProxy* proxy = f->m_proxies + i; - b2AABB aabb = bp->GetFatAABB(proxy->proxyId); - b2Vec2 vs[4]; - vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y); - vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y); - vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); - vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); - - g_debugDraw->DrawPolygon(vs, 4, color); - } - } - } - } - - if (flags & b2Draw::e_centerOfMassBit) - { - for (b2Body* b = m_bodyList; b; b = b->GetNext()) - { - b2Transform xf = b->GetTransform(); - xf.p = b->GetWorldCenter(); - g_debugDraw->DrawTransform(xf); - } - } + if (g_debugDraw == nullptr) { + return; + } + + uint32 flags = g_debugDraw->GetFlags(); + + if (flags & b2Draw::e_shapeBit) { + for (b2Body * b = m_bodyList; b; b = b->GetNext()) { + const b2Transform & xf = b->GetTransform(); + for (b2Fixture * f = b->GetFixtureList(); f; f = f->GetNext()) { + if (b->IsActive() == false) { + DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f)); + } else if (b->GetType() == b2_staticBody) { + DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f)); + } else if (b->GetType() == b2_kinematicBody) { + DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f)); + } else if (b->IsAwake() == false) { + DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f)); + } else { + DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f)); + } + } + } + } + + if (flags & b2Draw::e_jointBit) { + for (b2Joint * j = m_jointList; j; j = j->GetNext()) { + DrawJoint(j); + } + } + + if (flags & b2Draw::e_pairBit) { + b2Color color(0.3f, 0.9f, 0.9f); + for (b2Contact * c = m_contactManager.m_contactList; c; c = c->GetNext()) { + // b2Fixture* fixtureA = c->GetFixtureA(); + // b2Fixture* fixtureB = c->GetFixtureB(); + + // b2Vec2 cA = fixtureA->GetAABB().GetCenter(); + // b2Vec2 cB = fixtureB->GetAABB().GetCenter(); + + // g_debugDraw->DrawSegment(cA, cB, color); + } + } + + if (flags & b2Draw::e_aabbBit) { + b2Color color(0.9f, 0.3f, 0.9f); + b2BroadPhase * bp = &m_contactManager.m_broadPhase; + + for (b2Body * b = m_bodyList; b; b = b->GetNext()) { + if (b->IsActive() == false) { + continue; + } + + for (b2Fixture * f = b->GetFixtureList(); f; f = f->GetNext()) { + for (int32 i = 0; i < f->m_proxyCount; ++i) { + b2FixtureProxy * proxy = f->m_proxies + i; + b2AABB aabb = bp->GetFatAABB(proxy->proxyId); + b2Vec2 vs[4]; + vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y); + vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y); + vs[2].Set(aabb.upperBound.x, aabb.upperBound.y); + vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y); + + g_debugDraw->DrawPolygon(vs, 4, color); + } + } + } + } + + if (flags & b2Draw::e_centerOfMassBit) { + for (b2Body * b = m_bodyList; b; b = b->GetNext()) { + b2Transform xf = b->GetTransform(); + xf.p = b->GetWorldCenter(); + g_debugDraw->DrawTransform(xf); + } + } } -int32 b2World::GetProxyCount() const -{ - return m_contactManager.m_broadPhase.GetProxyCount(); -} +int32 b2World::GetProxyCount() const { return m_contactManager.m_broadPhase.GetProxyCount(); } -int32 b2World::GetTreeHeight() const -{ - return m_contactManager.m_broadPhase.GetTreeHeight(); -} +int32 b2World::GetTreeHeight() const { return m_contactManager.m_broadPhase.GetTreeHeight(); } -int32 b2World::GetTreeBalance() const -{ - return m_contactManager.m_broadPhase.GetTreeBalance(); -} +int32 b2World::GetTreeBalance() const { return m_contactManager.m_broadPhase.GetTreeBalance(); } -float32 b2World::GetTreeQuality() const -{ - return m_contactManager.m_broadPhase.GetTreeQuality(); -} +float32 b2World::GetTreeQuality() const { return m_contactManager.m_broadPhase.GetTreeQuality(); } -void b2World::ShiftOrigin(const b2Vec2& newOrigin) +void b2World::ShiftOrigin(const b2Vec2 & newOrigin) { - b2Assert((m_flags & e_locked) == 0); - if ((m_flags & e_locked) == e_locked) - { - return; - } - - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_xf.p -= newOrigin; - b->m_sweep.c0 -= newOrigin; - b->m_sweep.c -= newOrigin; - } - - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->ShiftOrigin(newOrigin); - } - - m_contactManager.m_broadPhase.ShiftOrigin(newOrigin); + b2Assert((m_flags & e_locked) == 0); + if ((m_flags & e_locked) == e_locked) { + return; + } + + for (b2Body * b = m_bodyList; b; b = b->m_next) { + b->m_xf.p -= newOrigin; + b->m_sweep.c0 -= newOrigin; + b->m_sweep.c -= newOrigin; + } + + for (b2Joint * j = m_jointList; j; j = j->m_next) { + j->ShiftOrigin(newOrigin); + } + + m_contactManager.m_broadPhase.ShiftOrigin(newOrigin); } void b2World::Dump() { - if ((m_flags & e_locked) == e_locked) - { - return; - } - - b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y); - b2Log("m_world->SetGravity(g);\n"); - - b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); - b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount); - int32 i = 0; - for (b2Body* b = m_bodyList; b; b = b->m_next) - { - b->m_islandIndex = i; - b->Dump(); - ++i; - } - - i = 0; - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - j->m_index = i; - ++i; - } - - // First pass on joints, skip gear joints. - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - if (j->m_type == e_gearJoint) - { - continue; - } - - b2Log("{\n"); - j->Dump(); - b2Log("}\n"); - } - - // Second pass on joints, only gear joints. - for (b2Joint* j = m_jointList; j; j = j->m_next) - { - if (j->m_type != e_gearJoint) - { - continue; - } - - b2Log("{\n"); - j->Dump(); - b2Log("}\n"); - } - - b2Log("b2Free(joints);\n"); - b2Log("b2Free(bodies);\n"); - b2Log("joints = nullptr;\n"); - b2Log("bodies = nullptr;\n"); + if ((m_flags & e_locked) == e_locked) { + return; + } + + b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y); + b2Log("m_world->SetGravity(g);\n"); + + b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); + b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount); + int32 i = 0; + for (b2Body * b = m_bodyList; b; b = b->m_next) { + b->m_islandIndex = i; + b->Dump(); + ++i; + } + + i = 0; + for (b2Joint * j = m_jointList; j; j = j->m_next) { + j->m_index = i; + ++i; + } + + // First pass on joints, skip gear joints. + for (b2Joint * j = m_jointList; j; j = j->m_next) { + if (j->m_type == e_gearJoint) { + continue; + } + + b2Log("{\n"); + j->Dump(); + b2Log("}\n"); + } + + // Second pass on joints, only gear joints. + for (b2Joint * j = m_jointList; j; j = j->m_next) { + if (j->m_type != e_gearJoint) { + continue; + } + + b2Log("{\n"); + j->Dump(); + b2Log("}\n"); + } + + b2Log("b2Free(joints);\n"); + b2Log("b2Free(bodies);\n"); + b2Log("joints = nullptr;\n"); + b2Log("bodies = nullptr;\n"); } diff --git a/flatland_box2d/src/Dynamics/b2WorldCallbacks.cpp b/flatland_box2d/src/Dynamics/b2WorldCallbacks.cpp index 573642a8..544a2e1d 100644 --- a/flatland_box2d/src/Dynamics/b2WorldCallbacks.cpp +++ b/flatland_box2d/src/Dynamics/b2WorldCallbacks.cpp @@ -1,36 +1,38 @@ /* -* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Dynamics/b2WorldCallbacks.h" + #include "Box2D/Dynamics/b2Fixture.h" -// Return true if contact calculations should be performed between these two shapes. -// If you implement your own collision filter you may want to build from this implementation. -bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB) +// Return true if contact calculations should be performed between these two +// shapes. If you implement your own collision filter you may want to build from +// this implementation. +bool b2ContactFilter::ShouldCollide(b2Fixture * fixtureA, b2Fixture * fixtureB) { - const b2Filter& filterA = fixtureA->GetFilterData(); - const b2Filter& filterB = fixtureB->GetFilterData(); + const b2Filter & filterA = fixtureA->GetFilterData(); + const b2Filter & filterB = fixtureB->GetFilterData(); - if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0) - { - return filterA.groupIndex > 0; - } + if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0) { + return filterA.groupIndex > 0; + } - bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0; - return collide; + bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && + (filterA.categoryBits & filterB.maskBits) != 0; + return collide; } diff --git a/flatland_box2d/src/Rope/b2Rope.cpp b/flatland_box2d/src/Rope/b2Rope.cpp index 11e31399..821b7a7d 100644 --- a/flatland_box2d/src/Rope/b2Rope.cpp +++ b/flatland_box2d/src/Rope/b2Rope.cpp @@ -1,259 +1,239 @@ /* -* Copyright (c) 2011 Erin Catto http://box2d.org -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Copyright (c) 2011 Erin Catto http://box2d.org + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include "Box2D/Rope/b2Rope.h" + #include "Box2D/Common/b2Draw.h" b2Rope::b2Rope() { - m_count = 0; - m_ps = nullptr; - m_p0s = nullptr; - m_vs = nullptr; - m_ims = nullptr; - m_Ls = nullptr; - m_as = nullptr; - m_gravity.SetZero(); - m_k2 = 1.0f; - m_k3 = 0.1f; + m_count = 0; + m_ps = nullptr; + m_p0s = nullptr; + m_vs = nullptr; + m_ims = nullptr; + m_Ls = nullptr; + m_as = nullptr; + m_gravity.SetZero(); + m_k2 = 1.0f; + m_k3 = 0.1f; } b2Rope::~b2Rope() { - b2Free(m_ps); - b2Free(m_p0s); - b2Free(m_vs); - b2Free(m_ims); - b2Free(m_Ls); - b2Free(m_as); + b2Free(m_ps); + b2Free(m_p0s); + b2Free(m_vs); + b2Free(m_ims); + b2Free(m_Ls); + b2Free(m_as); } -void b2Rope::Initialize(const b2RopeDef* def) +void b2Rope::Initialize(const b2RopeDef * def) { - b2Assert(def->count >= 3); - m_count = def->count; - m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2)); - m_ims = (float32*)b2Alloc(m_count * sizeof(float32)); - - for (int32 i = 0; i < m_count; ++i) - { - m_ps[i] = def->vertices[i]; - m_p0s[i] = def->vertices[i]; - m_vs[i].SetZero(); - - float32 m = def->masses[i]; - if (m > 0.0f) - { - m_ims[i] = 1.0f / m; - } - else - { - m_ims[i] = 0.0f; - } - } - - int32 count2 = m_count - 1; - int32 count3 = m_count - 2; - m_Ls = (float32*)b2Alloc(count2 * sizeof(float32)); - m_as = (float32*)b2Alloc(count3 * sizeof(float32)); - - for (int32 i = 0; i < count2; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i+1]; - m_Ls[i] = b2Distance(p1, p2); - } - - for (int32 i = 0; i < count3; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; - b2Vec2 p3 = m_ps[i + 2]; - - b2Vec2 d1 = p2 - p1; - b2Vec2 d2 = p3 - p2; - - float32 a = b2Cross(d1, d2); - float32 b = b2Dot(d1, d2); - - m_as[i] = b2Atan2(a, b); - } - - m_gravity = def->gravity; - m_damping = def->damping; - m_k2 = def->k2; - m_k3 = def->k3; + b2Assert(def->count >= 3); + m_count = def->count; + m_ps = (b2Vec2 *)b2Alloc(m_count * sizeof(b2Vec2)); + m_p0s = (b2Vec2 *)b2Alloc(m_count * sizeof(b2Vec2)); + m_vs = (b2Vec2 *)b2Alloc(m_count * sizeof(b2Vec2)); + m_ims = (float32 *)b2Alloc(m_count * sizeof(float32)); + + for (int32 i = 0; i < m_count; ++i) { + m_ps[i] = def->vertices[i]; + m_p0s[i] = def->vertices[i]; + m_vs[i].SetZero(); + + float32 m = def->masses[i]; + if (m > 0.0f) { + m_ims[i] = 1.0f / m; + } else { + m_ims[i] = 0.0f; + } + } + + int32 count2 = m_count - 1; + int32 count3 = m_count - 2; + m_Ls = (float32 *)b2Alloc(count2 * sizeof(float32)); + m_as = (float32 *)b2Alloc(count3 * sizeof(float32)); + + for (int32 i = 0; i < count2; ++i) { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + m_Ls[i] = b2Distance(p1, p2); + } + + for (int32 i = 0; i < count3; ++i) { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + b2Vec2 p3 = m_ps[i + 2]; + + b2Vec2 d1 = p2 - p1; + b2Vec2 d2 = p3 - p2; + + float32 a = b2Cross(d1, d2); + float32 b = b2Dot(d1, d2); + + m_as[i] = b2Atan2(a, b); + } + + m_gravity = def->gravity; + m_damping = def->damping; + m_k2 = def->k2; + m_k3 = def->k3; } void b2Rope::Step(float32 h, int32 iterations) { - if (h == 0.0) - { - return; - } - - float32 d = expf(- h * m_damping); - - for (int32 i = 0; i < m_count; ++i) - { - m_p0s[i] = m_ps[i]; - if (m_ims[i] > 0.0f) - { - m_vs[i] += h * m_gravity; - } - m_vs[i] *= d; - m_ps[i] += h * m_vs[i]; - - } - - for (int32 i = 0; i < iterations; ++i) - { - SolveC2(); - SolveC3(); - SolveC2(); - } - - float32 inv_h = 1.0f / h; - for (int32 i = 0; i < m_count; ++i) - { - m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]); - } + if (h == 0.0) { + return; + } + + float32 d = expf(-h * m_damping); + + for (int32 i = 0; i < m_count; ++i) { + m_p0s[i] = m_ps[i]; + if (m_ims[i] > 0.0f) { + m_vs[i] += h * m_gravity; + } + m_vs[i] *= d; + m_ps[i] += h * m_vs[i]; + } + + for (int32 i = 0; i < iterations; ++i) { + SolveC2(); + SolveC3(); + SolveC2(); + } + + float32 inv_h = 1.0f / h; + for (int32 i = 0; i < m_count; ++i) { + m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]); + } } void b2Rope::SolveC2() { - int32 count2 = m_count - 1; + int32 count2 = m_count - 1; - for (int32 i = 0; i < count2; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; + for (int32 i = 0; i < count2; ++i) { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; - b2Vec2 d = p2 - p1; - float32 L = d.Normalize(); + b2Vec2 d = p2 - p1; + float32 L = d.Normalize(); - float32 im1 = m_ims[i]; - float32 im2 = m_ims[i + 1]; + float32 im1 = m_ims[i]; + float32 im2 = m_ims[i + 1]; - if (im1 + im2 == 0.0f) - { - continue; - } + if (im1 + im2 == 0.0f) { + continue; + } - float32 s1 = im1 / (im1 + im2); - float32 s2 = im2 / (im1 + im2); + float32 s1 = im1 / (im1 + im2); + float32 s2 = im2 / (im1 + im2); - p1 -= m_k2 * s1 * (m_Ls[i] - L) * d; - p2 += m_k2 * s2 * (m_Ls[i] - L) * d; + p1 -= m_k2 * s1 * (m_Ls[i] - L) * d; + p2 += m_k2 * s2 * (m_Ls[i] - L) * d; - m_ps[i] = p1; - m_ps[i + 1] = p2; - } + m_ps[i] = p1; + m_ps[i + 1] = p2; + } } void b2Rope::SetAngle(float32 angle) { - int32 count3 = m_count - 2; - for (int32 i = 0; i < count3; ++i) - { - m_as[i] = angle; - } + int32 count3 = m_count - 2; + for (int32 i = 0; i < count3; ++i) { + m_as[i] = angle; + } } void b2Rope::SolveC3() { - int32 count3 = m_count - 2; + int32 count3 = m_count - 2; - for (int32 i = 0; i < count3; ++i) - { - b2Vec2 p1 = m_ps[i]; - b2Vec2 p2 = m_ps[i + 1]; - b2Vec2 p3 = m_ps[i + 2]; + for (int32 i = 0; i < count3; ++i) { + b2Vec2 p1 = m_ps[i]; + b2Vec2 p2 = m_ps[i + 1]; + b2Vec2 p3 = m_ps[i + 2]; - float32 m1 = m_ims[i]; - float32 m2 = m_ims[i + 1]; - float32 m3 = m_ims[i + 2]; + float32 m1 = m_ims[i]; + float32 m2 = m_ims[i + 1]; + float32 m3 = m_ims[i + 2]; - b2Vec2 d1 = p2 - p1; - b2Vec2 d2 = p3 - p2; + b2Vec2 d1 = p2 - p1; + b2Vec2 d2 = p3 - p2; - float32 L1sqr = d1.LengthSquared(); - float32 L2sqr = d2.LengthSquared(); + float32 L1sqr = d1.LengthSquared(); + float32 L2sqr = d2.LengthSquared(); - if (L1sqr * L2sqr == 0.0f) - { - continue; - } + if (L1sqr * L2sqr == 0.0f) { + continue; + } - float32 a = b2Cross(d1, d2); - float32 b = b2Dot(d1, d2); + float32 a = b2Cross(d1, d2); + float32 b = b2Dot(d1, d2); - float32 angle = b2Atan2(a, b); + float32 angle = b2Atan2(a, b); - b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); - b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); + b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew(); + b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew(); - b2Vec2 J1 = -Jd1; - b2Vec2 J2 = Jd1 - Jd2; - b2Vec2 J3 = Jd2; + b2Vec2 J1 = -Jd1; + b2Vec2 J2 = Jd1 - Jd2; + b2Vec2 J3 = Jd2; - float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3); - if (mass == 0.0f) - { - continue; - } + float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3); + if (mass == 0.0f) { + continue; + } - mass = 1.0f / mass; + mass = 1.0f / mass; - float32 C = angle - m_as[i]; + float32 C = angle - m_as[i]; - while (C > b2_pi) - { - angle -= 2 * b2_pi; - C = angle - m_as[i]; - } + while (C > b2_pi) { + angle -= 2 * b2_pi; + C = angle - m_as[i]; + } - while (C < -b2_pi) - { - angle += 2.0f * b2_pi; - C = angle - m_as[i]; - } + while (C < -b2_pi) { + angle += 2.0f * b2_pi; + C = angle - m_as[i]; + } - float32 impulse = - m_k3 * mass * C; + float32 impulse = -m_k3 * mass * C; - p1 += (m1 * impulse) * J1; - p2 += (m2 * impulse) * J2; - p3 += (m3 * impulse) * J3; + p1 += (m1 * impulse) * J1; + p2 += (m2 * impulse) * J2; + p3 += (m3 * impulse) * J3; - m_ps[i] = p1; - m_ps[i + 1] = p2; - m_ps[i + 2] = p3; - } + m_ps[i] = p1; + m_ps[i + 1] = p2; + m_ps[i + 2] = p3; + } } -void b2Rope::Draw(b2Draw* draw) const +void b2Rope::Draw(b2Draw * draw) const { - b2Color c(0.4f, 0.5f, 0.7f); + b2Color c(0.4f, 0.5f, 0.7f); - for (int32 i = 0; i < m_count - 1; ++i) - { - draw->DrawSegment(m_ps[i], m_ps[i+1], c); - } + for (int32 i = 0; i < m_count - 1; ++i) { + draw->DrawSegment(m_ps[i], m_ps[i + 1], c); + } } diff --git a/flatland_plugins/include/flatland_plugins/bool_sensor.h b/flatland_plugins/include/flatland_plugins/bool_sensor.h index f04f29b0..5278dcf7 100644 --- a/flatland_plugins/include/flatland_plugins/bool_sensor.h +++ b/flatland_plugins/include/flatland_plugins/bool_sensor.h @@ -46,6 +46,7 @@ #include #include + #include #include @@ -54,15 +55,17 @@ using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ /** * This class defines a bumper plugin that is used to publish the collisions * states of bodies in the model */ -class BoolSensor : public ModelPlugin { - public: - Body *body_; ///< The body to check collisions with +class BoolSensor : public ModelPlugin +{ +public: + Body * body_; ///< The body to check collisions with double update_rate_; ///< rate to publish message at UpdateTimer update_timer_; ///< for managing update rate @@ -75,26 +78,26 @@ class BoolSensor : public ModelPlugin { * @brief Initialization for the plugin * @param[in] config Plugin YAML Node */ - void OnInitialize(const YAML::Node &config) override; + void OnInitialize(const YAML::Node & config) override; /** * @brief Called when just after physics update to publish state * @param[in] timekeeper Object managing the simulation time */ - void AfterPhysicsStep(const Timekeeper &timekeeper) override; + void AfterPhysicsStep(const Timekeeper & timekeeper) override; /** * @brief A method that is called for all Box2D begin contacts * @param[in] contact Box2D contact */ - void BeginContact(b2Contact *contact) override; + void BeginContact(b2Contact * contact) override; /** * @brief A method that is called for all Box2D end contacts * @param[in] contact Box2D contact */ - void EndContact(b2Contact *contact) override; -}; + void EndContact(b2Contact * contact) override; }; +}; // namespace flatland_plugins #endif \ No newline at end of file diff --git a/flatland_plugins/include/flatland_plugins/bumper.h b/flatland_plugins/include/flatland_plugins/bumper.h index 8129adae..d81d9b12 100644 --- a/flatland_plugins/include/flatland_plugins/bumper.h +++ b/flatland_plugins/include/flatland_plugins/bumper.h @@ -46,6 +46,7 @@ #include #include + #include #include @@ -54,25 +55,28 @@ using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ /** * This class defines a bumper plugin that is used to publish the collisions * states of bodies in the model */ -class Bumper : public ModelPlugin { - public: - struct ContactState { - int num_count; ///< stores number of times post solve is called +class Bumper : public ModelPlugin +{ +public: + struct ContactState + { + int num_count; ///< stores number of times post solve is called double sum_normal_impulses[2]; ///< sum of impulses for averaging later double sum_tangential_impulses[2]; ///< sum of impulses for averaging later - b2Vec2 points[2]; ///< Box2D collision points, max of 2 from Box2D - b2Vec2 normal; ///< normal of collision points, all points have same normal - int normal_sign; ///< for flipping direction of normal when necessary + b2Vec2 points[2]; ///< Box2D collision points, max of 2 from Box2D + b2Vec2 normal; ///< normal of collision points, all points have same normal + int normal_sign; ///< for flipping direction of normal when necessary - Body *body_A; ///< the body of the model involved in the collision - Body *body_B; ///< the other body involved in the collision - Entity *entity_b; /// the entity the other body belongs to + Body * body_A; ///< the body of the model involved in the collision + Body * body_B; ///< the other body involved in the collision + Entity * entity_b; /// the entity the other body belongs to ContactState(); ///< initializes counters and sums void Reset(); ///< Reset counter and sums @@ -89,45 +93,46 @@ class Bumper : public ModelPlugin { /// For keeping track of contacts std::map contact_states_; - rclcpp::Publisher::SharedPtr collisions_publisher_; ///< For publishing the collisions + rclcpp::Publisher::SharedPtr + collisions_publisher_; ///< For publishing the collisions /** * @brief Initialization for the plugin * @param[in] config Plugin YAML Node */ - void OnInitialize(const YAML::Node &config) override; + void OnInitialize(const YAML::Node & config) override; /** * @brief Called when just before physics update * @param[in] timekeeper Object managing the simulation time */ - void BeforePhysicsStep(const Timekeeper &timekeeper) override; + void BeforePhysicsStep(const Timekeeper & timekeeper) override; /** * @brief Called when just after physics update * @param[in] timekeeper Object managing the simulation time */ - void AfterPhysicsStep(const Timekeeper &timekeeper) override; + void AfterPhysicsStep(const Timekeeper & timekeeper) override; /** * @brief A method that is called for all Box2D begin contacts * @param[in] contact Box2D contact */ - void BeginContact(b2Contact *contact) override; + void BeginContact(b2Contact * contact) override; /** * @brief A method that is called for all Box2D end contacts * @param[in] contact Box2D contact */ - void EndContact(b2Contact *contact) override; + void EndContact(b2Contact * contact) override; /* * @brief A method that is called for Box2D presolve * @param[in] contact Box2D contact * @param[in] oldManifold Manifold from the previous iteration */ - void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) override; -}; + void PostSolve(b2Contact * contact, const b2ContactImpulse * impulse) override; }; +}; // namespace flatland_plugins #endif \ No newline at end of file diff --git a/flatland_plugins/include/flatland_plugins/diff_drive.h b/flatland_plugins/include/flatland_plugins/diff_drive.h index ca0d5423..cf53a300 100644 --- a/flatland_plugins/include/flatland_plugins/diff_drive.h +++ b/flatland_plugins/include/flatland_plugins/diff_drive.h @@ -48,10 +48,11 @@ #include #include #include +#include + #include #include #include -#include #include #ifndef FLATLAND_PLUGINS_DIFFDRIVE_H @@ -59,15 +60,17 @@ using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ -class DiffDrive : public flatland_server::ModelPlugin { - public: +class DiffDrive : public flatland_server::ModelPlugin +{ +public: rclcpp::Subscription::SharedPtr twist_sub_; rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Publisher::SharedPtr ground_truth_pub_; rclcpp::Publisher::SharedPtr twist_pub_; - Body* body_; + Body * body_; geometry_msgs::msg::Twist::SharedPtr twist_msg_ = std::make_shared(); nav_msgs::msg::Odometry odom_msg_; nav_msgs::msg::Odometry ground_truth_msg_; @@ -84,13 +87,13 @@ class DiffDrive : public flatland_server::ModelPlugin { * @brief override the BeforePhysicsStep method * @param[in] config The plugin YAML node */ - void OnInitialize(const YAML::Node& config) override; + void OnInitialize(const YAML::Node & config) override; /** * @name BeforePhysicsStep * @brief override the BeforePhysicsStep method * @param[in] config The plugin YAML node */ - void BeforePhysicsStep(const Timekeeper& timekeeper) override; + void BeforePhysicsStep(const Timekeeper & timekeeper) override; /** * @name TwistCallback * @brief callback to apply twist (velocity and omega) @@ -98,6 +101,6 @@ class DiffDrive : public flatland_server::ModelPlugin { */ void TwistCallback(const geometry_msgs::msg::Twist::SharedPtr msg); }; -} +} // namespace flatland_plugins #endif diff --git a/flatland_plugins/include/flatland_plugins/gps.h b/flatland_plugins/include/flatland_plugins/gps.h index da45eefb..105c1ce1 100644 --- a/flatland_plugins/include/flatland_plugins/gps.h +++ b/flatland_plugins/include/flatland_plugins/gps.h @@ -2,48 +2,52 @@ #include #include #include -#include -#include #include + #include +#include +#include #ifndef FLATLAND_PLUGINS_GPS_H #define FLATLAND_PLUGINS_GPS_H using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ /** * This class simulates a GPS receiver in Flatland */ -class Gps : public ModelPlugin { - public: +class Gps : public ModelPlugin +{ +public: std::string topic_; ///< topic name to publish the GPS fix std::string frame_id_; ///< GPS frame ID - Body *body_; ///< body the simulated GPS antenna attaches to + Body * body_; ///< body the simulated GPS antenna attaches to Pose origin_; ///< GPS sensor frame w.r.t the body - double ref_lat_rad_; ///< latitude in radians corresponding to (0, 0) in map - /// frame - double ref_lon_rad_; ///< longitude in radians corresponding to (0, 0) in map - /// frame - double ref_ecef_x_; ///< ECEF coordinates of reference lat and lon at zero - /// altitude - double ref_ecef_y_; ///< ECEF coordinates of reference lat and lon at zero - /// altitude - double ref_ecef_z_; ///< ECEF coordinates of reference lat and lon at zero - /// altitude - double update_rate_; ///< GPS fix publish rate - bool broadcast_tf_; ///< whether to broadcast laser origin w.r.t body + double ref_lat_rad_; ///< latitude in radians corresponding to (0, 0) in map + /// frame + double ref_lon_rad_; ///< longitude in radians corresponding to (0, 0) in map + /// frame + double ref_ecef_x_; ///< ECEF coordinates of reference lat and lon at zero + /// altitude + double ref_ecef_y_; ///< ECEF coordinates of reference lat and lon at zero + /// altitude + double ref_ecef_z_; ///< ECEF coordinates of reference lat and lon at zero + /// altitude + double update_rate_; ///< GPS fix publish rate + bool broadcast_tf_; ///< whether to broadcast laser origin w.r.t body static double WGS84_A; ///< Earth's major axis length static double WGS84_E2; ///< Square of Earth's first eccentricity - rclcpp::Publisher::SharedPtr fix_publisher_; ///< GPS fix topic publisher + rclcpp::Publisher::SharedPtr + fix_publisher_; ///< GPS fix topic publisher std::shared_ptr tf_broadcaster_; ///< broadcast GPS frame - geometry_msgs::msg::TransformStamped gps_tf_; ///< tf from body to GPS frame - sensor_msgs::msg::NavSatFix gps_fix_; ///< message for publishing output - UpdateTimer update_timer_; ///< for controlling update rate + geometry_msgs::msg::TransformStamped gps_tf_; ///< tf from body to GPS frame + sensor_msgs::msg::NavSatFix gps_fix_; ///< message for publishing output + UpdateTimer update_timer_; ///< for controlling update rate Eigen::Matrix3f m_body_to_gps_; ///< tf from body to GPS @@ -51,19 +55,19 @@ class Gps : public ModelPlugin { * @brief Initialization for the plugin * @param[in] config Plugin YAML Node */ - void OnInitialize(const YAML::Node &config) override; + void OnInitialize(const YAML::Node & config) override; /** * @brief Called when just before physics update * @param[in] timekeeper Object managing the simulation time */ - void BeforePhysicsStep(const Timekeeper &timekeeper) override; + void BeforePhysicsStep(const Timekeeper & timekeeper) override; /** * @brief Helper function to extract the paramters from the YAML Node * @param[in] config Plugin YAML Node */ - void ParseParameters(const YAML::Node &config); + void ParseParameters(const YAML::Node & config); /** * @brief Method to compute ECEF coordinates of reference @@ -77,6 +81,6 @@ class Gps : public ModelPlugin { */ void UpdateFix(); }; -} +} // namespace flatland_plugins #endif // FLATLAND_PLUGINS_GPS_H diff --git a/flatland_plugins/include/flatland_plugins/laser.h b/flatland_plugins/include/flatland_plugins/laser.h index 77045c26..c4a69cd9 100644 --- a/flatland_plugins/include/flatland_plugins/laser.h +++ b/flatland_plugins/include/flatland_plugins/laser.h @@ -48,30 +48,33 @@ #include #include #include -#include -#include #include #include -#include + #include #include +#include +#include #include +#include #ifndef FLATLAND_PLUGINS_LASER_H #define FLATLAND_PLUGINS_LASER_H using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ /** * This class implements the model plugin class and provides laser data * for the given configurations */ -class Laser : public ModelPlugin { - public: +class Laser : public ModelPlugin +{ +public: std::string topic_; ///< topic name to publish the laser scan - Body *body_; ///< body the laser frame attaches to + Body * body_; ///< body the laser frame attaches to Pose origin_; ///< laser frame w.r.t the body double range_; ///< laser max range double noise_std_dev_; ///< noise std deviation @@ -93,39 +96,42 @@ class Laser : public ModelPlugin { std::default_random_engine rng_; ///< random generator std::normal_distribution noise_gen_; ///< gaussian noise generator - Eigen::Matrix3f m_body_to_laser_; ///< tf from body to laser - Eigen::Matrix3f m_world_to_body_; ///< tf from world to body - Eigen::Matrix3f m_world_to_laser_; ///< tf from world to laser - Eigen::MatrixXf m_laser_points_; ///< laser points in the laser' frame - Eigen::MatrixXf m_world_laser_points_; /// laser point in the world frame - Eigen::Vector3f v_zero_point_; ///< point representing (0,0) - Eigen::Vector3f v_world_laser_origin_; ///< (0,0) in the laser frame - sensor_msgs::msg::LaserScan laser_scan_; ///< for publishing laser scan - - rclcpp::Publisher::SharedPtr scan_publisher_; ///< ros laser topic publisher - std::shared_ptr tf_broadcaster_; ///< broadcast laser frame - geometry_msgs::msg::TransformStamped laser_tf_; ///< tf from body to laser frame - UpdateTimer update_timer_; ///< for controlling update rate + Eigen::Matrix3f m_body_to_laser_; ///< tf from body to laser + Eigen::Matrix3f m_world_to_body_; ///< tf from world to body + Eigen::Matrix3f m_world_to_laser_; ///< tf from world to laser + Eigen::MatrixXf m_laser_points_; ///< laser points in the laser' frame + Eigen::MatrixXf m_world_laser_points_; /// laser point in the world frame + Eigen::Vector3f v_zero_point_; ///< point representing (0,0) + Eigen::Vector3f v_world_laser_origin_; ///< (0,0) in the laser frame + sensor_msgs::msg::LaserScan laser_scan_; ///< for publishing laser scan + + rclcpp::Publisher::SharedPtr + scan_publisher_; ///< ros laser topic publisher + std::shared_ptr tf_broadcaster_; ///< broadcast laser frame + geometry_msgs::msg::TransformStamped laser_tf_; ///< tf from body to laser frame + UpdateTimer update_timer_; ///< for controlling update rate /** * @brief Constructor to start the threadpool with N+1 threads */ - Laser() : pool_(std::thread::hardware_concurrency() + 1) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("Laser Plugin"), "Laser plugin loaded with " - << (std::thread::hardware_concurrency() + 1) << " threads"); + Laser() : pool_(std::thread::hardware_concurrency() + 1) + { + RCLCPP_INFO_STREAM( + rclcpp::get_logger("Laser Plugin"), + "Laser plugin loaded with " << (std::thread::hardware_concurrency() + 1) << " threads"); }; /** * @brief Initialization for the plugin * @param[in] config Plugin YAML Node */ - void OnInitialize(const YAML::Node &config) override; + void OnInitialize(const YAML::Node & config) override; /** * @brief Called when just before physics update * @param[in] timekeeper Object managing the simulation time */ - void BeforePhysicsStep(const Timekeeper &timekeeper) override; + void BeforePhysicsStep(const Timekeeper & timekeeper) override; /** * @brief Method that contains all of the laser range calculations @@ -136,24 +142,25 @@ class Laser : public ModelPlugin { * @brief helper function to extract the paramters from the YAML Node * @param[in] config Plugin YAML Node */ - void ParseParameters(const YAML::Node &config); + void ParseParameters(const YAML::Node & config); }; /** * This class handles the b2RayCastCallback ReportFixture method * allowing each thread to access its own callback object */ -class LaserCallback : public b2RayCastCallback { - public: +class LaserCallback : public b2RayCastCallback +{ +public: bool did_hit_ = false; ///< Box2D ray trace checking if ray hits anything float fraction_ = 0; ///< Box2D ray trace fraction float intensity_ = 0; ///< Intensity of raytrace collision - Laser *parent_; ///< The parent Laser plugin + Laser * parent_; ///< The parent Laser plugin /** * Default constructor to assign parent object */ - LaserCallback(Laser *parent) : parent_(parent){}; + LaserCallback(Laser * parent) : parent_(parent){}; /** * @brief Box2D raytrace call back method required for implementing the @@ -163,9 +170,9 @@ class LaserCallback : public b2RayCastCallback { * @param[in] normal Vector indicating the normal at the point hit * @param[in] fraction Fraction of ray length at hit point */ - float ReportFixture(b2Fixture *fixture, const b2Vec2 &point, - const b2Vec2 &normal, float fraction) override; + float ReportFixture( + b2Fixture * fixture, const b2Vec2 & point, const b2Vec2 & normal, float fraction) override; }; -} +} // namespace flatland_plugins #endif diff --git a/flatland_plugins/include/flatland_plugins/model_tf_publisher.h b/flatland_plugins/include/flatland_plugins/model_tf_publisher.h index 7588c945..97a8ae6b 100644 --- a/flatland_plugins/include/flatland_plugins/model_tf_publisher.h +++ b/flatland_plugins/include/flatland_plugins/model_tf_publisher.h @@ -47,43 +47,46 @@ #include #include #include -#include #include +#include + #ifndef FLATLAND_PLUGINS_MODEL_TF_PUBLISHER_H #define FLATLAND_PLUGINS_MODEL_TF_PUBLISHER_H using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ /** * This class implements the model plugin and provides the functionality of * publishing ROS TF transformations for the bodies inside a model */ -class ModelTfPublisher : public ModelPlugin { - public: - std::string world_frame_id_; ///< name of the world frame id - bool publish_tf_world_; ///< if to publish the world position of the bodies +class ModelTfPublisher : public ModelPlugin +{ +public: + std::string world_frame_id_; ///< name of the world frame id + bool publish_tf_world_; ///< if to publish the world position of the bodies std::vector excluded_bodies_; ///< list of bodies to ignore - Body *reference_body_; ///< body used as a reference to other bodies - double update_rate_; ///< publish rate + Body * reference_body_; ///< body used as a reference to other bodies + double update_rate_; ///< publish rate - std::shared_ptr tf_broadcaster_; ///< broadcast laser frame - UpdateTimer update_timer_; ///< for managing update rate + std::shared_ptr tf_broadcaster_; ///< broadcast laser frame + UpdateTimer update_timer_; ///< for managing update rate /** - * @brief Initialization for the plugin - * @param[in] config Plugin YAML Node - */ - void OnInitialize(const YAML::Node &config) override; + * @brief Initialization for the plugin + * @param[in] config Plugin YAML Node + */ + void OnInitialize(const YAML::Node & config) override; /** * @brief Called when just before physics update * @param[in] timekeeper Object managing the simulation time */ - void BeforePhysicsStep(const Timekeeper &timekeeper) override; -}; + void BeforePhysicsStep(const Timekeeper & timekeeper) override; }; +}; // namespace flatland_plugins #endif \ No newline at end of file diff --git a/flatland_plugins/include/flatland_plugins/tricycle_drive.h b/flatland_plugins/include/flatland_plugins/tricycle_drive.h index 53e2c933..0eb239ff 100644 --- a/flatland_plugins/include/flatland_plugins/tricycle_drive.h +++ b/flatland_plugins/include/flatland_plugins/tricycle_drive.h @@ -48,6 +48,7 @@ #include #include #include + #include #include #include @@ -59,24 +60,26 @@ using namespace flatland_server; using namespace std; -namespace flatland_plugins { +namespace flatland_plugins +{ -class TricycleDrive : public flatland_server::ModelPlugin { - public: - Body* body_; - Joint* front_wj_; ///< front wheel joint - Joint* rear_left_wj_; ///< rear left wheel joint - Joint* rear_right_wj_; ///< rear right wheel joint - double axel_track_; ///< normal distrance between the rear two wheels - double wheelbase_; ///< distance between the front and rear wheel - b2Vec2 rear_center_; ///< middle point between the two rear wheels +class TricycleDrive : public flatland_server::ModelPlugin +{ +public: + Body * body_; + Joint * front_wj_; ///< front wheel joint + Joint * rear_left_wj_; ///< rear left wheel joint + Joint * rear_right_wj_; ///< rear right wheel joint + double axel_track_; ///< normal distrance between the rear two wheels + double wheelbase_; ///< distance between the front and rear wheel + b2Vec2 rear_center_; ///< middle point between the two rear wheels bool invert_steering_angle_; ///< whether to invert steering angle double max_steer_angle_; ///< max abs. steering allowed [rad] double max_steer_velocity_; ///< max abs. steering velocity [rad/s] double max_steer_acceleration_; ///< max abs. steering acceleration [rad/s^2] - double delta_command_; ///< The current target (commanded) wheel angle - double delta_; ///< The current angular offset of the front wheel - double d_delta_; ///< The current angular speed of the front wheel + double delta_command_; ///< The current target (commanded) wheel angle + double delta_; ///< The current angular offset of the front wheel + double d_delta_; ///< The current angular speed of the front wheel geometry_msgs::msg::Twist::SharedPtr twist_msg_ = std::make_shared(); nav_msgs::msg::Odometry odom_msg_; @@ -95,7 +98,7 @@ class TricycleDrive : public flatland_server::ModelPlugin { * @brief initialize the bicycle plugin * @param world_file The path to the world.yaml file */ - void OnInitialize(const YAML::Node& config) override; + void OnInitialize(const YAML::Node & config) override; /** * @brief This is a helper function that is used to valid and extract @@ -120,13 +123,13 @@ class TricycleDrive : public flatland_server::ModelPlugin { * Some notation and ideas borrowed from * http://ai.stanford.edu/~gabeh/papers/hoffmann_stanley_control07.pdf */ - void BeforePhysicsStep(const Timekeeper& timekeeper) override; + void BeforePhysicsStep(const Timekeeper & timekeeper) override; /** - * @name TwistCallback - * @brief callback to apply twist (velocity and omega) - * @param[in] timestep how much the physics time will increment - */ + * @name TwistCallback + * @brief callback to apply twist (velocity and omega) + * @param[in] timestep how much the physics time will increment + */ void TwistCallback(const geometry_msgs::msg::Twist::SharedPtr msg); /** @@ -138,6 +141,6 @@ class TricycleDrive : public flatland_server::ModelPlugin { */ double Saturate(double in, double lower, double upper); }; -} +} // namespace flatland_plugins #endif diff --git a/flatland_plugins/include/flatland_plugins/tween.h b/flatland_plugins/include/flatland_plugins/tween.h index e07fb62f..7eee7609 100644 --- a/flatland_plugins/include/flatland_plugins/tween.h +++ b/flatland_plugins/include/flatland_plugins/tween.h @@ -49,9 +49,11 @@ #include #include #include + #include #include #include + #include "tweeny.h" #ifndef FLATLAND_PLUGINS_TWEEN_H @@ -59,16 +61,19 @@ using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ -class Tween : public flatland_server::ModelPlugin { - public: - Body* body_; // The body this plugin is attached to +class Tween : public flatland_server::ModelPlugin +{ +public: + Body * body_; // The body this plugin is attached to Pose start_; // The start pose of the model Pose delta_; // The maximum change float duration_; // Seconds to enact change over - rclcpp::Subscription::SharedPtr trigger_sub_; // Handle forward/reverse trigger + rclcpp::Subscription::SharedPtr + trigger_sub_; // Handle forward/reverse trigger bool triggered_ = false; // If true,animate forwards, otherwise backwards tweeny::tween tween_; // The tween object (x,y,theta) @@ -121,13 +126,13 @@ class Tween : public flatland_server::ModelPlugin { * @brief override the BeforePhysicsStep method * @param[in] config The plugin YAML node */ - void OnInitialize(const YAML::Node& config) override; + void OnInitialize(const YAML::Node & config) override; /** * @name BeforePhysicsStep * @brief override the BeforePhysicsStep method * @param[in] config The plugin YAML node */ - void BeforePhysicsStep(const Timekeeper& timekeeper) override; + void BeforePhysicsStep(const Timekeeper & timekeeper) override; /** * @name TriggerCallback @@ -136,6 +141,6 @@ class Tween : public flatland_server::ModelPlugin { */ void TriggerCallback(const std_msgs::msg::Bool::SharedPtr msg); }; -}; +}; // namespace flatland_plugins #endif \ No newline at end of file diff --git a/flatland_plugins/include/flatland_plugins/update_timer.h b/flatland_plugins/include/flatland_plugins/update_timer.h index e53f2731..6a11a8b9 100644 --- a/flatland_plugins/include/flatland_plugins/update_timer.h +++ b/flatland_plugins/include/flatland_plugins/update_timer.h @@ -48,13 +48,16 @@ #define FLATLAND_PLUGINS_UPDATE_TIMER_H #include -#include + #include +#include -namespace flatland_plugins { +namespace flatland_plugins +{ -class UpdateTimer { - public: +class UpdateTimer +{ +public: rclcpp::Duration period_; ///< period of update rclcpp::Time last_update_time_; ///< last time the update occured @@ -75,8 +78,8 @@ class UpdateTimer { * @param[in] timekeeper The object that manages time for the simulation, * update timers get the simulation as well as step size for calculation */ - bool CheckUpdate(const flatland_server::Timekeeper &timekeeper); + bool CheckUpdate(const flatland_server::Timekeeper & timekeeper); }; -} +} // namespace flatland_plugins #endif \ No newline at end of file diff --git a/flatland_plugins/include/flatland_plugins/world_modifier.h b/flatland_plugins/include/flatland_plugins/world_modifier.h index faea73df..1444895d 100644 --- a/flatland_plugins/include/flatland_plugins/world_modifier.h +++ b/flatland_plugins/include/flatland_plugins/world_modifier.h @@ -50,75 +50,81 @@ #include #include #include -#include #include + #include +#include #include #include using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ // sub class for loading laser Param -struct LaserRead { +struct LaserRead +{ std::string name_; // name of the laser b2Vec2 location_; // location of the laser in local frame double range_; // range of the laser double min_angle_; double max_angle_; double increment_; - LaserRead(std::string name, b2Vec2 location, double range, double min_angle, - double max_angle, double increment) - : name_(name), - location_(location), - range_(range), - min_angle_(min_angle), - max_angle_(max_angle), - increment_(increment) {} + LaserRead( + std::string name, b2Vec2 location, double range, double min_angle, double max_angle, + double increment) + : name_(name), + location_(location), + range_(range), + min_angle_(min_angle), + max_angle_(max_angle), + increment_(increment) + { + } }; -struct RayTrace : public b2RayCastCallback { +struct RayTrace : public b2RayCastCallback +{ bool is_hit_; float fraction_; uint16_t category_bits_; - RayTrace(uint16_t category_bits) - : is_hit_(false), category_bits_(category_bits) {} - float ReportFixture(b2Fixture *fixture, const b2Vec2 &point, - const b2Vec2 &normal, float fraction) override; + RayTrace(uint16_t category_bits) : is_hit_(false), category_bits_(category_bits) {} + float ReportFixture( + b2Fixture * fixture, const b2Vec2 & point, const b2Vec2 & normal, float fraction) override; }; -struct WorldModifier { +struct WorldModifier +{ // private members - World *world_; // the world we are modifying + World * world_; // the world we are modifying std::string layer_name_; double wall_wall_dist_; bool double_wall_; Pose robot_ini_pose_; /* - * @brief based on the info regard old wall and d, calculate new obstacle's - * vertices - * @param[in] double d, the sign of this value determine which side of the wall - * will the new obstacle be added - * @param[in] b2Vec2 vertex1, old wall's vertex1 - * @param[in] b2Vec2 vertex2, old wall's vertex2 - * @param[out] b2EdgeShape new_wall, reference passed in to set the vertices - */ - void CalculateNewWall(double d, b2Vec2 vertex1, b2Vec2 vertex2, - b2EdgeShape &new_wall); + * @brief based on the info regard old wall and d, calculate new obstacle's + * vertices + * @param[in] double d, the sign of this value determine which side of the + * wall will the new obstacle be added + * @param[in] b2Vec2 vertex1, old wall's vertex1 + * @param[in] b2Vec2 vertex2, old wall's vertex2 + * @param[out] b2EdgeShape new_wall, reference passed in to set the vertices + */ + void CalculateNewWall(double d, b2Vec2 vertex1, b2Vec2 vertex2, b2EdgeShape & new_wall); /* - * @brief add the new wall into the world - * @param[in] new_wall, the wall that's going to be added - */ - void AddWall(b2EdgeShape &new_wall); + * @brief add the new wall into the world + * @param[in] new_wall, the wall that's going to be added + */ + void AddWall(b2EdgeShape & new_wall); /* - * @brief add two side walls to make it a full obstacle - * @param[in] old_wall, the old wall where new wall is added on top to - * @param[in] new_wall, the new wall got added - */ - void AddSideWall(b2EdgeShape &old_wall, b2EdgeShape &new_wall); + * @brief add two side walls to make it a full obstacle + * @param[in] old_wall, the old wall where new wall is added on top to + * @param[in] new_wall, the new wall got added + */ + void AddSideWall(b2EdgeShape & old_wall, b2EdgeShape & new_wall); /* * @brief constructor for WorldModifier @@ -127,18 +133,19 @@ struct WorldModifier { * @param[in] wall_wall_dist, how thick is the obstacle * @param[in] double_wall, whether add obstacle on both side or not * @param[in] robot_ini_pose, the initial pose of the robot - */ - WorldModifier(flatland_server::World *world, std::string layer_name, - double wall_wall_dist, bool double_wall, Pose robot_ini_pose); + */ + WorldModifier( + flatland_server::World * world, std::string layer_name, double wall_wall_dist, bool double_wall, + Pose robot_ini_pose); /* - * @brief make a new wall in front of the old wall, also add two side walls to - * make a full object - * @param[in] b2EdgeShape *wall, old wall where new wall will be added on top - * to - */ - void AddFullWall(b2EdgeShape *wall); + * @brief make a new wall in front of the old wall, also add two side walls to + * make a full object + * @param[in] b2EdgeShape *wall, old wall where new wall will be added on top + * to + */ + void AddFullWall(b2EdgeShape * wall); }; // class WorldModifier -}; // namespace flatland_server +}; // namespace flatland_plugins #endif // WORLD_MODIFIER_H \ No newline at end of file diff --git a/flatland_plugins/include/flatland_plugins/world_random_wall.h b/flatland_plugins/include/flatland_plugins/world_random_wall.h index 05f1a76d..7b84ed65 100644 --- a/flatland_plugins/include/flatland_plugins/world_random_wall.h +++ b/flatland_plugins/include/flatland_plugins/world_random_wall.h @@ -47,6 +47,7 @@ #include #include #include + #include #include @@ -55,10 +56,12 @@ using namespace flatland_server; -namespace flatland_plugins { -class RandomWall : public WorldPlugin { - void OnInitialize(const YAML::Node &config, YamlReader &world_config) override; -}; +namespace flatland_plugins +{ +class RandomWall : public WorldPlugin +{ + void OnInitialize(const YAML::Node & config, YamlReader & world_config) override; }; +}; // namespace flatland_plugins #endif // FLATLAND_PLUGINS_WORLD_RANDOM_WALL_H \ No newline at end of file diff --git a/flatland_plugins/include/thirdparty/ThreadPool.h b/flatland_plugins/include/thirdparty/ThreadPool.h index d3521bde..a3e28f7e 100644 --- a/flatland_plugins/include/thirdparty/ThreadPool.h +++ b/flatland_plugins/include/thirdparty/ThreadPool.h @@ -26,98 +26,89 @@ distribution. #ifndef THREAD_POOL_H #define THREAD_POOL_H -#include -#include -#include -#include -#include #include -#include #include +#include +#include +#include +#include #include +#include +#include -class ThreadPool { +class ThreadPool +{ public: - ThreadPool(size_t); - template - auto enqueue(F&& f, Args&&... args) - -> std::future::type>; - ~ThreadPool(); + ThreadPool(size_t); + template + auto enqueue(F && f, Args &&... args) -> std::future::type>; + ~ThreadPool(); + private: - // need to keep track of threads so we can join them - std::vector< std::thread > workers; - // the task queue - std::queue< std::function > tasks; - - // synchronization - std::mutex queue_mutex; - std::condition_variable condition; - bool stop; + // need to keep track of threads so we can join them + std::vector workers; + // the task queue + std::queue > tasks; + + // synchronization + std::mutex queue_mutex; + std::condition_variable condition; + bool stop; }; - + // the constructor just launches some amount of workers -inline ThreadPool::ThreadPool(size_t threads) - : stop(false) +inline ThreadPool::ThreadPool(size_t threads) : stop(false) { - for(size_t i = 0;i task; - - { - std::unique_lock lock(this->queue_mutex); - this->condition.wait(lock, - [this]{ return this->stop || !this->tasks.empty(); }); - if(this->stop && this->tasks.empty()) - return; - task = std::move(this->tasks.front()); - this->tasks.pop(); - } - - task(); - } - } - ); + for (size_t i = 0; i < threads; ++i) + workers.emplace_back([this] { + for (;;) { + std::function task; + + { + std::unique_lock lock(this->queue_mutex); + this->condition.wait(lock, [this] { return this->stop || !this->tasks.empty(); }); + if (this->stop && this->tasks.empty()) return; + task = std::move(this->tasks.front()); + this->tasks.pop(); + } + + task(); + } + }); } // add new work item to the pool -template -auto ThreadPool::enqueue(F&& f, Args&&... args) - -> std::future::type> +template +auto ThreadPool::enqueue(F && f, Args &&... args) + -> std::future::type> { - using return_type = typename std::result_of::type; - - auto task = std::make_shared< std::packaged_task >( - std::bind(std::forward(f), std::forward(args)...) - ); - - std::future res = task->get_future(); - { - std::unique_lock lock(queue_mutex); - - // don't allow enqueueing after stopping the pool - if(stop) - throw std::runtime_error("enqueue on stopped ThreadPool"); - - tasks.emplace([task](){ (*task)(); }); - } - condition.notify_one(); - return res; + using return_type = typename std::result_of::type; + + auto task = std::make_shared >( + std::bind(std::forward(f), std::forward(args)...)); + + std::future res = task->get_future(); + { + std::unique_lock lock(queue_mutex); + + // don't allow enqueueing after stopping the pool + if (stop) throw std::runtime_error("enqueue on stopped ThreadPool"); + + tasks.emplace([task]() { (*task)(); }); + } + condition.notify_one(); + return res; } // the destructor joins all threads inline ThreadPool::~ThreadPool() { - { - std::unique_lock lock(queue_mutex); - stop = true; - } - condition.notify_all(); - for(std::thread &worker: workers) - worker.join(); + { + std::unique_lock lock(queue_mutex); + stop = true; + } + condition.notify_all(); + for (std::thread & worker : workers) worker.join(); } #endif diff --git a/flatland_plugins/include/thirdparty/tweeny/dispatcher.h b/flatland_plugins/include/thirdparty/tweeny/dispatcher.h index 97ae234d..1c8fc941 100644 --- a/flatland_plugins/include/thirdparty/tweeny/dispatcher.h +++ b/flatland_plugins/include/thirdparty/tweeny/dispatcher.h @@ -7,47 +7,60 @@ Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to - use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - the Software, and to permit persons to whom the Software is furnished to do so, - subject to the following conditions: + use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is furnished to do + so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. */ -/* This file contains code to help call a function applying a tuple as its arguments. - * This code is private and not documented. */ +/* This file contains code to help call a function applying a tuple as its + * arguments. This code is private and not documented. */ #ifndef TWEENY_DISPATCHER_H #define TWEENY_DISPATCHER_H #include -namespace tweeny { - namespace detail { - template struct seq { }; - template struct gens : gens { }; - template struct gens<0, S...> { - typedef seq type; - }; - - template - R dispatch(Func && f, TupleType && args, seq) { - return f(std::get(args) ...); - } - - template - R call(Func && f, const std::tuple & args) { - return dispatch(f, args, typename gens::type()); - } - } +namespace tweeny +{ +namespace detail +{ +template +struct seq +{ +}; +template +struct gens : gens +{ +}; +template +struct gens<0, S...> +{ + typedef seq type; +}; + +template +R dispatch(Func && f, TupleType && args, seq) +{ + return f(std::get(args)...); +} + +template +R call(Func && f, const std::tuple & args) +{ + return dispatch(f, args, typename gens::type()); } +} // namespace detail +} // namespace tweeny -#endif //TWEENY_DISPATCHER_H +#endif // TWEENY_DISPATCHER_H diff --git a/flatland_plugins/include/thirdparty/tweeny/easing.h b/flatland_plugins/include/thirdparty/tweeny/easing.h index 9e79db0f..eca2387c 100644 --- a/flatland_plugins/include/thirdparty/tweeny/easing.h +++ b/flatland_plugins/include/thirdparty/tweeny/easing.h @@ -7,25 +7,26 @@ Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to - use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - the Software, and to permit persons to whom the Software is furnished to do so, - subject to the following conditions: + use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is furnished to do + so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. */ /** * @file easing.h - * The purpose of this file is to list all bundled easings. All easings are based on Robert Penner's easing - * functions: http://robertpenner.com/easing/ + * The purpose of this file is to list all bundled easings. All easings are + * based on Robert Penner's easing functions: http://robertpenner.com/easing/ */ #ifndef TWEENY_EASING_H @@ -39,13 +40,15 @@ #endif /** - * @defgroup easings Easings - * @brief Bundled easing functions based on - * Robert Penner's Easing Functions - * @details You should plug these functions into @ref tweeny::tween::via function to specify the easing used in a tween. - * @sa tweeny::easing - * @{ - *//** + * @defgroup easings Easings + * @brief Bundled easing functions based on + * Robert Penner's Easing + * Functions + * @details You should plug these functions into @ref tweeny::tween::via + * function to specify the easing used in a tween. + * @sa tweeny::easing + * @{ + *//** * @defgroup linear Linear * @{ * @brief The most boring ever easing function. It has no acceleration and change values in constant speed. @@ -104,459 +107,550 @@ * @} */ -namespace tweeny { - /** - * @brief The easing class holds all the bundled easings. - * - * You should pass the easing function to the @p tweeny::tween::via method, to set the easing function that will - * be used to interpolate values in a tween currentPoint. - * - * **Example**: - * - * @code - * auto tween = tweeny::from(0).to(100).via(tweeny::easing::linear); - * @endcode - */ - class easing { - public: - /** - * @ingroup linear - * @brief Values change with constant speed. - */ - static constexpr struct linearEasing { - template - static typename std::enable_if::value, T>::type run(float position, T start, T end) { - return static_cast(roundf((end - start) * position + start)); - } - - template - static typename std::enable_if::value, T>::type run(float position, T start, T end) { - return static_cast((end - start) * position + start); - } - } linear = linearEasing{}; - - /** - * @ingroup quadratic - * @brief Accelerate initial values with a quadratic equation. - */ - static constexpr struct quadraticInEasing { - template - static T run(float position, T start, T end) { - return static_cast((end - start) * position * position + start); - } - } quadraticIn = quadraticInEasing{}; - - /** - * @ingroup quadratic - * @brief Deaccelerate ending values with a quadratic equation. - */ - static constexpr struct quadraticOutEasing { - template - static T run(float position, T start, T end) { - return static_cast((-(end - start)) * position * (position - 2) + start); - } - } quadraticOut = quadraticOutEasing{}; - - /** - * @ingroup quadratic - * @brief Acceelerate initial and deaccelerate ending values with a quadratic equation. - */ - static constexpr struct quadraticInOutEasing { - template - static T run(float position, T start, T end) { - position *= 2; - if (position < 1) { - return static_cast(((end - start) / 2) * position * position + start); - } - - --position; - return static_cast((-(end - start) / 2) * (position * (position - 2) - 1) + start); - } - } quadraticInOut = quadraticInOutEasing{}; - - /** - * @ingroup cubic - * @brief Aaccelerate initial values with a cubic equation. - */ - static constexpr struct cubicInEasing { - template - static T run(float position, T start, T end) { - return static_cast((end - start) * position * position * position + start); - } - } cubicIn = cubicInEasing{}; - - /** - * @ingroup cubic - * @brief Deaccelerate ending values with a cubic equation. - */ - static constexpr struct cubicOutEasing { - template - static T run(float position, T start, T end) { - --position; - return static_cast((end - start) * (position * position * position + 1) + start); - } - } cubicOut = cubicOutEasing{}; - - /** - * @ingroup cubic - * @brief Acceelerate initial and deaccelerate ending values with a cubic equation. - */ - static constexpr struct cubicInOutEasing { - template - static T run(float position, T start, T end) { - position *= 2; - if (position < 1) { - return static_cast(((end - start) / 2) * position * position * position + start); - } - position -= 2; - return static_cast(((end - start) / 2) * (position * position * position + 2) + start); - } - } cubicInOut = cubicInOutEasing{}; - - /** - * @ingroup quartic - * @brief Acceelerate initial values with a quartic equation. - */ - static constexpr struct quarticInEasing { - template - static T run(float position, T start, T end) { - return static_cast((end - start) * position * position * position * position + start); - } - } quarticIn = quarticInEasing{}; - - /** - * @ingroup quartic - * @brief Deaccelerate ending values with a quartic equation. - */ - static constexpr struct quarticOutEasing { - template - static T run(float position, T start, T end) { - --position; - return static_cast( -(end - start) * (position * position * position * position - 1) + start); - } - } quarticOut = quarticOutEasing{}; - - /** - * @ingroup quartic - * @brief Acceelerate initial and deaccelerate ending values with a quartic equation. - */ - static constexpr struct quarticInOutEasing { - template - static T run(float position, T start, T end) { - position *= 2; - if (position < 1) { - return static_cast(((end - start) / 2) * (position * position * position * position) + - start); - } - position -= 2; - return static_cast((-(end - start) / 2) * (position * position * position * position - 2) + - start); - } - } quarticInOut = quarticInOutEasing{}; - - /** - * @ingroup quintic - * @brief Acceelerate initial values with a quintic equation. - */ - static constexpr struct quinticInEasing { - template - static T run(float position, T start, T end) { - return static_cast((end - start) * position * position * position * position * position + start); - } - } quinticIn = quinticInEasing{}; - - /** - * @ingroup quintic - * @brief Deaccelerate ending values with a quintic equation. - */ - static constexpr struct quinticOutEasing { - template - static T run(float position, T start, T end) { - position--; - return static_cast((end - start) * (position * position * position * position * position + 1) + - start); - } - } quinticOut = quinticOutEasing{}; - - /** - * @ingroup quintic - * @brief Acceelerate initial and deaccelerate ending values with a quintic equation. - */ - static constexpr struct quinticInOutEasing { - template - static T run(float position, T start, T end) { - position *= 2; - if (position < 1) { - return static_cast( - ((end - start) / 2) * (position * position * position * position * position) + - start); - } - position -= 2; - return static_cast( - ((end - start) / 2) * (position * position * position * position * position + 2) + - start); - } - } quinticInOut = quinticInOutEasing{}; - - /** - * @ingroup sinusoidal - * @brief Acceelerate initial values with a sinusoidal equation. - */ - static constexpr struct sinusoidalInEasing { - template - static T run(float position, T start, T end) { - return static_cast(-(end - start) * cosf(position * static_cast(M_PI) / 2) + (end - start) + start); - } - } sinusoidalIn = sinusoidalInEasing{}; - - /** - * @ingroup sinusoidal - * @brief Deaccelerate ending values with a sinusoidal equation. - */ - static constexpr struct sinusoidalOutEasing { - template - static T run(float position, T start, T end) { - return static_cast((end - start) * sinf(position * static_cast(M_PI) / 2) + start); - } - } sinusoidalOut = sinusoidalOutEasing{}; - - /** - * @ingroup sinusoidal - * @brief Acceelerate initial and deaccelerate ending values with a sinusoidal equation. - */ - static constexpr struct sinusoidalInOutEasing { - template - static T run(float position, T start, T end) { - return static_cast((-(end - start) / 2) * (cosf(position * static_cast(M_PI)) - 1) + start); - } - } sinusoidalInOut = sinusoidalInOutEasing{}; - - /** - * @ingroup exponential - * @brief Acceelerate initial values with an exponential equation. - */ - static constexpr struct exponentialInEasing { - template - static T run(float position, T start, T end) { - return static_cast((end - start) * powf(2, 10 * (position - 1)) + start); - } - } exponentialIn = exponentialInEasing{}; - - /** - * @ingroup exponential - * @brief Deaccelerate ending values with an exponential equation. - */ - static constexpr struct exponentialOutEasing { - template - static T run(float position, T start, T end) { - return static_cast((end - start) * (-powf(2, -10 * position) + 1) + start); - } - } exponentialOut = exponentialOutEasing{}; - - /** - * @ingroup exponential - * @brief Acceelerate initial and deaccelerate ending values with an exponential equation. - */ - static constexpr struct exponentialInOutEasing { - template - static T run(float position, T start, T end) { - position *= 2; - if (position < 1) { - return static_cast(((end - start) / 2) * powf(2, 10 * (position - 1)) + start); - } - --position; - return static_cast(((end - start) / 2) * (-powf(2, -10 * position) + 2) + start); - } - } exponentialInOut = exponentialInOutEasing{}; - - /** - * @ingroup circular - * @brief Acceelerate initial values with a circular equation. - */ - static constexpr struct circularInEasing { - template - static T run(float position, T start, T end) { - return static_cast( -(end - start) * (sqrtf(1 - position * position) - 1) + start ); - } - } circularIn = circularInEasing{}; - - /** - * @ingroup circular - * @brief Deaccelerate ending values with a circular equation. - */ - static constexpr struct circularOutEasing { - template - static T run(float position, T start, T end) { - --position; - return static_cast((end - start) * (sqrtf(1 - position * position)) + start); - } - } circularOut = circularOutEasing{}; - - /** - * @ingroup circular - * @brief Acceelerate initial and deaccelerate ending values with a circular equation. - */ - static constexpr struct circularInOutEasing { - template - static T run(float position, T start, T end) { - position *= 2; - if (position < 1) { - return static_cast((-(end - start) / 2) * (sqrtf(1 - position * position) - 1) + start); - } - - position -= 2; - return static_cast(((end - start) / 2) * (sqrtf(1 - position * position) + 1) + start); - } - } circularInOut = circularInOutEasing{}; - - /** - * @ingroup bounce - * @brief Acceelerate initial values with a "bounce" equation. - */ - static constexpr struct bounceInEasing { - template - static T run(float position, T start, T end) { - return (end - start) - bounceOut.run((1 - position), T(), end) + start; - } - } bounceIn = bounceInEasing{}; - - /** - * @ingroup bounce - * @brief Deaccelerate ending values with a "bounce" equation. - */ - static constexpr struct bounceOutEasing { - template - static T run(float position, T start, T end) { - T c = end - start; - if (position < (1 / 2.75f)) { - return static_cast(c * (7.5625f * position * position) + start); - } else if (position < (2.0f / 2.75f)) { - float postFix = position -= (1.5f / 2.75f); - return static_cast(c * (7.5625f * (postFix) * position + .75f) + start); - } else if (position < (2.5f / 2.75f)) { - float postFix = position -= (2.25f / 2.75f); - return static_cast(c * (7.5625f * (postFix) * position + .9375f) + start); - } else { - float postFix = position -= (2.625f / 2.75f); - return static_cast(c * (7.5625f * (postFix) * position + .984375f) + start); - } - } - } bounceOut = bounceOutEasing{}; - - /** - * @ingroup bounce - * @brief Acceelerate initial and deaccelerate ending values with a "bounce" equation. - */ - static constexpr struct bounceInOutEasing { - template - static T run(float position, T start, T end) { - if (position < 0.5f) return static_cast(bounceIn.run(position * 2, T(), end) * .5f + start); - else return static_cast(bounceOut.run((position * 2 - 1), T(), end) * .5f + (end - start) * .5f + start); - } - } bounceInOut = bounceInOutEasing{}; - - /** - * @ingroup elastic - * @brief Acceelerate initial values with an "elastic" equation. - */ - static constexpr struct elasticInEasing { - template - static T run(float position, T start, T end) { - if (position <= 0.00001f) return start; - if (position >= 0.999f) return end; - float p = .3f; - float a = end - start; - float s = p / 4; - float postFix = - a * powf(2, 10 * (position -= 1)); // this is a fix, again, with post-increment operators - return static_cast(-(postFix * sinf((position - s) * (2 * static_cast(M_PI)) / p)) + start); - } - } elasticIn = elasticInEasing{}; - - /** - * @ingroup elastic - * @brief Deaccelerate ending values with an "elastic" equation. - */ - static constexpr struct elasticOutEasing { - template - static T run(float position, T start, T end) { - if (position <= 0.00001f) return start; - if (position >= 0.999f) return end; - float p = .3f; - float a = end - start; - float s = p / 4; - return static_cast(a * powf(2, -10 * position) * sinf((position - s) * (2 * static_cast(M_PI)) / p) + end); - } - } elasticOut = elasticOutEasing{}; - - /** - * @ingroup elastic - * @brief Acceelerate initial and deaccelerate ending values with an "elastic" equation. - */ - static constexpr struct elasticInOutEasing { - template - static T run(float position, T start, T end) { - if (position <= 0.00001f) return start; - if (position >= 0.999f) return end; - position *= 2; - float p = (.3f * 1.5f); - float a = end - start; - float s = p / 4; - float postFix; - - if (position < 1) { - postFix = a * powf(2, 10 * (position -= 1)); // postIncrement is evil - return static_cast(-0.5f * (postFix * sinf((position - s) * (2 * static_cast(M_PI)) / p)) + start); - } - postFix = a * powf(2, -10 * (position -= 1)); // postIncrement is evil - return static_cast(postFix * sinf((position - s) * (2 * static_cast(M_PI)) / p) * .5f + end); - } - } elasticInOut = elasticInOutEasing{}; - - /** - * @ingroup back - * @brief Acceelerate initial values with a "back" equation. - */ - static constexpr struct backInEasing { - template - static T run(float position, T start, T end) { - float s = 1.70158f; - float postFix = position; - return static_cast((end - start) * (postFix) * position * ((s + 1) * position - s) + start); - } - } backIn = backInEasing{}; - - /** - * @ingroup back - * @brief Deaccelerate ending values with a "back" equation. - */ - static constexpr struct backOutEasing { - template - static T run(float position, T start, T end) { - float s = 1.70158f; - position -= 1; - return static_cast((end - start) * ((position) * position * ((s + 1) * position + s) + 1) + start); - } - } backOut = backOutEasing{}; - - /** - * @ingroup back - * @brief Acceelerate initial and deaccelerate ending values with a "back" equation. - */ - static constexpr struct backInOutEasing { - template - static T run(float position, T start, T end) { - float s = 1.70158f; - float t = position; - T b = start; - T c = end - start; - float d = 1; - s *= (1.525f); - if ((t /= d / 2) < 1) return static_cast(c / 2 * (t * t * (((s) + 1) * t - s)) + b); - float postFix = t -= 2; - return static_cast(c / 2 * ((postFix) * t * (((s) + 1) * t + s) + 2) + b); - } - } backInOut = backInOutEasing{}; - }; -} -#endif //TWEENY_EASING_H +namespace tweeny +{ +/** + * @brief The easing class holds all the bundled easings. + * + * You should pass the easing function to the @p tweeny::tween::via method, to + * set the easing function that will be used to interpolate values in a tween + * currentPoint. + * + * **Example**: + * + * @code + * auto tween = tweeny::from(0).to(100).via(tweeny::easing::linear); + * @endcode + */ +class easing +{ +public: + /** + * @ingroup linear + * @brief Values change with constant speed. + */ + static constexpr struct linearEasing + { + template + static typename std::enable_if::value, T>::type run( + float position, T start, T end) + { + return static_cast(roundf((end - start) * position + start)); + } + + template + static typename std::enable_if::value, T>::type run( + float position, T start, T end) + { + return static_cast((end - start) * position + start); + } + } linear = linearEasing{}; + + /** + * @ingroup quadratic + * @brief Accelerate initial values with a quadratic equation. + */ + static constexpr struct quadraticInEasing + { + template + static T run(float position, T start, T end) + { + return static_cast((end - start) * position * position + start); + } + } quadraticIn = quadraticInEasing{}; + + /** + * @ingroup quadratic + * @brief Deaccelerate ending values with a quadratic equation. + */ + static constexpr struct quadraticOutEasing + { + template + static T run(float position, T start, T end) + { + return static_cast((-(end - start)) * position * (position - 2) + start); + } + } quadraticOut = quadraticOutEasing{}; + + /** + * @ingroup quadratic + * @brief Acceelerate initial and deaccelerate ending values with a quadratic + * equation. + */ + static constexpr struct quadraticInOutEasing + { + template + static T run(float position, T start, T end) + { + position *= 2; + if (position < 1) { + return static_cast(((end - start) / 2) * position * position + start); + } + + --position; + return static_cast((-(end - start) / 2) * (position * (position - 2) - 1) + start); + } + } quadraticInOut = quadraticInOutEasing{}; + + /** + * @ingroup cubic + * @brief Aaccelerate initial values with a cubic equation. + */ + static constexpr struct cubicInEasing + { + template + static T run(float position, T start, T end) + { + return static_cast((end - start) * position * position * position + start); + } + } cubicIn = cubicInEasing{}; + + /** + * @ingroup cubic + * @brief Deaccelerate ending values with a cubic equation. + */ + static constexpr struct cubicOutEasing + { + template + static T run(float position, T start, T end) + { + --position; + return static_cast((end - start) * (position * position * position + 1) + start); + } + } cubicOut = cubicOutEasing{}; + + /** + * @ingroup cubic + * @brief Acceelerate initial and deaccelerate ending values with a cubic + * equation. + */ + static constexpr struct cubicInOutEasing + { + template + static T run(float position, T start, T end) + { + position *= 2; + if (position < 1) { + return static_cast(((end - start) / 2) * position * position * position + start); + } + position -= 2; + return static_cast(((end - start) / 2) * (position * position * position + 2) + start); + } + } cubicInOut = cubicInOutEasing{}; + + /** + * @ingroup quartic + * @brief Acceelerate initial values with a quartic equation. + */ + static constexpr struct quarticInEasing + { + template + static T run(float position, T start, T end) + { + return static_cast((end - start) * position * position * position * position + start); + } + } quarticIn = quarticInEasing{}; + + /** + * @ingroup quartic + * @brief Deaccelerate ending values with a quartic equation. + */ + static constexpr struct quarticOutEasing + { + template + static T run(float position, T start, T end) + { + --position; + return static_cast( + -(end - start) * (position * position * position * position - 1) + start); + } + } quarticOut = quarticOutEasing{}; + + /** + * @ingroup quartic + * @brief Acceelerate initial and deaccelerate ending values with a quartic + * equation. + */ + static constexpr struct quarticInOutEasing + { + template + static T run(float position, T start, T end) + { + position *= 2; + if (position < 1) { + return static_cast( + ((end - start) / 2) * (position * position * position * position) + start); + } + position -= 2; + return static_cast( + (-(end - start) / 2) * (position * position * position * position - 2) + start); + } + } quarticInOut = quarticInOutEasing{}; + + /** + * @ingroup quintic + * @brief Acceelerate initial values with a quintic equation. + */ + static constexpr struct quinticInEasing + { + template + static T run(float position, T start, T end) + { + return static_cast( + (end - start) * position * position * position * position * position + start); + } + } quinticIn = quinticInEasing{}; + + /** + * @ingroup quintic + * @brief Deaccelerate ending values with a quintic equation. + */ + static constexpr struct quinticOutEasing + { + template + static T run(float position, T start, T end) + { + position--; + return static_cast( + (end - start) * (position * position * position * position * position + 1) + start); + } + } quinticOut = quinticOutEasing{}; + + /** + * @ingroup quintic + * @brief Acceelerate initial and deaccelerate ending values with a quintic + * equation. + */ + static constexpr struct quinticInOutEasing + { + template + static T run(float position, T start, T end) + { + position *= 2; + if (position < 1) { + return static_cast( + ((end - start) / 2) * (position * position * position * position * position) + start); + } + position -= 2; + return static_cast( + ((end - start) / 2) * (position * position * position * position * position + 2) + start); + } + } quinticInOut = quinticInOutEasing{}; + + /** + * @ingroup sinusoidal + * @brief Acceelerate initial values with a sinusoidal equation. + */ + static constexpr struct sinusoidalInEasing + { + template + static T run(float position, T start, T end) + { + return static_cast( + -(end - start) * cosf(position * static_cast(M_PI) / 2) + (end - start) + start); + } + } sinusoidalIn = sinusoidalInEasing{}; + + /** + * @ingroup sinusoidal + * @brief Deaccelerate ending values with a sinusoidal equation. + */ + static constexpr struct sinusoidalOutEasing + { + template + static T run(float position, T start, T end) + { + return static_cast((end - start) * sinf(position * static_cast(M_PI) / 2) + start); + } + } sinusoidalOut = sinusoidalOutEasing{}; + + /** + * @ingroup sinusoidal + * @brief Acceelerate initial and deaccelerate ending values with a sinusoidal + * equation. + */ + static constexpr struct sinusoidalInOutEasing + { + template + static T run(float position, T start, T end) + { + return static_cast( + (-(end - start) / 2) * (cosf(position * static_cast(M_PI)) - 1) + start); + } + } sinusoidalInOut = sinusoidalInOutEasing{}; + + /** + * @ingroup exponential + * @brief Acceelerate initial values with an exponential equation. + */ + static constexpr struct exponentialInEasing + { + template + static T run(float position, T start, T end) + { + return static_cast((end - start) * powf(2, 10 * (position - 1)) + start); + } + } exponentialIn = exponentialInEasing{}; + + /** + * @ingroup exponential + * @brief Deaccelerate ending values with an exponential equation. + */ + static constexpr struct exponentialOutEasing + { + template + static T run(float position, T start, T end) + { + return static_cast((end - start) * (-powf(2, -10 * position) + 1) + start); + } + } exponentialOut = exponentialOutEasing{}; + + /** + * @ingroup exponential + * @brief Acceelerate initial and deaccelerate ending values with an + * exponential equation. + */ + static constexpr struct exponentialInOutEasing + { + template + static T run(float position, T start, T end) + { + position *= 2; + if (position < 1) { + return static_cast(((end - start) / 2) * powf(2, 10 * (position - 1)) + start); + } + --position; + return static_cast(((end - start) / 2) * (-powf(2, -10 * position) + 2) + start); + } + } exponentialInOut = exponentialInOutEasing{}; + + /** + * @ingroup circular + * @brief Acceelerate initial values with a circular equation. + */ + static constexpr struct circularInEasing + { + template + static T run(float position, T start, T end) + { + return static_cast(-(end - start) * (sqrtf(1 - position * position) - 1) + start); + } + } circularIn = circularInEasing{}; + + /** + * @ingroup circular + * @brief Deaccelerate ending values with a circular equation. + */ + static constexpr struct circularOutEasing + { + template + static T run(float position, T start, T end) + { + --position; + return static_cast((end - start) * (sqrtf(1 - position * position)) + start); + } + } circularOut = circularOutEasing{}; + + /** + * @ingroup circular + * @brief Acceelerate initial and deaccelerate ending values with a circular + * equation. + */ + static constexpr struct circularInOutEasing + { + template + static T run(float position, T start, T end) + { + position *= 2; + if (position < 1) { + return static_cast((-(end - start) / 2) * (sqrtf(1 - position * position) - 1) + start); + } + + position -= 2; + return static_cast(((end - start) / 2) * (sqrtf(1 - position * position) + 1) + start); + } + } circularInOut = circularInOutEasing{}; + + /** + * @ingroup bounce + * @brief Acceelerate initial values with a "bounce" equation. + */ + static constexpr struct bounceInEasing + { + template + static T run(float position, T start, T end) + { + return (end - start) - bounceOut.run((1 - position), T(), end) + start; + } + } bounceIn = bounceInEasing{}; + + /** + * @ingroup bounce + * @brief Deaccelerate ending values with a "bounce" equation. + */ + static constexpr struct bounceOutEasing + { + template + static T run(float position, T start, T end) + { + T c = end - start; + if (position < (1 / 2.75f)) { + return static_cast(c * (7.5625f * position * position) + start); + } else if (position < (2.0f / 2.75f)) { + float postFix = position -= (1.5f / 2.75f); + return static_cast(c * (7.5625f * (postFix)*position + .75f) + start); + } else if (position < (2.5f / 2.75f)) { + float postFix = position -= (2.25f / 2.75f); + return static_cast(c * (7.5625f * (postFix)*position + .9375f) + start); + } else { + float postFix = position -= (2.625f / 2.75f); + return static_cast(c * (7.5625f * (postFix)*position + .984375f) + start); + } + } + } bounceOut = bounceOutEasing{}; + + /** + * @ingroup bounce + * @brief Acceelerate initial and deaccelerate ending values with a "bounce" + * equation. + */ + static constexpr struct bounceInOutEasing + { + template + static T run(float position, T start, T end) + { + if (position < 0.5f) + return static_cast(bounceIn.run(position * 2, T(), end) * .5f + start); + else + return static_cast( + bounceOut.run((position * 2 - 1), T(), end) * .5f + (end - start) * .5f + start); + } + } bounceInOut = bounceInOutEasing{}; + + /** + * @ingroup elastic + * @brief Acceelerate initial values with an "elastic" equation. + */ + static constexpr struct elasticInEasing + { + template + static T run(float position, T start, T end) + { + if (position <= 0.00001f) return start; + if (position >= 0.999f) return end; + float p = .3f; + float a = end - start; + float s = p / 4; + float postFix = + a * powf( + 2, + 10 * (position -= 1)); // this is a fix, again, with post-increment operators + return static_cast( + -(postFix * sinf((position - s) * (2 * static_cast(M_PI)) / p)) + start); + } + } elasticIn = elasticInEasing{}; + + /** + * @ingroup elastic + * @brief Deaccelerate ending values with an "elastic" equation. + */ + static constexpr struct elasticOutEasing + { + template + static T run(float position, T start, T end) + { + if (position <= 0.00001f) return start; + if (position >= 0.999f) return end; + float p = .3f; + float a = end - start; + float s = p / 4; + return static_cast( + a * powf(2, -10 * position) * sinf((position - s) * (2 * static_cast(M_PI)) / p) + + end); + } + } elasticOut = elasticOutEasing{}; + + /** + * @ingroup elastic + * @brief Acceelerate initial and deaccelerate ending values with an "elastic" + * equation. + */ + static constexpr struct elasticInOutEasing + { + template + static T run(float position, T start, T end) + { + if (position <= 0.00001f) return start; + if (position >= 0.999f) return end; + position *= 2; + float p = (.3f * 1.5f); + float a = end - start; + float s = p / 4; + float postFix; + + if (position < 1) { + postFix = a * powf(2, 10 * (position -= 1)); // postIncrement is evil + return static_cast( + -0.5f * (postFix * sinf((position - s) * (2 * static_cast(M_PI)) / p)) + start); + } + postFix = a * powf(2, -10 * (position -= 1)); // postIncrement is evil + return static_cast( + postFix * sinf((position - s) * (2 * static_cast(M_PI)) / p) * .5f + end); + } + } elasticInOut = elasticInOutEasing{}; + + /** + * @ingroup back + * @brief Acceelerate initial values with a "back" equation. + */ + static constexpr struct backInEasing + { + template + static T run(float position, T start, T end) + { + float s = 1.70158f; + float postFix = position; + return static_cast((end - start) * (postFix)*position * ((s + 1) * position - s) + start); + } + } backIn = backInEasing{}; + + /** + * @ingroup back + * @brief Deaccelerate ending values with a "back" equation. + */ + static constexpr struct backOutEasing + { + template + static T run(float position, T start, T end) + { + float s = 1.70158f; + position -= 1; + return static_cast( + (end - start) * ((position)*position * ((s + 1) * position + s) + 1) + start); + } + } backOut = backOutEasing{}; + + /** + * @ingroup back + * @brief Acceelerate initial and deaccelerate ending values with a "back" + * equation. + */ + static constexpr struct backInOutEasing + { + template + static T run(float position, T start, T end) + { + float s = 1.70158f; + float t = position; + T b = start; + T c = end - start; + float d = 1; + s *= (1.525f); + if ((t /= d / 2) < 1) return static_cast(c / 2 * (t * t * (((s) + 1) * t - s)) + b); + float postFix = t -= 2; + return static_cast(c / 2 * ((postFix)*t * (((s) + 1) * t + s) + 2) + b); + } + } backInOut = backInOutEasing{}; +}; +} // namespace tweeny +#endif // TWEENY_EASING_H diff --git a/flatland_plugins/include/thirdparty/tweeny/easingresolve.h b/flatland_plugins/include/thirdparty/tweeny/easingresolve.h index 02119902..b432de55 100644 --- a/flatland_plugins/include/thirdparty/tweeny/easingresolve.h +++ b/flatland_plugins/include/thirdparty/tweeny/easingresolve.h @@ -7,102 +7,118 @@ Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to - use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - the Software, and to permit persons to whom the Software is furnished to do so, - subject to the following conditions: + use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is furnished to do + so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. */ /* - * This file provides the easing resolution mechanism so that the library user can mix lambdas and the bundled - * pre-defined easing functions. It shall not be used directly. - * This file is private. + * This file provides the easing resolution mechanism so that the library user + * can mix lambdas and the bundled pre-defined easing functions. It shall not be + * used directly. This file is private. */ #ifndef TWEENY_EASINGRESOLVE_H #define TWEENY_EASINGRESOLVE_H #include + #include "easing.h" -namespace tweeny { - namespace detail { - using std::get; - - template - struct easingresolve { - static void impl(FunctionTuple &b, Fs... fs) { - if (sizeof...(Fs) == 0) return; - easingresolve::impl(b, fs...); - } - }; - - template - struct easingresolve { - static void impl(FunctionTuple &b, F1 f1, Fs... fs) { - get(b) = f1; - easingresolve::impl(b, fs...); - } - }; - - template - struct easingresolve { - typedef typename std::tuple_element::type ArgType; - - static void impl(FunctionTuple &b, easing::linearEasing, Fs... fs) { - get(b) = easing::linear.run; - easingresolve::impl(b, fs...); - } - }; - - #define DECLARE_EASING_RESOLVE(__EASING_TYPE__) \ - template \ - struct easingresolve { \ - typedef typename std::tuple_element::type ArgType; \ - static void impl(FunctionTuple & b, decltype(easing::__EASING_TYPE__ ## In), Fs... fs) { \ - get(b) = easing::__EASING_TYPE__ ## In.run; \ - easingresolve::impl(b, fs...); \ - } \ - }; \ - \ - template \ - struct easingresolve { \ - typedef typename std::tuple_element::type ArgType; \ - static void impl(FunctionTuple & b, decltype(easing::__EASING_TYPE__ ## Out), Fs... fs) { \ - get(b) = easing::__EASING_TYPE__ ## Out.run; \ - easingresolve::impl(b, fs...); \ - } \ - }; \ - \ - template \ - struct easingresolve { \ - typedef typename std::tuple_element::type ArgType; \ - static void impl(FunctionTuple & b, decltype(easing::__EASING_TYPE__ ## InOut), Fs... fs) { \ - get(b) = easing::__EASING_TYPE__ ## InOut.run; \ - easingresolve::impl(b, fs...); \ - } \ - } - - DECLARE_EASING_RESOLVE(quadratic); - DECLARE_EASING_RESOLVE(cubic); - DECLARE_EASING_RESOLVE(quartic); - DECLARE_EASING_RESOLVE(quintic); - DECLARE_EASING_RESOLVE(sinusoidal); - DECLARE_EASING_RESOLVE(exponential); - DECLARE_EASING_RESOLVE(circular); - DECLARE_EASING_RESOLVE(bounce); - DECLARE_EASING_RESOLVE(elastic); - DECLARE_EASING_RESOLVE(back); - } -} - -#endif //TWEENY_EASINGRESOLVE_H +namespace tweeny +{ +namespace detail +{ +using std::get; + +template +struct easingresolve +{ + static void impl(FunctionTuple & b, Fs... fs) + { + if (sizeof...(Fs) == 0) return; + easingresolve::impl(b, fs...); + } +}; + +template +struct easingresolve +{ + static void impl(FunctionTuple & b, F1 f1, Fs... fs) + { + get(b) = f1; + easingresolve::impl(b, fs...); + } +}; + +template +struct easingresolve +{ + typedef typename std::tuple_element::type ArgType; + + static void impl(FunctionTuple & b, easing::linearEasing, Fs... fs) + { + get(b) = easing::linear.run; + easingresolve::impl(b, fs...); + } +}; + +#define DECLARE_EASING_RESOLVE(__EASING_TYPE__) \ + template \ + struct easingresolve \ + { \ + typedef typename std::tuple_element::type ArgType; \ + static void impl(FunctionTuple & b, decltype(easing::__EASING_TYPE__##In), Fs... fs) \ + { \ + get(b) = easing::__EASING_TYPE__##In.run; \ + easingresolve::impl(b, fs...); \ + } \ + }; \ + \ + template \ + struct easingresolve \ + { \ + typedef typename std::tuple_element::type ArgType; \ + static void impl(FunctionTuple & b, decltype(easing::__EASING_TYPE__##Out), Fs... fs) \ + { \ + get(b) = easing::__EASING_TYPE__##Out.run; \ + easingresolve::impl(b, fs...); \ + } \ + }; \ + \ + template \ + struct easingresolve \ + { \ + typedef typename std::tuple_element::type ArgType; \ + static void impl(FunctionTuple & b, decltype(easing::__EASING_TYPE__##InOut), Fs... fs) \ + { \ + get(b) = easing::__EASING_TYPE__##InOut.run; \ + easingresolve::impl(b, fs...); \ + } \ + } + +DECLARE_EASING_RESOLVE(quadratic); +DECLARE_EASING_RESOLVE(cubic); +DECLARE_EASING_RESOLVE(quartic); +DECLARE_EASING_RESOLVE(quintic); +DECLARE_EASING_RESOLVE(sinusoidal); +DECLARE_EASING_RESOLVE(exponential); +DECLARE_EASING_RESOLVE(circular); +DECLARE_EASING_RESOLVE(bounce); +DECLARE_EASING_RESOLVE(elastic); +DECLARE_EASING_RESOLVE(back); +} // namespace detail +} // namespace tweeny + +#endif // TWEENY_EASINGRESOLVE_H diff --git a/flatland_plugins/include/thirdparty/tweeny/int2type.h b/flatland_plugins/include/thirdparty/tweeny/int2type.h index 047526b4..2f59a961 100644 --- a/flatland_plugins/include/thirdparty/tweeny/int2type.h +++ b/flatland_plugins/include/thirdparty/tweeny/int2type.h @@ -7,31 +7,37 @@ Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to - use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - the Software, and to permit persons to whom the Software is furnished to do so, - subject to the following conditions: + use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is furnished to do + so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. */ /* - * This file declares a helper struct to create a type from a integer value, to aid in template tricks. - * This file is private. + * This file declares a helper struct to create a type from a integer value, to + * aid in template tricks. This file is private. */ #ifndef TWEENY_INT2TYPE_H #define TWEENY_INT2TYPE_H -namespace tweeny { - namespace detail { - template struct int2type { }; - } -} -#endif //TWEENY_INT2TYPE_H +namespace tweeny +{ +namespace detail +{ +template +struct int2type +{ +}; +} // namespace detail +} // namespace tweeny +#endif // TWEENY_INT2TYPE_H diff --git a/flatland_plugins/include/thirdparty/tweeny/tween.h b/flatland_plugins/include/thirdparty/tweeny/tween.h index 1c68d88b..fca7620f 100644 --- a/flatland_plugins/include/thirdparty/tweeny/tween.h +++ b/flatland_plugins/include/thirdparty/tweeny/tween.h @@ -7,19 +7,20 @@ Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to - use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - the Software, and to permit persons to whom the Software is furnished to do so, - subject to the following conditions: + use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is furnished to do + so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. */ /** @@ -30,569 +31,647 @@ #ifndef TWEENY_TWEEN_H #define TWEENY_TWEEN_H +#include #include #include -#include -#include "tweentraits.h" #include "tweenpoint.h" +#include "tweentraits.h" -namespace tweeny { - /** - * @brief The tween class is the core class of tweeny. It controls the interpolation steps, easings and durations. - * - * It should not be constructed manually but rather from @p tweeny::from, to facilitate template argument - * deduction (and also to keep your code clean). - */ - template - class tween { - public: - /** - * @brief Instantiates a tween from a starting currentPoint. - * - * This is a static factory helper function to be used by @p tweeny::from. You should not use this directly. - * @p t The first value in the point - * @p vs The remaining values - */ - static tween from(T t, Ts... vs); - - public: - /** - * @brief Default constructor for a tween - * - * This constructor is provided to facilitate the usage of containers of tweens (e.g, std::vector). It - * should not be used manually as the tweening created by it is invalid. - */ - tween(); - - /** - * @brief Adds a new currentPoint in this tweening. - * - * This will add a new tweening currentPoint with the specified values. Next calls to @p via and @p during - * will refer to this currentPoint. - * - * **Example** - * - * @code - * auto t = tweeny::from(0).to(100).to(200); - * @endcode - * - * @param t, vs Point values - * @returns *this - */ - tween & to(T t, Ts... vs); - - /** - * @brief Specifies the easing function for the last added currentPoint. - * - * This will specify the easing between the last tween currentPoint added by @p to and its previous step. You can - * use any callable object. Additionally, you can use the easing objects specified in the class @p easing. - * - * If it is a multi-value currentPoint, you can either specify a single easing function that will be used for - * every value or you can specify an easing function for each value. You can mix and match callable objects, - * lambdas and bundled easing objects. - * - * **Example**: - * - * @code - * // use bundled linear easing - * auto tween1 = tweeny::from(0).to(100).via(tweeny::easing::linear); - * - * // use custom lambda easing - * auto tween2 = tweeny::from(0).to(100).via([](float p, int a, int b) { return (b-a) * p + a; }); - * @endcode - * - * @param fs The functions - * @returns *this - * @see tweeny::easing - */ - template tween & via(Fs... fs); - - /** - * @brief Specifies the easing function for a specific currentPoint. - * - * Points starts at index 0. The index 0 refers to the first @p to call. - * Using this function without adding a currentPoint with @p to leads to undefined - * behaviour. - * - * @param index The tween currentPoint index - * @param fs The functions - * @returns *this - * @see tweeny::easing - */ - template tween & via(int index, Fs... fs); - - /** - * @brief Specifies the duration, typically in milliseconds, for the tweening of values in last currentPoint. - * - * You can either specify a single duration for all values or give every value its own duration. Value types - * must be convertible to the uint16_t type. - * - * **Example**: - * - * @code - * // Specify that the first currentPoint will be reached in 100 milliseconds and the first value in the second - * // currentPoint in 100, whereas the second value will be reached in 500. - * auto tween = tweeny::from(0, 0).to(100, 200).during(100).to(200, 300).during(100, 500); - * @endcode - * - * @param ds Duration values - * @returns *this - */ - template tween & during(Ds... ds); - - /** - * @brief Steps the animation by the designated delta amount. - * - * You should call this every frame of your application, passing in the amount of delta time that - * you want to animate. - * - * **Example**: - * - * @code - * // tween duration is 100ms - * auto tween = tweeny::from(0).to(100).during(100); - * - * // steps for 16ms - * tween.step(16); - * @endcode - * - * @param dt Delta duration - * @param suppressCallbacks (Optional) Suppress callbacks registered with tween::onStep() - * @returns std::tuple with the current tween values. - */ - const typename detail::tweentraits::valuesType & step(int32_t dt, bool suppressCallbacks = false); - - /** - * @brief Steps the animation by the designated delta amount. - * - * You should call this every frame of your application, passing in the amount of delta time that - * you want to animate. This overload exists to match unsigned int arguments. - * - * @param dt Delta duration - * @param suppressCallbacks (Optional) Suppress callbacks registered with tween::onStep() - * @returns std::tuple with the current tween values. - */ - const typename detail::tweentraits::valuesType & step(uint32_t dt, bool suppressCallbacks = false); - - /** - * @brief Steps the animation by the designated percentage amount. - * - * You can use this function to step the tweening by a specified percentage delta. - - * **Example**: - * - * @code - * // tween duration is 100ms - * auto tween = tweeny::from(0).to(100).during(100); - * - * // steps for 16ms - * tween.step(0.001f); - * @endcode - * - * @param dp Delta percentage, between `0.0f` and `1.0f` - * @param suppressCallbacks (Optional) Suppress callbacks registered with tween::onStep() - * @returns std::tuple with the current tween values. - */ - const typename detail::tweentraits::valuesType & step(float dp, bool suppressCallbacks = false); - - /** - * @brief Seeks to a specified currentPoint in time based on the currentProgress. - * - * This function sets the current animation time and currentProgress. Callbacks set by @p call will be triggered. - * - * @param p The percentage to seek to, between 0.0f and 1.0f, inclusive. - * @param suppressCallbacks (Optional) Suppress callbacks registered with tween::onSeek() - * @returns std::tuple with the current tween values. - */ - const typename detail::tweentraits::valuesType & seek(float p, bool suppressCallbacks = false); - - /** - * @brief Seeks to a specified currentPoint in time. - * - * This function sets the current animation time and currentProgress. Callbacks set by @p call will be triggered. - * - * @param d The duration to seek to, between 0 and the total duration. - * @param suppressCallbacks (Optional) Suppress callbacks registered with tween::onSeek() - * @returns std::tuple with the current tween values. - * @see duration - */ - const typename detail::tweentraits::valuesType & seek(int32_t d, bool suppressCallbacks = false); - - /** - * @brief Seeks to a specified currentPoint in time. - * - * This function sets the current animation time and currentProgress. Callbacks set by @p call will be triggered. - * - * @param d The duration to seek to, between 0 and the total duration. - * @param suppressCallbacks (Optional) Suppress callbacks registered with tween::onSeek() - * @returns std::tuple with the current tween values. - * @see duration - */ - const typename detail::tweentraits::valuesType & seek(uint32_t d, bool suppressCallbacks = false); - - /** - * @brief Adds a callback that will be called when stepping occurs, accepting both the tween and - * its values. - * - * You can add as many callbacks as you want. Its arguments types must be equal to the argument types - * of a tween instance, preceded by a variable of the tween type. Callbacks can be of any callable type. It will only be called - * via tween::step() functions. For seek callbacks, see tween::onSeek(). - * - * Keep in mind that the function will be *copied* into an array, so any variable captured by value - * will also be copied with it. - * - * If the callback returns false, it will be called next time. If it returns true, it will be removed from - * the callback queue. - * - * **Example**: - * - * @code - * auto t = tweeny:from(0).to(100).during(100); - * - * // pass a lambda - * t.onStep([](tweeny::tween & t, int v) { printf("%d ", v); return false; }); - * - * // pass a functor instance - * struct ftor { void operator()(tweeny::tween & t, int v) { printf("%d ", v); return false; } }; - * t.onStep(ftor()); - * @endcode - * @sa step - * @sa seek - * @sa onSeek - * @param callback A callback in with the prototype `bool callback(tween & t, Ts...)` - */ - tween & onStep(typename detail::tweentraits::callbackType callback); - - /** - * @brief Adds a callback that will be called when stepping occurs, accepting only the tween. - * - * You can add as many callbacks as you want. It must receive the tween as an argument. - * Callbacks can be of any callable type. It will only be called - * via tween::step() functions. For seek callbacks, see tween::onSeek(). - * - * Keep in mind that the function will be *copied* into an array, so any variable captured by value - * will also be copied with it. - * - * If the callback returns false, it will be called next time. If it returns true, it will be removed from - * the callback queue. - * - * **Example**: - * - * @code - * auto t = tweeny:from(0).to(100).during(100); - * - * // pass a lambda - * t.onStep([](tweeny::tween & t) { printf("%d ", t.value()); return false; }); - * - * // pass a functor instance - * struct ftor { void operator()(tweeny::tween & t) { printf("%d ", t.values()); return false; } }; - * t.onStep(ftor()); - * @endcode - * @sa step - * @sa seek - * @sa onSeek - * @param callback A callback in the form `bool f(tween & t)` - */ - tween & onStep(typename detail::tweentraits::noValuesCallbackType callback); - - /** - * @brief Adds a callback that will be called when stepping occurs, accepting only the tween values. - * - * You can add as many callbacks as you want. It must receive the tween values as an argument. - * Callbacks can be of any callable type. It will only be called - * via tween::step() functions. For seek callbacks, see tween::onSeek(). - * - * Keep in mind that the function will be *copied* into an array, so any variable captured by value - * will also be copied with it. - * - * If the callback returns false, it will be called next time. If it returns true, it will be removed from - * the callback queue. - * - * **Example**: - * - * @code - * auto t = tweeny:from(0).to(100).during(100); - * - * // pass a lambda - * t.onStep([](int v) { printf("%d ", v); return false; }); - * - * // pass a functor instance - * struct ftor { void operator()(int x) { printf("%d ", x); return false; } }; - * t.onStep(ftor()); - * @endcode - * @sa step - * @sa seek - * @sa onSeek - * @param callback A callback in the form `bool f(Ts...)` - */ - tween & onStep(typename detail::tweentraits::noTweenCallbackType callback); - - /** - * @brief Adds a callback for that will be called when seeking occurs - * - * You can add as many callbacks as you want. Its arguments types must be equal to the argument types - * of a tween instance, preceded by a variable of the tween typve. Callbacks can be of any callable type. It will be called - * via tween::seek() functions. For step callbacks, see tween::onStep(). - * - * Keep in mind that the function will be *copied* into an array, so any variable captured by value - * will also be copied with it. - * - * If the callback returns false, it will be called next time. If it returns true, it will be removed from - * the callback queue. - * - * **Example**: - * - * @code - * auto t = t:from(0).to(100).during(100); - * - * // pass a lambda - * t.onSeek([](tweeny::tween & t, int v) { printf("%d ", v); }); - * - * // pass a functor instance - * struct ftor { void operator()(tweeny::tween & t, int v) { printf("%d ", v); } }; - * t.onSeek(ftor()); - * @endcode - * @param callback A callback in with the prototype `bool callback(tween & t, Ts...)` - */ - tween & onSeek(typename detail::tweentraits::callbackType callback); - - /** - * @brief Adds a callback for that will be called when seeking occurs, accepting only the tween values. - * - * You can add as many callbacks as you want. It must receive the tween as an argument. - * Callbacks can be of any callable type. It will be called - * via tween::seek() functions. For step callbacks, see tween::onStep(). - * - * Keep in mind that the function will be *copied* into an array, so any variable captured by value - * will also be copied again. - * - * If the callback returns false, it will be called next time. If it returns true, it will be removed from - * the callback queue. - * - * **Example**: - * - * @code - * auto t = t:from(0).to(100).during(100); - * - * // pass a lambda - * t.onSeek([](int v) { printf("%d ", v); }); - * - * // pass a functor instance - * struct ftor { void operator()(int v) { printf("%d ", v); return false; } }; - * t.onSeek(ftor()); - * @endcode - * @param callback A callback in the form `bool f(Ts...)` - */ - tween & onSeek(typename detail::tweentraits::noTweenCallbackType callback); - - /** - * @brief Adds a callback for that will be called when seeking occurs, accepting only the tween. - * - * You can add as many callbacks as you want. It must receive the tween as an argument. - * Callbacks can be of any callable type. It will be called - * via tween::seek() functions. For step callbacks, see tween::onStep(). - * - * Keep in mind that the function will be *copied* into an array, so any variable captured by value - * will also be copied again. - * - * If the callback returns false, it will be called next time. If it returns true, it will be removed from - * the callback queue. - * - * **Example**: - * - * @code - * auto t = t:from(0).to(100).during(100); - * - * // pass a lambda - * t.onSeek([](tweeny::tween & t) { printf("%d ", t.value()); return false; }); - * - * // pass a functor instance - * struct ftor { void operator()(tweeny::tween & t) { printf("%d ", t.value()); return false; } }; - * t.onSeek(ftor()); - * @endcode - * @param callback A callback in the form `bool f(tween & t)` - */ - tween & onSeek(typename detail::tweentraits::noValuesCallbackType callback); - - /** - * @brief Returns the total duration of this tween - * - * @returns The duration of all the tween points. - */ - uint32_t duration() const; - - /** - * @brief Returns the current tween values - * - * This returns the current tween value as returned by the - * tween::step() function, except that it does not perform a step. - * @returns std::tuple with the current tween values. - */ - const typename detail::tweentraits::valuesType & peek() const; - - /** - * @brief Calculates and returns the tween values at a given progress - * - * This returns the tween value at the requested progress, without stepping - * or seeking. - * @returns std::tuple with the current tween values. - */ - const typename detail::tweentraits::valuesType peek(float progress) const; - - - /** - * @brief Calculates and return the tween values at a given time - * - * This returns the tween values at the requested time, without stepping - * or seeking. - * @returns std::tuple with the calculated tween values. - */ - const typename detail::tweentraits::valuesType peek(uint32_t time) const; - - /** - * @brief Returns the current currentProgress of the interpolation. - * - * 0 means its at the values passed in the construction, 1 means the last step. - * @returns the current currentProgress between 0 and 1 (inclusive) - */ - float progress() const; - - /** - * @brief Sets the direction of this tween forward. - * - * Note that this only affects tween::step() function. - * @returns *this - * @sa backward - */ - tween & forward(); - - /** - * @brief Sets the direction of this tween backward. - * - * Note that this only affects tween::step() function. - * @returns *this - * @sa forward - */ - tween & backward(); - - /** - * @brief Returns the current direction of this tween - * - * @returns -1 If it is mobin backwards in time, 1 if it is moving forward in time - */ - int direction() const; - - /** - * @brief Jumps to a specific tween currentPoint - * - * This will seek the tween to a percentage matching the beginning of that step. - * - * @param point The currentPoint to seek to. 0 means the currentPoint passed in tweeny::from - * @param suppressCallbacks (optional) set to true to suppress seek() callbacks - * @returns current values - * @sa seek - */ - const typename detail::tweentraits::valuesType & jump(int32_t point, bool suppressCallbacks = false); - - /** - * @brief Returns the current tween point - * - * @returns Current tween point - */ - uint16_t point() const; - - private /* member types */: - using traits = detail::tweentraits; - - private /* member variables */: - uint32_t total = 0; // total runtime - uint16_t currentPoint = 0; // current currentPoint - float currentProgress = 0; // current currentProgress - std::vector> points; - typename traits::valuesType current; - std::vector onStepCallbacks; - std::vector onSeekCallbacks; - int8_t currentDirection = 1; - - private: - /* member functions */ - tween(T t, Ts... vs); - template void interpolate(float prog, unsigned point, typename traits::valuesType & values, detail::int2type) const; - void interpolate(float prog, unsigned point, typename traits::valuesType & values, detail::int2type<0>) const; - void render(float p); - void dispatch(std::vector & cbVector); - uint16_t pointAt(float progress) const; - }; - - /** - * @brief Class specialization when a tween has a single value - * - * This class is preferred automatically by your compiler when your tween has only one value. It exists mainly - * so that you dont need to use std::get<0> to obtain a single value when using tween::step, tween::seek or any other - * value returning function. Other than that, you should look at the - * tweeny::tween documentation. - * - * Except for this little detail, this class methods and behaviours are exactly the same. - */ - template - class tween { - public: - static tween from(T t); - - public: - tween(); ///< @sa tween::tween - tween & to(T t); ///< @sa tween::to - template tween & via(Fs... fs); ///< @sa tween::via - template tween & via(int index, Fs... fs); ///< @sa tween::via - template tween & during(Ds... ds); ///< @sa tween::during - const T & step(int32_t dt, bool suppressCallbacks = false); ///< @sa tween::step(int32_t dt, bool suppressCallbacks) - const T & step(uint32_t dt, bool suppressCallbacks = false); ///< @sa tween::step(uint32_t dt, bool suppressCallbacks) - const T & step(float dp, bool suppressCallbacks = false); ///< @sa tween::step(float dp, bool suppressCallbacks) - const T & seek(float p, bool suppressCallbacks = false); ///< @sa tween::seek(float p, bool suppressCallbacks) - const T & seek(int32_t d, bool suppressCallbacks = false); ///< @sa tween::seek(int32_t d, bool suppressCallbacks) - const T & seek(uint32_t d, bool suppressCallbacks = false); ///< @sa tween::seek(uint32_t d, bool suppressCallbacks) - tween & onStep(typename detail::tweentraits::callbackType callback); ///< @sa tween::onStep - tween & onStep(typename detail::tweentraits::noValuesCallbackType callback); ///< @sa tween::onStep - tween & onStep(typename detail::tweentraits::noTweenCallbackType callback); ///< @sa tween::onStep - tween & onSeek(typename detail::tweentraits::callbackType callback); ///< @sa tween::onSeek - tween & onSeek(typename detail::tweentraits::noValuesCallbackType callback); ///< @sa tween::onSeek - tween & onSeek(typename detail::tweentraits::noTweenCallbackType callback); ///< @sa tween::onSeek - const T & peek() const; ///< @sa tween::peek - T peek(float progress) const; ///< @sa tween::peek - T peek(uint32_t time) const; ///< @sa tween::peek - uint32_t duration() const; ///< @sa tween::duration - float progress() const; ///< @sa tween::progress - tween & forward(); ///< @sa tween::forward - tween & backward(); ///< @sa tween::backward - int direction() const; ///< @sa tween::direction - const T & jump(int32_t point, bool suppressCallbacks = false); ///< @sa tween::jump - uint16_t point() const; ///< @sa tween::point - - private /* member types */: - using traits = detail::tweentraits; - - private /* member variables */: - uint32_t total = 0; // total runtime - uint16_t currentPoint = 0; // current currentPoint - float currentProgress = 0; // current currentProgress - std::vector> points; - T current; - std::vector onStepCallbacks; - std::vector onSeekCallbacks; - int8_t currentDirection = 1; - - private: - /* member functions */ - tween(T t); - void interpolate(float prog, unsigned point, T & value) const; - void render(float p); - void dispatch(std::vector & cbVector); - uint16_t pointAt(float progress) const; - }; -} +namespace tweeny +{ +/** + * @brief The tween class is the core class of tweeny. It controls the + * interpolation steps, easings and durations. + * + * It should not be constructed manually but rather from @p tweeny::from, to + * facilitate template argument deduction (and also to keep your code clean). + */ +template +class tween +{ +public: + /** + * @brief Instantiates a tween from a starting currentPoint. + * + * This is a static factory helper function to be used by @p tweeny::from. You + * should not use this directly. + * @p t The first value in the point + * @p vs The remaining values + */ + static tween from(T t, Ts... vs); + +public: + /** + * @brief Default constructor for a tween + * + * This constructor is provided to facilitate the usage of containers of + * tweens (e.g, std::vector). It should not be used manually as the tweening + * created by it is invalid. + */ + tween(); + + /** + * @brief Adds a new currentPoint in this tweening. + * + * This will add a new tweening currentPoint with the specified values. Next + * calls to @p via and @p during will refer to this currentPoint. + * + * **Example** + * + * @code + * auto t = tweeny::from(0).to(100).to(200); + * @endcode + * + * @param t, vs Point values + * @returns *this + */ + tween & to(T t, Ts... vs); + + /** + * @brief Specifies the easing function for the last added currentPoint. + * + * This will specify the easing between the last tween currentPoint added by + * @p to and its previous step. You can use any callable object. Additionally, + * you can use the easing objects specified in the class @p easing. + * + * If it is a multi-value currentPoint, you can either specify a single easing + * function that will be used for every value or you can specify an easing + * function for each value. You can mix and match callable objects, lambdas + * and bundled easing objects. + * + * **Example**: + * + * @code + * // use bundled linear easing + * auto tween1 = tweeny::from(0).to(100).via(tweeny::easing::linear); + * + * // use custom lambda easing + * auto tween2 = tweeny::from(0).to(100).via([](float p, int a, int b) { + * return (b-a) * p + a; }); + * @endcode + * + * @param fs The functions + * @returns *this + * @see tweeny::easing + */ + template + tween & via(Fs... fs); + + /** + * @brief Specifies the easing function for a specific currentPoint. + * + * Points starts at index 0. The index 0 refers to the first @p to call. + * Using this function without adding a currentPoint with @p to leads to + * undefined behaviour. + * + * @param index The tween currentPoint index + * @param fs The functions + * @returns *this + * @see tweeny::easing + */ + template + tween & via(int index, Fs... fs); + + /** + * @brief Specifies the duration, typically in milliseconds, for the tweening + * of values in last currentPoint. + * + * You can either specify a single duration for all values or give every value + * its own duration. Value types must be convertible to the uint16_t type. + * + * **Example**: + * + * @code + * // Specify that the first currentPoint will be reached in 100 milliseconds + * and the first value in the second + * // currentPoint in 100, whereas the second value will be reached in 500. + * auto tween = tweeny::from(0, 0).to(100, 200).during(100).to(200, + * 300).during(100, 500); + * @endcode + * + * @param ds Duration values + * @returns *this + */ + template + tween & during(Ds... ds); + + /** + * @brief Steps the animation by the designated delta amount. + * + * You should call this every frame of your application, passing in the amount + * of delta time that you want to animate. + * + * **Example**: + * + * @code + * // tween duration is 100ms + * auto tween = tweeny::from(0).to(100).during(100); + * + * // steps for 16ms + * tween.step(16); + * @endcode + * + * @param dt Delta duration + * @param suppressCallbacks (Optional) Suppress callbacks registered with + * tween::onStep() + * @returns std::tuple with the current tween values. + */ + const typename detail::tweentraits::valuesType & step( + int32_t dt, bool suppressCallbacks = false); + + /** + * @brief Steps the animation by the designated delta amount. + * + * You should call this every frame of your application, passing in the amount + * of delta time that you want to animate. This overload exists to match + * unsigned int arguments. + * + * @param dt Delta duration + * @param suppressCallbacks (Optional) Suppress callbacks registered with + * tween::onStep() + * @returns std::tuple with the current tween values. + */ + const typename detail::tweentraits::valuesType & step( + uint32_t dt, bool suppressCallbacks = false); + + /** + * @brief Steps the animation by the designated percentage amount. + * + * You can use this function to step the tweening by a specified percentage + delta. + + * **Example**: + * + * @code + * // tween duration is 100ms + * auto tween = tweeny::from(0).to(100).during(100); + * + * // steps for 16ms + * tween.step(0.001f); + * @endcode + * + * @param dp Delta percentage, between `0.0f` and `1.0f` + * @param suppressCallbacks (Optional) Suppress callbacks registered with + tween::onStep() + * @returns std::tuple with the current tween values. + */ + const typename detail::tweentraits::valuesType & step( + float dp, bool suppressCallbacks = false); + + /** + * @brief Seeks to a specified currentPoint in time based on the + * currentProgress. + * + * This function sets the current animation time and currentProgress. + * Callbacks set by @p call will be triggered. + * + * @param p The percentage to seek to, between 0.0f and 1.0f, inclusive. + * @param suppressCallbacks (Optional) Suppress callbacks registered with + * tween::onSeek() + * @returns std::tuple with the current tween values. + */ + const typename detail::tweentraits::valuesType & seek( + float p, bool suppressCallbacks = false); + + /** + * @brief Seeks to a specified currentPoint in time. + * + * This function sets the current animation time and currentProgress. + * Callbacks set by @p call will be triggered. + * + * @param d The duration to seek to, between 0 and the total duration. + * @param suppressCallbacks (Optional) Suppress callbacks registered with + * tween::onSeek() + * @returns std::tuple with the current tween values. + * @see duration + */ + const typename detail::tweentraits::valuesType & seek( + int32_t d, bool suppressCallbacks = false); + + /** + * @brief Seeks to a specified currentPoint in time. + * + * This function sets the current animation time and currentProgress. + * Callbacks set by @p call will be triggered. + * + * @param d The duration to seek to, between 0 and the total duration. + * @param suppressCallbacks (Optional) Suppress callbacks registered with + * tween::onSeek() + * @returns std::tuple with the current tween values. + * @see duration + */ + const typename detail::tweentraits::valuesType & seek( + uint32_t d, bool suppressCallbacks = false); + + /** + * @brief Adds a callback that will be called when stepping occurs, accepting + * both the tween and its values. + * + * You can add as many callbacks as you want. Its arguments types must be + * equal to the argument types of a tween instance, preceded by a variable of + * the tween type. Callbacks can be of any callable type. It will only be + * called via tween::step() functions. For seek callbacks, see + * tween::onSeek(). + * + * Keep in mind that the function will be *copied* into an array, so any + * variable captured by value will also be copied with it. + * + * If the callback returns false, it will be called next time. If it returns + * true, it will be removed from the callback queue. + * + * **Example**: + * + * @code + * auto t = tweeny:from(0).to(100).during(100); + * + * // pass a lambda + * t.onStep([](tweeny::tween & t, int v) { printf("%d ", v); return + * false; }); + * + * // pass a functor instance + * struct ftor { void operator()(tweeny::tween & t, int v) { printf("%d + * ", v); return false; } }; t.onStep(ftor()); + * @endcode + * @sa step + * @sa seek + * @sa onSeek + * @param callback A callback in with the prototype `bool + * callback(tween & t, Ts...)` + */ + tween & onStep(typename detail::tweentraits::callbackType callback); + + /** + * @brief Adds a callback that will be called when stepping occurs, accepting + * only the tween. + * + * You can add as many callbacks as you want. It must receive the tween as an + * argument. Callbacks can be of any callable type. It will only be called via + * tween::step() functions. For seek callbacks, see tween::onSeek(). + * + * Keep in mind that the function will be *copied* into an array, so any + * variable captured by value will also be copied with it. + * + * If the callback returns false, it will be called next time. If it returns + * true, it will be removed from the callback queue. + * + * **Example**: + * + * @code + * auto t = tweeny:from(0).to(100).during(100); + * + * // pass a lambda + * t.onStep([](tweeny::tween & t) { printf("%d ", t.value()); return + * false; }); + * + * // pass a functor instance + * struct ftor { void operator()(tweeny::tween & t) { printf("%d ", + * t.values()); return false; } }; t.onStep(ftor()); + * @endcode + * @sa step + * @sa seek + * @sa onSeek + * @param callback A callback in the form `bool f(tween & t)` + */ + tween & onStep(typename detail::tweentraits::noValuesCallbackType callback); + + /** + * @brief Adds a callback that will be called when stepping occurs, accepting + * only the tween values. + * + * You can add as many callbacks as you want. It must receive the tween values + * as an argument. Callbacks can be of any callable type. It will only be + * called via tween::step() functions. For seek callbacks, see + * tween::onSeek(). + * + * Keep in mind that the function will be *copied* into an array, so any + * variable captured by value will also be copied with it. + * + * If the callback returns false, it will be called next time. If it returns + * true, it will be removed from the callback queue. + * + * **Example**: + * + * @code + * auto t = tweeny:from(0).to(100).during(100); + * + * // pass a lambda + * t.onStep([](int v) { printf("%d ", v); return false; }); + * + * // pass a functor instance + * struct ftor { void operator()(int x) { printf("%d ", x); return false; } }; + * t.onStep(ftor()); + * @endcode + * @sa step + * @sa seek + * @sa onSeek + * @param callback A callback in the form `bool f(Ts...)` + */ + tween & onStep(typename detail::tweentraits::noTweenCallbackType callback); + + /** + * @brief Adds a callback for that will be called when seeking occurs + * + * You can add as many callbacks as you want. Its arguments types must be + * equal to the argument types of a tween instance, preceded by a variable of + * the tween typve. Callbacks can be of any callable type. It will be called + * via tween::seek() functions. For step callbacks, see tween::onStep(). + * + * Keep in mind that the function will be *copied* into an array, so any + * variable captured by value will also be copied with it. + * + * If the callback returns false, it will be called next time. If it returns + * true, it will be removed from the callback queue. + * + * **Example**: + * + * @code + * auto t = t:from(0).to(100).during(100); + * + * // pass a lambda + * t.onSeek([](tweeny::tween & t, int v) { printf("%d ", v); }); + * + * // pass a functor instance + * struct ftor { void operator()(tweeny::tween & t, int v) { printf("%d + * ", v); } }; t.onSeek(ftor()); + * @endcode + * @param callback A callback in with the prototype `bool + * callback(tween & t, Ts...)` + */ + tween & onSeek(typename detail::tweentraits::callbackType callback); + + /** + * @brief Adds a callback for that will be called when seeking occurs, + * accepting only the tween values. + * + * You can add as many callbacks as you want. It must receive the tween as an + * argument. Callbacks can be of any callable type. It will be called via + * tween::seek() functions. For step callbacks, see tween::onStep(). + * + * Keep in mind that the function will be *copied* into an array, so any + * variable captured by value will also be copied again. + * + * If the callback returns false, it will be called next time. If it returns + * true, it will be removed from the callback queue. + * + * **Example**: + * + * @code + * auto t = t:from(0).to(100).during(100); + * + * // pass a lambda + * t.onSeek([](int v) { printf("%d ", v); }); + * + * // pass a functor instance + * struct ftor { void operator()(int v) { printf("%d ", v); return false; } }; + * t.onSeek(ftor()); + * @endcode + * @param callback A callback in the form `bool f(Ts...)` + */ + tween & onSeek(typename detail::tweentraits::noTweenCallbackType callback); + + /** + * @brief Adds a callback for that will be called when seeking occurs, + * accepting only the tween. + * + * You can add as many callbacks as you want. It must receive the tween as an + * argument. Callbacks can be of any callable type. It will be called via + * tween::seek() functions. For step callbacks, see tween::onStep(). + * + * Keep in mind that the function will be *copied* into an array, so any + * variable captured by value will also be copied again. + * + * If the callback returns false, it will be called next time. If it returns + * true, it will be removed from the callback queue. + * + * **Example**: + * + * @code + * auto t = t:from(0).to(100).during(100); + * + * // pass a lambda + * t.onSeek([](tweeny::tween & t) { printf("%d ", t.value()); return + * false; }); + * + * // pass a functor instance + * struct ftor { void operator()(tweeny::tween & t) { printf("%d ", + * t.value()); return false; } }; t.onSeek(ftor()); + * @endcode + * @param callback A callback in the form `bool f(tween & t)` + */ + tween & onSeek(typename detail::tweentraits::noValuesCallbackType callback); + + /** + * @brief Returns the total duration of this tween + * + * @returns The duration of all the tween points. + */ + uint32_t duration() const; + + /** + * @brief Returns the current tween values + * + * This returns the current tween value as returned by the + * tween::step() function, except that it does not perform a step. + * @returns std::tuple with the current tween values. + */ + const typename detail::tweentraits::valuesType & peek() const; + + /** + * @brief Calculates and returns the tween values at a given progress + * + * This returns the tween value at the requested progress, without stepping + * or seeking. + * @returns std::tuple with the current tween values. + */ + const typename detail::tweentraits::valuesType peek(float progress) const; + + /** + * @brief Calculates and return the tween values at a given time + * + * This returns the tween values at the requested time, without stepping + * or seeking. + * @returns std::tuple with the calculated tween values. + */ + const typename detail::tweentraits::valuesType peek(uint32_t time) const; + + /** + * @brief Returns the current currentProgress of the interpolation. + * + * 0 means its at the values passed in the construction, 1 means the last + * step. + * @returns the current currentProgress between 0 and 1 (inclusive) + */ + float progress() const; + + /** + * @brief Sets the direction of this tween forward. + * + * Note that this only affects tween::step() function. + * @returns *this + * @sa backward + */ + tween & forward(); + + /** + * @brief Sets the direction of this tween backward. + * + * Note that this only affects tween::step() function. + * @returns *this + * @sa forward + */ + tween & backward(); + + /** + * @brief Returns the current direction of this tween + * + * @returns -1 If it is mobin backwards in time, 1 if it is moving forward in + * time + */ + int direction() const; + + /** + * @brief Jumps to a specific tween currentPoint + * + * This will seek the tween to a percentage matching the beginning of that + * step. + * + * @param point The currentPoint to seek to. 0 means the currentPoint passed + * in tweeny::from + * @param suppressCallbacks (optional) set to true to suppress seek() + * callbacks + * @returns current values + * @sa seek + */ + const typename detail::tweentraits::valuesType & jump( + int32_t point, bool suppressCallbacks = false); + + /** + * @brief Returns the current tween point + * + * @returns Current tween point + */ + uint16_t point() const; + + private /* member types */: + using traits = detail::tweentraits; + + private /* member variables */: + uint32_t total = 0; // total runtime + uint16_t currentPoint = 0; // current currentPoint + float currentProgress = 0; // current currentProgress + std::vector> points; + typename traits::valuesType current; + std::vector onStepCallbacks; + std::vector onSeekCallbacks; + int8_t currentDirection = 1; + +private: + /* member functions */ + tween(T t, Ts... vs); + template + void interpolate( + float prog, unsigned point, typename traits::valuesType & values, detail::int2type) const; + void interpolate( + float prog, unsigned point, typename traits::valuesType & values, detail::int2type<0>) const; + void render(float p); + void dispatch(std::vector & cbVector); + uint16_t pointAt(float progress) const; +}; + +/** + * @brief Class specialization when a tween has a single value + * + * This class is preferred automatically by your compiler when your tween has + * only one value. It exists mainly so that you dont need to use std::get<0> to + * obtain a single value when using tween::step, tween::seek or any other value + * returning function. Other than that, you should look at the tweeny::tween + * documentation. + * + * Except for this little detail, this class methods and behaviours are exactly + * the same. + */ +template +class tween +{ +public: + static tween from(T t); + +public: + tween(); ///< @sa tween::tween + tween & to(T t); ///< @sa tween::to + template + tween & via(Fs... fs); ///< @sa tween::via + template + tween & via(int index, Fs... fs); ///< @sa tween::via + template + tween & during(Ds... ds); ///< @sa tween::during + const T & step( + int32_t dt, + bool suppressCallbacks = false); ///< @sa tween::step(int32_t dt, bool suppressCallbacks) + const T & step( + uint32_t dt, + bool suppressCallbacks = false); ///< @sa tween::step(uint32_t dt, bool suppressCallbacks) + const T & step( + float dp, + bool suppressCallbacks = false); ///< @sa tween::step(float dp, + ///< bool suppressCallbacks) + const T & seek( + float p, + bool suppressCallbacks = false); ///< @sa tween::seek(float p, + ///< bool suppressCallbacks) + const T & seek( + int32_t d, + bool suppressCallbacks = false); ///< @sa tween::seek(int32_t d, bool suppressCallbacks) + const T & seek( + uint32_t d, + bool suppressCallbacks = false); ///< @sa tween::seek(uint32_t d, bool suppressCallbacks) + tween & onStep(typename detail::tweentraits::callbackType callback); ///< @sa tween::onStep + tween & onStep( + typename detail::tweentraits::noValuesCallbackType callback); ///< @sa tween::onStep + tween & onStep( + typename detail::tweentraits::noTweenCallbackType callback); ///< @sa tween::onStep + tween & onSeek(typename detail::tweentraits::callbackType callback); ///< @sa tween::onSeek + tween & onSeek( + typename detail::tweentraits::noValuesCallbackType callback); ///< @sa tween::onSeek + tween & onSeek( + typename detail::tweentraits::noTweenCallbackType callback); ///< @sa tween::onSeek + const T & peek() const; ///< @sa tween::peek + T peek(float progress) const; ///< @sa tween::peek + T peek(uint32_t time) const; ///< @sa tween::peek + uint32_t duration() const; ///< @sa tween::duration + float progress() const; ///< @sa tween::progress + tween & forward(); ///< @sa tween::forward + tween & backward(); ///< @sa tween::backward + int direction() const; ///< @sa tween::direction + const T & jump(int32_t point, + bool suppressCallbacks = false); ///< @sa tween::jump + uint16_t point() const; ///< @sa tween::point + + private /* member types */: + using traits = detail::tweentraits; + + private /* member variables */: + uint32_t total = 0; // total runtime + uint16_t currentPoint = 0; // current currentPoint + float currentProgress = 0; // current currentProgress + std::vector> points; + T current; + std::vector onStepCallbacks; + std::vector onSeekCallbacks; + int8_t currentDirection = 1; + +private: + /* member functions */ + tween(T t); + void interpolate(float prog, unsigned point, T & value) const; + void render(float p); + void dispatch(std::vector & cbVector); + uint16_t pointAt(float progress) const; +}; +} // namespace tweeny #include "tween.tcc" #include "tweenone.tcc" -#endif //TWEENY_TWEEN_H +#endif // TWEENY_TWEEN_H diff --git a/flatland_plugins/include/thirdparty/tweeny/tweenpoint.h b/flatland_plugins/include/thirdparty/tweeny/tweenpoint.h index 24d84231..0086964c 100644 --- a/flatland_plugins/include/thirdparty/tweeny/tweenpoint.h +++ b/flatland_plugins/include/thirdparty/tweeny/tweenpoint.h @@ -7,76 +7,83 @@ Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to - use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - the Software, and to permit persons to whom the Software is furnished to do so, - subject to the following conditions: + use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is furnished to do + so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. */ /* - * This file provides the declarations for a tween currentPoint utility class. A tweenpoint holds the tween values, - * easings and durations. + * This file provides the declarations for a tween currentPoint utility class. A + * tweenpoint holds the tween values, easings and durations. */ - #ifndef TWEENY_TWEENPOINT_H #define TWEENY_TWEENPOINT_H - -#include #include +#include #include "tweentraits.h" -namespace tweeny { - namespace detail { - /* - * The tweenpoint class aids in the management of a tweening currentPoint by the tween class. - * This class is private. - */ - template - struct tweenpoint { - typedef detail::tweentraits traits; - - typename traits::valuesType values; - typename traits::durationsArrayType durations; - typename traits::easingCollection easings; - typename traits::callbackType onEnterCallbacks; - uint32_t stacked; - - /* Constructs a tweenpoint from a set of values, filling their durations and easings */ - tweenpoint(Ts... vs); - - /* Set the duration for all the values in this currentPoint */ - template void during(D milis); - - /* Sets the duration for each value in this currentPoint */ - template void during(Ds... vs); - - /* Sets the easing functions of each value */ - template void via(Fs... fs); - - /* Sets the same easing function for all values */ - template void via(F f); - - /* Returns the highest value in duration array */ - uint16_t duration() const; - - /* Returns the value of that specific value */ - uint16_t duration(size_t i) const; - }; - } -} +namespace tweeny +{ +namespace detail +{ +/* + * The tweenpoint class aids in the management of a tweening currentPoint by the + * tween class. This class is private. + */ +template +struct tweenpoint +{ + typedef detail::tweentraits traits; + + typename traits::valuesType values; + typename traits::durationsArrayType durations; + typename traits::easingCollection easings; + typename traits::callbackType onEnterCallbacks; + uint32_t stacked; + + /* Constructs a tweenpoint from a set of values, filling their durations and + * easings */ + tweenpoint(Ts... vs); + + /* Set the duration for all the values in this currentPoint */ + template + void during(D milis); + + /* Sets the duration for each value in this currentPoint */ + template + void during(Ds... vs); + + /* Sets the easing functions of each value */ + template + void via(Fs... fs); + + /* Sets the same easing function for all values */ + template + void via(F f); + + /* Returns the highest value in duration array */ + uint16_t duration() const; + + /* Returns the value of that specific value */ + uint16_t duration(size_t i) const; +}; +} // namespace detail +} // namespace tweeny #include "tweenpoint.tcc" -#endif //TWEENY_TWEENPOINT_H +#endif // TWEENY_TWEENPOINT_H diff --git a/flatland_plugins/include/thirdparty/tweeny/tweentraits.h b/flatland_plugins/include/thirdparty/tweeny/tweentraits.h index d0891329..07b7d8a2 100644 --- a/flatland_plugins/include/thirdparty/tweeny/tweentraits.h +++ b/flatland_plugins/include/thirdparty/tweeny/tweentraits.h @@ -7,19 +7,20 @@ Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to - use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - the Software, and to permit persons to whom the Software is furnished to do so, - subject to the following conditions: + use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is furnished to do + so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. */ /* @@ -29,46 +30,74 @@ #ifndef TWEENY_TWEENTRAITS_H #define TWEENY_TWEENTRAITS_H -#include #include +#include #include -namespace tweeny { - template class tween; - - namespace detail { - - template struct equal { enum { value = true }; }; - template struct equal { enum { value = true }; }; - template struct equal { enum { value = true && equal::value }; }; - template struct equal { enum { value = false }; }; - - template struct first { typedef T type; }; - - template - struct valuetype { }; - - template - struct valuetype { - typedef std::tuple type; - }; - - template - struct valuetype { - typedef std::array::type, sizeof...(Ts)> type; - }; - - template - struct tweentraits { - typedef std::tuple...> easingCollection; - typedef std::function &, Ts...)> callbackType; - typedef std::function &)> noValuesCallbackType; - typedef std::function noTweenCallbackType; - typedef typename valuetype::value, Ts...>::type valuesType; - typedef std::array durationsArrayType; - typedef tween type; - }; - } -} - -#endif //TWEENY_TWEENTRAITS_H +namespace tweeny +{ +template +class tween; + +namespace detail +{ + +template +struct equal +{ + enum { value = true }; +}; +template +struct equal +{ + enum { value = true }; +}; +template +struct equal +{ + enum { value = true && equal::value }; +}; +template +struct equal +{ + enum { value = false }; +}; + +template +struct first +{ + typedef T type; +}; + +template +struct valuetype +{ +}; + +template +struct valuetype +{ + typedef std::tuple type; +}; + +template +struct valuetype +{ + typedef std::array::type, sizeof...(Ts)> type; +}; + +template +struct tweentraits +{ + typedef std::tuple...> easingCollection; + typedef std::function &, Ts...)> callbackType; + typedef std::function &)> noValuesCallbackType; + typedef std::function noTweenCallbackType; + typedef typename valuetype::value, Ts...>::type valuesType; + typedef std::array durationsArrayType; + typedef tween type; +}; +} // namespace detail +} // namespace tweeny + +#endif // TWEENY_TWEENTRAITS_H diff --git a/flatland_plugins/include/thirdparty/tweeny/tweeny.h b/flatland_plugins/include/thirdparty/tweeny/tweeny.h index e3d83bdd..521cfa06 100644 --- a/flatland_plugins/include/thirdparty/tweeny/tweeny.h +++ b/flatland_plugins/include/thirdparty/tweeny/tweeny.h @@ -7,38 +7,44 @@ Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to - use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of - the Software, and to permit persons to whom the Software is furnished to do so, - subject to the following conditions: + use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is furnished to do + so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS - FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR - COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER - IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN - CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. */ /** * @file tweeny.h - * This file is the main header file for Tweeny. You should not need to include anything else. + * This file is the main header file for Tweeny. You should not need to include + * anything else. */ /** * @mainpage Tweeny * - * Tweeny is an inbetweening library designed for the creation of complex animations for games and other beautiful - * interactive software. It leverages features of modern C++ to empower developers with an intuitive API for - * declaring tweenings of any type of value, as long as they support arithmetic operations. + * Tweeny is an inbetweening library designed for the creation of complex + * animations for games and other beautiful interactive software. It leverages + * features of modern C++ to empower developers with an intuitive API for + * declaring tweenings of any type of value, as long as they support arithmetic + * operations. * - * This document contains Tweeny's API reference. The most interesting parts are: + * This document contains Tweeny's API reference. The most interesting parts + * are: * * * The Fine @ref manual * * The tweeny::from global function, to start a new tween. - * * The tweeny::tween class itself, that has all the interesting methods for a tween. + * * The tweeny::tween class itself, that has all the interesting methods for a + * tween. * * The modules page has a list of type of easings. * * This is how the API looks like: @@ -55,31 +61,36 @@ * while (tween.progress() < 1.0f) tween.step(0.01f); * * // a tween with multiple values - * auto tween2 = tweeny::from(0, 1.0f).to(1200, 7.0f).during(1000).via(easing::backInOut, easing::linear); + * auto tween2 = + * tweeny::from(0, 1.0f).to(1200, 7.0f).during(1000).via(easing::backInOut, + * easing::linear); * * // a tween with multiple points, different easings and durations * auto tween3 = tweeny::from(0, 0) - * .to(100, 100).during(100).via(easing::backOut, easing::backOut) - * .to(200, 200).during(500).via(easing::linear); - * return 0; + * .to(100, 100).during(100).via(easing::backOut, + * easing::backOut) .to(200, 200).during(500).via(easing::linear); return 0; * } * * @endcode * * **Examples** * - * * @ref easingsexample prints on the console the easing curve for each easing function. + * * @ref easingsexample prints on the console the easing curve for each easing + * function. * * @ref loop provides an example on how to make a tween loop * * @ref zombieexample holds a sprite animation example using two tweens. * * **Useful links and references** - * * Tim Groleau's easing function generator (requires flash) + * * Tim + * Groleau's easing function generator (requires flash) * * Easing cheat sheet (contains graphics!) */ /** * @page loop Loop example - * @details The loop example shows various forms of implementing tween loops using the tween::onStep function. + * @details The loop example shows various forms of implementing tween loops + * using the tween::onStep function. * * @include loop/loop.cpp */ @@ -93,9 +104,10 @@ /** * @page zombieexample Sprite animation example - * @details This example uses a tween to control the animation frames of a sprite. It uses two tweens, one for - * the idle animation with a yoyo loop and the walking animation with a wrap loop. The complete example can be - * obtained by examining the source code. + * @details This example uses a tween to control the animation frames of a + * sprite. It uses two tweens, one for the idle animation with a yoyo loop and + * the walking animation with a wrap loop. The complete example can be obtained + * by examining the source code. * * @include sprite/main.cpp */ @@ -110,26 +122,29 @@ #ifndef TWEENY_H #define TWEENY_H -#include "tween.h" -#include "tween.h" #include "easing.h" +#include "tween.h" /** - * @brief The tweeny namespace contains all symbols and names for the Tweeny library. + * @brief The tweeny namespace contains all symbols and names for the Tweeny + * library. + */ +namespace tweeny +{ +/** + * @brief Creates a tween starting from the values defined in the arguments. + * + * Starting values can have heterogeneous types, even user-defined types, + * provided they implement the four arithmetic operators (+, -, * and /). The + * types used will also define the type of each next step, the type of the + * callback and the type of arguments the passed easing functions must have. + * + * @sa tweeny::tween */ -namespace tweeny { - /** - * @brief Creates a tween starting from the values defined in the arguments. - * - * Starting values can have heterogeneous types, even user-defined types, provided they implement the - * four arithmetic operators (+, -, * and /). The types used will also define the type of each next step, the type - * of the callback and the type of arguments the passed easing functions must have. - * - * @sa tweeny::tween - */ - template tween from(Ts... vs); -} +template +tween from(Ts... vs); +} // namespace tweeny #include "tweeny.tcc" -#endif //TWEENY_TWEENY_H +#endif // TWEENY_TWEENY_H diff --git a/flatland_plugins/src/bool_sensor.cpp b/flatland_plugins/src/bool_sensor.cpp index f3b5650f..c9c0861d 100644 --- a/flatland_plugins/src/bool_sensor.cpp +++ b/flatland_plugins/src/bool_sensor.cpp @@ -47,27 +47,27 @@ #include #include #include + #include using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ -void BoolSensor::OnInitialize(const YAML::Node &config) { +void BoolSensor::OnInitialize(const YAML::Node & config) +{ YamlReader reader(node_, config); // defaults std::string topic_name = reader.Get("topic", "bool_sensor"); - update_rate_ = reader.Get("update_rate", - std::numeric_limits::infinity()); + update_rate_ = reader.Get("update_rate", std::numeric_limits::infinity()); // sensor defaults to the first model in the list if (GetModel()->bodies_.size() == 0) { - throw YAMLException("You didn't provide any bodies for model" + - GetModel()->name_); + throw YAMLException("You didn't provide any bodies for model" + GetModel()->name_); } - std::string body_name = - reader.Get("body", GetModel()->bodies_[0]->name_); + std::string body_name = reader.Get("body", GetModel()->bodies_[0]->name_); reader.EnsureAccessedAllKeys(); @@ -82,15 +82,17 @@ void BoolSensor::OnInitialize(const YAML::Node &config) { // Init publisher publisher_ = - node_->create_publisher(GetModel()->NameSpaceTopic(topic_name), 1); + node_->create_publisher(GetModel()->NameSpaceTopic(topic_name), 1); - RCLCPP_DEBUG(rclcpp::get_logger("BoolSensor"), - "Initialized with params: topic(%s) body(%s) " - "update_rate(%f)", - topic_name.c_str(), body_name.c_str(), update_rate_); + RCLCPP_DEBUG( + rclcpp::get_logger("BoolSensor"), + "Initialized with params: topic(%s) body(%s) " + "update_rate(%f)", + topic_name.c_str(), body_name.c_str(), update_rate_); } -void BoolSensor::AfterPhysicsStep(const Timekeeper &timekeeper) { +void BoolSensor::AfterPhysicsStep(const Timekeeper & timekeeper) +{ // Publish the boolean timer at the desired update rate if (!update_timer_.CheckUpdate(timekeeper)) { return; @@ -112,7 +114,8 @@ void BoolSensor::AfterPhysicsStep(const Timekeeper &timekeeper) { publisher_->publish(msg); } -void BoolSensor::BeginContact(b2Contact *contact) { +void BoolSensor::BeginContact(b2Contact * contact) +{ if (!FilterContact(contact)) return; // Skip collisions with other fixtures on this body @@ -124,7 +127,8 @@ void BoolSensor::BeginContact(b2Contact *contact) { hit_something_ = true; } -void BoolSensor::EndContact(b2Contact *contact) { +void BoolSensor::EndContact(b2Contact * contact) +{ if (!FilterContact(contact)) return; // Skip collisions with other fixtures on this body @@ -134,7 +138,6 @@ void BoolSensor::EndContact(b2Contact *contact) { collisions_--; } -} // End flatland_plugins namespace +} // namespace flatland_plugins -PLUGINLIB_EXPORT_CLASS(flatland_plugins::BoolSensor, - flatland_server::ModelPlugin) +PLUGINLIB_EXPORT_CLASS(flatland_plugins::BoolSensor, flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/bumper.cpp b/flatland_plugins/src/bumper.cpp index c68c3c39..d2a92156 100644 --- a/flatland_plugins/src/bumper.cpp +++ b/flatland_plugins/src/bumper.cpp @@ -44,22 +44,25 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include #include #include #include #include -#include + #include +#include +#include +#include using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ Bumper::ContactState::ContactState() { Reset(); } -void Bumper::ContactState::Reset() { +void Bumper::ContactState::Reset() +{ num_count = 0; sum_normal_impulses[0] = 0; sum_normal_impulses[1] = 0; @@ -67,45 +70,43 @@ void Bumper::ContactState::Reset() { sum_tangential_impulses[1] = 0; } -void Bumper::OnInitialize(const YAML::Node &config) { +void Bumper::OnInitialize(const YAML::Node & config) +{ YamlReader reader(node_, config); // defaults world_frame_id_ = reader.Get("world_frame_id", "map"); topic_name_ = reader.Get("topic", "collisions"); publish_all_collisions_ = reader.Get("publish_all_collisions", true); - update_rate_ = reader.Get("update_rate", - std::numeric_limits::infinity()); + update_rate_ = reader.Get("update_rate", std::numeric_limits::infinity()); - std::vector excluded_body_names = - reader.GetList("exclude", {}, -1, -1); + std::vector excluded_body_names = reader.GetList("exclude", {}, -1, -1); reader.EnsureAccessedAllKeys(); for (unsigned int i = 0; i < excluded_body_names.size(); i++) { - Body *body = GetModel()->GetBody(excluded_body_names[i]); + Body * body = GetModel()->GetBody(excluded_body_names[i]); if (body == nullptr) { - throw YAMLException("Body with name \"" + excluded_body_names[i] + - "\" does not exist"); + throw YAMLException("Body with name \"" + excluded_body_names[i] + "\" does not exist"); } else { excluded_bodies_.push_back(body); } } update_timer_.SetRate(update_rate_); - collisions_publisher_ = - node_->create_publisher(topic_name_, 1); - - RCLCPP_DEBUG(rclcpp::get_logger("Bumper"), - "Initialized with params: topic(%s) world_frame_id(%s) " - "publish_all_collisions(%d) update_rate(%f) exclude({%s})", - topic_name_.c_str(), world_frame_id_.c_str(), - publish_all_collisions_, update_rate_, - boost::algorithm::join(excluded_body_names, ",").c_str()); + collisions_publisher_ = node_->create_publisher(topic_name_, 1); + + RCLCPP_DEBUG( + rclcpp::get_logger("Bumper"), + "Initialized with params: topic(%s) world_frame_id(%s) " + "publish_all_collisions(%d) update_rate(%f) exclude({%s})", + topic_name_.c_str(), world_frame_id_.c_str(), publish_all_collisions_, update_rate_, + boost::algorithm::join(excluded_body_names, ",").c_str()); } -void Bumper::BeforePhysicsStep(const Timekeeper &timekeeper) { +void Bumper::BeforePhysicsStep(const Timekeeper & timekeeper) +{ std::map::iterator it; // Clear the forces at the begining of every physics step since brand @@ -115,7 +116,8 @@ void Bumper::BeforePhysicsStep(const Timekeeper &timekeeper) { } } -void Bumper::AfterPhysicsStep(const Timekeeper &timekeeper) { +void Bumper::AfterPhysicsStep(const Timekeeper & timekeeper) +{ // The expected behaviour is to always publish non-empty collisions unless // publish_all_collisions set to false. The publishing of empty collision // manages the publishing rate of empty collisions when @@ -135,8 +137,8 @@ void Bumper::AfterPhysicsStep(const Timekeeper &timekeeper) { // loop through all collisions in our record and publish for (it = contact_states_.begin(); it != contact_states_.end(); it++) { - b2Contact *c = it->first; - ContactState *s = &it->second; + b2Contact * c = it->first; + ContactState * s = &it->second; flatland_msgs::msg::Collision collision; collision.entity_a = GetModel()->GetName(); collision.entity_b = s->entity_b->name_; @@ -147,25 +149,23 @@ void Bumper::AfterPhysicsStep(const Timekeeper &timekeeper) { // If there was no post solve called, which means that the collision // probably involves a Box2D sensor, therefore there are no contact points, if (s->num_count > 0) { - b2Manifold *m = c->GetManifold(); + b2Manifold * m = c->GetManifold(); // go through each collision point for (int i = 0; i < m->pointCount; i++) { // calculate average impulse during each time step, the impulse are // converted to the applied force by dividing the step size double ave_normal_impulse = s->sum_normal_impulses[i] / s->num_count; - double ave_tangential_impulse = - s->sum_tangential_impulses[i] / s->num_count; + double ave_tangential_impulse = s->sum_tangential_impulses[i] / s->num_count; double ave_normal_force = ave_normal_impulse / timekeeper.GetStepSize(); - double ave_tangential_force = - ave_tangential_impulse / timekeeper.GetStepSize(); + double ave_tangential_force = ave_tangential_impulse / timekeeper.GetStepSize(); // Calculate the absolute magnitude of forces, forces are not provided // in vector form because the forces are obtained from averaging // Box2D impulses which are very inaccurate making them completely // useless for anything other than ball parking the impact strength - double force_abs = sqrt(ave_normal_force * ave_normal_force + - ave_tangential_force * ave_tangential_force); + double force_abs = + sqrt(ave_normal_force * ave_normal_force + ave_tangential_force * ave_tangential_force); collision.magnitude_forces.push_back(force_abs); flatland_msgs::msg::Vector2 point; @@ -185,8 +185,9 @@ void Bumper::AfterPhysicsStep(const Timekeeper &timekeeper) { collisions_publisher_->publish(collisions); } -void Bumper::BeginContact(b2Contact *contact) { - Entity *other_entity; +void Bumper::BeginContact(b2Contact * contact) +{ + Entity * other_entity; b2Fixture *this_fixture, *other_fixture; if (!FilterContact(contact, other_entity, this_fixture, other_fixture)) { return; @@ -194,8 +195,7 @@ void Bumper::BeginContact(b2Contact *contact) { // If this is a new contact, add it to the records of alive contacts if (!contact_states_.count(contact)) { - Body *collision_body = - static_cast(this_fixture->GetBody()->GetUserData()); + Body * collision_body = static_cast(this_fixture->GetBody()->GetUserData()); bool ignore = false; @@ -210,7 +210,7 @@ void Bumper::BeginContact(b2Contact *contact) { // add the body to the record of active contacts if (!ignore) { contact_states_[contact] = ContactState(); - ContactState *c = &contact_states_[contact]; + ContactState * c = &contact_states_[contact]; c->entity_b = other_entity; c->body_B = static_cast(other_fixture->GetBody()->GetUserData()); c->body_A = collision_body; @@ -227,7 +227,8 @@ void Bumper::BeginContact(b2Contact *contact) { } } -void Bumper::EndContact(b2Contact *contact) { +void Bumper::EndContact(b2Contact * contact) +{ if (!FilterContact(contact)) return; // The contact ended, remove it from the list of contacts @@ -239,10 +240,11 @@ void Bumper::EndContact(b2Contact *contact) { } } -void Bumper::PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) { +void Bumper::PostSolve(b2Contact * contact, const b2ContactImpulse * impulse) +{ if (!FilterContact(contact)) return; - ContactState *state; + ContactState * state; if (contact_states_.count(contact)) { state = &contact_states_[contact]; } else { @@ -279,6 +281,6 @@ void Bumper::PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) { state->normal = m.normal; state->normal *= state->normal_sign; } -}; +}; // namespace flatland_plugins PLUGINLIB_EXPORT_CLASS(flatland_plugins::Bumper, flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/diff_drive.cpp b/flatland_plugins/src/diff_drive.cpp index 3425f3c7..60880e59 100644 --- a/flatland_plugins/src/diff_drive.cpp +++ b/flatland_plugins/src/diff_drive.cpp @@ -48,20 +48,21 @@ #include #include #include -#include #include -#include -#include #include #include -namespace flatland_plugins { +#include +#include +#include -void DiffDrive::TwistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { - twist_msg_ = msg; -} +namespace flatland_plugins +{ -void DiffDrive::OnInitialize(const YAML::Node& config) { +void DiffDrive::TwistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { twist_msg_ = msg; } + +void DiffDrive::OnInitialize(const YAML::Node & config) +{ tf_broadcaster_ = std::make_shared(node_); YamlReader reader(node_, config); @@ -71,20 +72,17 @@ void DiffDrive::OnInitialize(const YAML::Node& config) { std::string odom_frame_id = reader.Get("odom_frame_id", "odom"); std::string twist_topic = reader.Get("twist_sub", "cmd_vel"); - std::string odom_topic = - reader.Get("odom_pub", "odometry/filtered"); + std::string odom_topic = reader.Get("odom_pub", "odometry/filtered"); std::string ground_truth_topic = - reader.Get("ground_truth_pub", "odometry/ground_truth"); + reader.Get("ground_truth_pub", "odometry/ground_truth"); std::string twist_pub_topic = reader.Get("twist_pub", "twist"); // noise are in the form of linear x, linear y, angular variances std::vector odom_twist_noise = - reader.GetList("odom_twist_noise", {0, 0, 0}, 3, 3); - std::vector odom_pose_noise = - reader.GetList("odom_pose_noise", {0, 0, 0}, 3, 3); + reader.GetList("odom_twist_noise", {0, 0, 0}, 3, 3); + std::vector odom_pose_noise = reader.GetList("odom_pose_noise", {0, 0, 0}, 3, 3); - double pub_rate = - reader.Get("pub_rate", std::numeric_limits::infinity()); + double pub_rate = reader.Get("pub_rate", std::numeric_limits::infinity()); update_timer_.SetRate(pub_rate); // by default the covariance diagonal is the variance of actual noise @@ -100,10 +98,10 @@ void DiffDrive::OnInitialize(const YAML::Node& config) { odom_twist_covar_default[7] = odom_twist_noise[1]; odom_twist_covar_default[35] = odom_twist_noise[2]; - auto odom_twist_covar = reader.GetArray("odom_twist_covariance", - odom_twist_covar_default); - auto odom_pose_covar = reader.GetArray("odom_pose_covariance", - odom_pose_covar_default); + auto odom_twist_covar = + reader.GetArray("odom_twist_covariance", odom_twist_covar_default); + auto odom_pose_covar = + reader.GetArray("odom_pose_covariance", odom_pose_covar_default); reader.EnsureAccessedAllKeys(); @@ -118,8 +116,7 @@ void DiffDrive::OnInitialize(const YAML::Node& config) { twist_topic, 1, std::bind(&DiffDrive::TwistCallback, this, _1)); if (enable_odom_pub_) { odom_pub_ = node_->create_publisher(odom_topic, 1); - ground_truth_pub_ = - node_->create_publisher(ground_truth_topic, 1); + ground_truth_pub_ = node_->create_publisher(ground_truth_topic, 1); } if (enable_twist_pub_) { @@ -144,39 +141,36 @@ void DiffDrive::OnInitialize(const YAML::Node& config) { rng_ = std::default_random_engine(rd()); for (unsigned int i = 0; i < 3; i++) { // variance is standard deviation squared - noise_gen_[i] = - std::normal_distribution(0.0, sqrt(odom_pose_noise[i])); + noise_gen_[i] = std::normal_distribution(0.0, sqrt(odom_pose_noise[i])); } for (unsigned int i = 0; i < 3; i++) { - noise_gen_[i + 3] = - std::normal_distribution(0.0, sqrt(odom_twist_noise[i])); + noise_gen_[i + 3] = std::normal_distribution(0.0, sqrt(odom_twist_noise[i])); } - RCLCPP_DEBUG(rclcpp::get_logger("DiffDrive"), - "Initialized with params body(%p %s) odom_frame_id(%s) " - "twist_sub(%s) odom_pub(%s) ground_truth_pub(%s) " - "odom_pose_noise({%f,%f,%f}) odom_twist_noise({%f,%f,%f}) " - "pub_rate(%f)\n", - body_, body_->name_.c_str(), odom_frame_id.c_str(), - twist_topic.c_str(), odom_topic.c_str(), - ground_truth_topic.c_str(), odom_pose_noise[0], - odom_pose_noise[1], odom_pose_noise[2], odom_twist_noise[0], - odom_twist_noise[1], odom_twist_noise[2], pub_rate); + RCLCPP_DEBUG( + rclcpp::get_logger("DiffDrive"), + "Initialized with params body(%p %s) odom_frame_id(%s) " + "twist_sub(%s) odom_pub(%s) ground_truth_pub(%s) " + "odom_pose_noise({%f,%f,%f}) odom_twist_noise({%f,%f,%f}) " + "pub_rate(%f)\n", + body_, body_->name_.c_str(), odom_frame_id.c_str(), twist_topic.c_str(), odom_topic.c_str(), + ground_truth_topic.c_str(), odom_pose_noise[0], odom_pose_noise[1], odom_pose_noise[2], + odom_twist_noise[0], odom_twist_noise[1], odom_twist_noise[2], pub_rate); } -void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { +void DiffDrive::BeforePhysicsStep(const Timekeeper & timekeeper) +{ bool publish = update_timer_.CheckUpdate(timekeeper); - b2Body* b2body = body_->physics_body_; + b2Body * b2body = body_->physics_body_; b2Vec2 position = b2body->GetPosition(); float angle = b2body->GetAngle(); if (publish) { // get the state of the body and publish the data - b2Vec2 linear_vel_local = - b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0)); + b2Vec2 linear_vel_local = b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0)); float angular_vel = b2body->GetAngularVelocity(); ground_truth_msg_.header.stamp = timekeeper.GetSimTime(); @@ -186,7 +180,7 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { tf2::Quaternion q; q.setRPY(0, 0, angle); - ground_truth_msg_.pose.pose.orientation= tf2::toMsg(q); + ground_truth_msg_.pose.pose.orientation = tf2::toMsg(q); ground_truth_msg_.twist.twist.linear.x = linear_vel_local.x; ground_truth_msg_.twist.twist.linear.y = linear_vel_local.y; ground_truth_msg_.twist.twist.linear.z = 0; @@ -201,7 +195,7 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { odom_msg_.pose.pose.position.x += noise_gen_[0](rng_); odom_msg_.pose.pose.position.y += noise_gen_[1](rng_); q.setRPY(0, 0, angle + noise_gen_[2](rng_)); - odom_msg_.pose.pose.orientation= tf2::toMsg(q); + odom_msg_.pose.pose.orientation = tf2::toMsg(q); odom_msg_.twist.twist.linear.x += noise_gen_[3](rng_); odom_msg_.twist.twist.linear.y += noise_gen_[4](rng_); odom_msg_.twist.twist.angular.z += noise_gen_[5](rng_); @@ -219,9 +213,8 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { twist_pub_msg.header.frame_id = odom_msg_.child_frame_id; // Forward velocity in twist.linear.x - twist_pub_msg.twist.linear.x = cos(angle) * linear_vel_local.x + - sin(angle) * linear_vel_local.y + - noise_gen_[3](rng_); + twist_pub_msg.twist.linear.x = + cos(angle) * linear_vel_local.x + sin(angle) * linear_vel_local.y + noise_gen_[3](rng_); // Angular velocity in twist.angular.z twist_pub_msg.twist.angular.z = angular_vel + noise_gen_[5](rng_); @@ -260,7 +253,6 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { b2body->SetLinearVelocity(linear_vel_cm); b2body->SetAngularVelocity(angular_vel); } -} +} // namespace flatland_plugins -PLUGINLIB_EXPORT_CLASS(flatland_plugins::DiffDrive, - flatland_server::ModelPlugin) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(flatland_plugins::DiffDrive, flatland_server::ModelPlugin) \ No newline at end of file diff --git a/flatland_plugins/src/gps.cpp b/flatland_plugins/src/gps.cpp index 81a3d52e..7862a8d4 100644 --- a/flatland_plugins/src/gps.cpp +++ b/flatland_plugins/src/gps.cpp @@ -1,14 +1,17 @@ #include + #include using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ double Gps::WGS84_A = 6378137.0; double Gps::WGS84_E2 = 0.0066943799831668; -void Gps::OnInitialize(const YAML::Node &config) { +void Gps::OnInitialize(const YAML::Node & config) +{ tf_broadcaster_ = std::make_shared(node_); ParseParameters(config); update_timer_.SetRate(update_rate_); @@ -20,7 +23,8 @@ void Gps::OnInitialize(const YAML::Node &config) { m_body_to_gps_ << c, -s, x, s, c, y, 0, 0, 1; } -void Gps::BeforePhysicsStep(const Timekeeper &timekeeper) { +void Gps::BeforePhysicsStep(const Timekeeper & timekeeper) +{ // keep the update rate if (!update_timer_.CheckUpdate(timekeeper)) { return; @@ -39,7 +43,8 @@ void Gps::BeforePhysicsStep(const Timekeeper &timekeeper) { } } -void Gps::ComputeReferenceEcef() { +void Gps::ComputeReferenceEcef() +{ double s_lat = sin(ref_lat_rad_); double c_lat = cos(ref_lat_rad_); double s_lon = sin(ref_lon_rad_); @@ -52,8 +57,9 @@ void Gps::ComputeReferenceEcef() { ref_ecef_z_ = n * (1.0 - WGS84_E2) * s_lat; } -void Gps::UpdateFix() { - const b2Transform &t = body_->GetPhysicsBody()->GetTransform(); +void Gps::UpdateFix() +{ + const b2Transform & t = body_->GetPhysicsBody()->GetTransform(); Eigen::Matrix3f m_world_to_body; m_world_to_body << t.q.c, -t.q.s, t.p.x, t.q.s, t.q.c, t.p.y, 0, 0, 1; Eigen::Matrix3f m_world_to_gps = m_world_to_body * m_body_to_gps_; @@ -88,7 +94,8 @@ void Gps::UpdateFix() { gps_fix_.altitude = 0.0; } -void Gps::ParseParameters(const YAML::Node &config) { +void Gps::ParseParameters(const YAML::Node & config) +{ YamlReader reader(node_, config); std::string body_name = reader.Get("body"); topic_ = reader.Get("topic", "gps/fix"); @@ -105,10 +112,8 @@ void Gps::ParseParameters(const YAML::Node &config) { throw YAMLException("Cannot find body with name " + body_name); } - std::string parent_frame_id = - GetModel()->NameSpaceTF(body_->GetName()); - std::string child_frame_id = - GetModel()->NameSpaceTF(frame_id_); + std::string parent_frame_id = GetModel()->NameSpaceTF(body_->GetName()); + std::string child_frame_id = GetModel()->NameSpaceTF(frame_id_); // Set constant frame ID in GPS fix message gps_fix_.header.frame_id = child_frame_id; @@ -124,6 +129,6 @@ void Gps::ParseParameters(const YAML::Node &config) { gps_tf_.transform.rotation.z = sin(0.5 * origin_.theta); gps_tf_.transform.rotation.w = cos(0.5 * origin_.theta); } -} +} // namespace flatland_plugins PLUGINLIB_EXPORT_CLASS(flatland_plugins::Gps, flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index eb153acc..f044b006 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -49,18 +49,21 @@ #include #include #include -#include -#include -#include #include + +#include #include +#include #include +#include using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ -void Laser::OnInitialize(const YAML::Node &config) { +void Laser::OnInitialize(const YAML::Node & config) +{ tf_broadcaster_ = std::make_shared(node_); ParseParameters(config); @@ -74,8 +77,7 @@ void Laser::OnInitialize(const YAML::Node &config) { double x = origin_.x, y = origin_.y; m_body_to_laser_ << c, -s, x, s, c, y, 0, 0, 1; - unsigned int num_laser_points = - std::lround((max_angle_ - min_angle_) / increment_) + 1; + unsigned int num_laser_points = std::lround((max_angle_ - min_angle_) / increment_) + 1; // initialize size for the matrix storing the laser points m_laser_points_ = Eigen::MatrixXf(3, num_laser_points); @@ -108,8 +110,7 @@ void Laser::OnInitialize(const YAML::Node &config) { laser_scan_.intensities.resize(num_laser_points); else laser_scan_.intensities.resize(0); - laser_scan_.header.frame_id = - GetModel()->NameSpaceTF(frame_id_); + laser_scan_.header.frame_id = GetModel()->NameSpaceTF(frame_id_); // Broadcast transform between the body and laser tf2::Quaternion q; @@ -126,7 +127,8 @@ void Laser::OnInitialize(const YAML::Node &config) { laser_tf_.transform.rotation.w = q.w(); } -void Laser::BeforePhysicsStep(const Timekeeper &timekeeper) { +void Laser::BeforePhysicsStep(const Timekeeper & timekeeper) +{ // keep the update rate if (!update_timer_.CheckUpdate(timekeeper)) { return; @@ -145,11 +147,12 @@ void Laser::BeforePhysicsStep(const Timekeeper &timekeeper) { } } -void Laser::ComputeLaserRanges() { +void Laser::ComputeLaserRanges() +{ // get the transformation matrix from the world to the body, and get the // world to laser frame transformation matrix by multiplying the world to body // and body to laser - const b2Transform &t = body_->GetPhysicsBody()->GetTransform(); + const b2Transform & t = body_->GetPhysicsBody()->GetTransform(); m_world_to_body_ << t.q.c, -t.q.s, t.p.x, t.q.s, t.q.c, t.p.y, 0, 0, 1; m_world_to_laser_ = m_world_to_body_ * m_body_to_laser_; @@ -163,29 +166,25 @@ void Laser::ComputeLaserRanges() { b2Vec2 laser_origin_point(v_world_laser_origin_(0), v_world_laser_origin_(1)); // Results vector - std::vector>> results( - laser_scan_.ranges.size()); + std::vector>> results(laser_scan_.ranges.size()); // loop through the laser points and call the Box2D world raycast by // enqueueing the callback for (unsigned int i = 0; i < laser_scan_.ranges.size(); ++i) { - results[i] = - pool_.enqueue([i, this, laser_origin_point] { // Lambda function - b2Vec2 laser_point; - laser_point.x = m_world_laser_points_(0, i); - laser_point.y = m_world_laser_points_(1, i); - LaserCallback cb(this); - - GetModel()->GetPhysicsWorld()->RayCast(&cb, laser_origin_point, - laser_point); - - if (!cb.did_hit_) { - return std::make_pair(NAN, 0); - } else { - return std::make_pair(cb.fraction_ * this->range_, - cb.intensity_); - } - }); + results[i] = pool_.enqueue([i, this, laser_origin_point] { // Lambda function + b2Vec2 laser_point; + laser_point.x = m_world_laser_points_(0, i); + laser_point.y = m_world_laser_points_(1, i); + LaserCallback cb(this); + + GetModel()->GetPhysicsWorld()->RayCast(&cb, laser_origin_point, laser_point); + + if (!cb.did_hit_) { + return std::make_pair(NAN, 0); + } else { + return std::make_pair(cb.fraction_ * this->range_, cb.intensity_); + } + }); } // Unqueue all of the future'd results @@ -196,8 +195,9 @@ void Laser::ComputeLaserRanges() { } } -float LaserCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, - const b2Vec2 &normal, float fraction) { +float LaserCallback::ReportFixture( + b2Fixture * fixture, const b2Vec2 & point, const b2Vec2 & normal, float fraction) +{ uint16_t category_bits = fixture->GetFilterData().categoryBits; // only register hit in the specified layers if (!(category_bits & parent_->layers_bits_)) { @@ -217,20 +217,19 @@ float LaserCallback::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, return fraction; } -void Laser::ParseParameters(const YAML::Node &config) { +void Laser::ParseParameters(const YAML::Node & config) +{ YamlReader reader(node_, config); std::string body_name = reader.Get("body"); topic_ = reader.Get("topic", "scan"); frame_id_ = reader.Get("frame", GetName()); broadcast_tf_ = reader.Get("broadcast_tf", true); - update_rate_ = reader.Get("update_rate", - std::numeric_limits::infinity()); + update_rate_ = reader.Get("update_rate", std::numeric_limits::infinity()); origin_ = reader.GetPose("origin", Pose(0, 0, 0)); range_ = reader.Get("range"); noise_std_dev_ = reader.Get("noise_std_dev", 0); - std::vector layers = - reader.GetList("layers", {"all"}, -1, -1); + std::vector layers = reader.GetList("layers", {"all"}, -1, -1); YamlReader angle_reader = reader.Subnode("angle", YamlReader::MAP); min_angle_ = angle_reader.Get("min"); @@ -252,30 +251,29 @@ void Laser::ParseParameters(const YAML::Node &config) { std::vector invalid_layers; layers_bits_ = GetModel()->GetCfr()->GetCategoryBits(layers, &invalid_layers); if (!invalid_layers.empty()) { - throw YAMLException("Cannot find layer(s): {" + - boost::algorithm::join(invalid_layers, ",") + "}"); + throw YAMLException( + "Cannot find layer(s): {" + boost::algorithm::join(invalid_layers, ",") + "}"); } std::vector reflectance_layer = {"reflectance"}; reflectance_layers_bits_ = - GetModel()->GetCfr()->GetCategoryBits(reflectance_layer, &invalid_layers); + GetModel()->GetCfr()->GetCategoryBits(reflectance_layer, &invalid_layers); // init the random number generators std::random_device rd; rng_ = std::default_random_engine(rd()); noise_gen_ = std::normal_distribution(0.0, noise_std_dev_); - RCLCPP_DEBUG(rclcpp::get_logger("LaserPlugin"), - "Laser %s params: topic(%s) body(%s, %p) origin(%f,%f,%f) " - "frame_id(%s) broadcast_tf(%d) update_rate(%f) range(%f) " - "noise_std_dev(%f) angle_min(%f) angle_max(%f) " - "angle_increment(%f) layers(0x%u {%s})", - GetName().c_str(), topic_.c_str(), body_name.c_str(), body_, - origin_.x, origin_.y, origin_.theta, frame_id_.c_str(), - broadcast_tf_, update_rate_, range_, noise_std_dev_, - min_angle_, max_angle_, increment_, layers_bits_, - boost::algorithm::join(layers, ",").c_str()); -} + RCLCPP_DEBUG( + rclcpp::get_logger("LaserPlugin"), + "Laser %s params: topic(%s) body(%s, %p) origin(%f,%f,%f) " + "frame_id(%s) broadcast_tf(%d) update_rate(%f) range(%f) " + "noise_std_dev(%f) angle_min(%f) angle_max(%f) " + "angle_increment(%f) layers(0x%u {%s})", + GetName().c_str(), topic_.c_str(), body_name.c_str(), body_, origin_.x, origin_.y, + origin_.theta, frame_id_.c_str(), broadcast_tf_, update_rate_, range_, noise_std_dev_, + min_angle_, max_angle_, increment_, layers_bits_, boost::algorithm::join(layers, ",").c_str()); } +} // namespace flatland_plugins PLUGINLIB_EXPORT_CLASS(flatland_plugins::Laser, flatland_server::ModelPlugin) diff --git a/flatland_plugins/src/model_tf_publisher.cpp b/flatland_plugins/src/model_tf_publisher.cpp index 55af16e8..03b1e42f 100644 --- a/flatland_plugins/src/model_tf_publisher.cpp +++ b/flatland_plugins/src/model_tf_publisher.cpp @@ -48,37 +48,37 @@ #include #include #include -#include -#include +#include + #include #include -#include +#include +#include using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ -void ModelTfPublisher::OnInitialize(const YAML::Node &config) { +void ModelTfPublisher::OnInitialize(const YAML::Node & config) +{ tf_broadcaster_ = std::make_shared(node_); YamlReader reader(node_, config); // default values publish_tf_world_ = reader.Get("publish_tf_world", false); world_frame_id_ = reader.Get("world_frame_id", "map"); - update_rate_ = reader.Get("update_rate", - std::numeric_limits::infinity()); + update_rate_ = reader.Get("update_rate", std::numeric_limits::infinity()); std::string ref_body_name = reader.Get("reference", ""); - std::vector excluded_body_names = - reader.GetList("exclude", {}, -1, -1); + std::vector excluded_body_names = reader.GetList("exclude", {}, -1, -1); reader.EnsureAccessedAllKeys(); if (ref_body_name.size() != 0) { reference_body_ = GetModel()->GetBody(ref_body_name); if (reference_body_ == nullptr) { - throw YAMLException("Body with name \"" + ref_body_name + - "\" does not exist"); + throw YAMLException("Body with name \"" + ref_body_name + "\" does not exist"); } } else { // defaults to the first body, the reference body has no effect on the @@ -87,11 +87,10 @@ void ModelTfPublisher::OnInitialize(const YAML::Node &config) { } for (unsigned int i = 0; i < excluded_body_names.size(); i++) { - Body *body = GetModel()->GetBody(excluded_body_names[i]); + Body * body = GetModel()->GetBody(excluded_body_names[i]); if (body == nullptr) { - throw YAMLException("Body with name \"" + excluded_body_names[i] + - "\" does not exist"); + throw YAMLException("Body with name \"" + excluded_body_names[i] + "\" does not exist"); } else { excluded_bodies_.push_back(body); } @@ -99,15 +98,16 @@ void ModelTfPublisher::OnInitialize(const YAML::Node &config) { update_timer_.SetRate(update_rate_); - RCLCPP_DEBUG(rclcpp::get_logger("ModelTFPublisher"), - "Initialized with params: reference(%s, %p) " - "publish_tf_world(%d) world_frame_id(%s) update_rate(%f), exclude({%s})", - reference_body_->name_.c_str(), reference_body_, publish_tf_world_, - world_frame_id_.c_str(), update_rate_, - boost::algorithm::join(excluded_body_names, ",").c_str()); + RCLCPP_DEBUG( + rclcpp::get_logger("ModelTFPublisher"), + "Initialized with params: reference(%s, %p) " + "publish_tf_world(%d) world_frame_id(%s) update_rate(%f), exclude({%s})", + reference_body_->name_.c_str(), reference_body_, publish_tf_world_, world_frame_id_.c_str(), + update_rate_, boost::algorithm::join(excluded_body_names, ",").c_str()); } -void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) { +void ModelTfPublisher::BeforePhysicsStep(const Timekeeper & timekeeper) +{ if (!update_timer_.CheckUpdate(timekeeper)) { return; } @@ -116,7 +116,7 @@ void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) { Eigen::Matrix3f rel_tf; ///< for storing TF from ref. body to other bodies // fill the world to ref. body TF with data from Box2D - const b2Transform &r = reference_body_->physics_body_->GetTransform(); + const b2Transform & r = reference_body_->physics_body_->GetTransform(); ref_tf_m << r.q.c, -r.q.s, r.p.x, r.q.s, r.q.c, r.p.y, 0, 0, 1; geometry_msgs::msg::TransformStamped tf_stamped; @@ -124,7 +124,7 @@ void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) { // loop through the bodies to calculate TF, and ignores excluded bodies for (unsigned int i = 0; i < GetModel()->bodies_.size(); i++) { - Body *body = GetModel()->bodies_[i]; + Body * body = GetModel()->bodies_[i]; bool is_excluded = false; for (unsigned int j = 0; j < excluded_bodies_.size(); j++) { @@ -138,7 +138,7 @@ void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) { } // Get transformation of body w.r.t to the world - const b2Transform &b = body->physics_body_->GetTransform(); + const b2Transform & b = body->physics_body_->GetTransform(); Eigen::Matrix3f body_tf_m; body_tf_m << b.q.c, -b.q.s, b.p.x, b.q.s, b.q.c, b.p.y, 0, 0, 1; @@ -153,10 +153,8 @@ void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) { double yaw = atan2(sine, cosine); // publish TF - tf_stamped.header.frame_id = - GetModel()->NameSpaceTF(reference_body_->name_); - tf_stamped.child_frame_id = - GetModel()->NameSpaceTF(body->name_); + tf_stamped.header.frame_id = GetModel()->NameSpaceTF(reference_body_->name_); + tf_stamped.child_frame_id = GetModel()->NameSpaceTF(body->name_); tf_stamped.transform.translation.x = rel_tf(0, 2); tf_stamped.transform.translation.y = rel_tf(1, 2); tf_stamped.transform.translation.z = 0; @@ -172,12 +170,11 @@ void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) { // publish world TF if necessary if (publish_tf_world_) { - const b2Vec2 &p = reference_body_->physics_body_->GetPosition(); + const b2Vec2 & p = reference_body_->physics_body_->GetPosition(); double yaw = reference_body_->physics_body_->GetAngle(); tf_stamped.header.frame_id = world_frame_id_; - tf_stamped.child_frame_id = - GetModel()->NameSpaceTF(reference_body_->name_); + tf_stamped.child_frame_id = GetModel()->NameSpaceTF(reference_body_->name_); tf_stamped.transform.translation.x = p.x; tf_stamped.transform.translation.y = p.y; tf_stamped.transform.translation.z = 0; @@ -191,7 +188,6 @@ void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) { tf_broadcaster_->sendTransform(tf_stamped); } } -}; +}; // namespace flatland_plugins -PLUGINLIB_EXPORT_CLASS(flatland_plugins::ModelTfPublisher, - flatland_server::ModelPlugin) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(flatland_plugins::ModelTfPublisher, flatland_server::ModelPlugin) \ No newline at end of file diff --git a/flatland_plugins/src/tricycle_drive.cpp b/flatland_plugins/src/tricycle_drive.cpp index ceadd335..987ba358 100644 --- a/flatland_plugins/src/tricycle_drive.cpp +++ b/flatland_plugins/src/tricycle_drive.cpp @@ -49,16 +49,19 @@ #include #include #include -#include -#include #include #include #include + #include +#include +#include -namespace flatland_plugins { +namespace flatland_plugins +{ -void TricycleDrive::OnInitialize(const YAML::Node& config) { +void TricycleDrive::OnInitialize(const YAML::Node & config) +{ YamlReader r(node_, config); // load all the parameters @@ -70,17 +73,13 @@ void TricycleDrive::OnInitialize(const YAML::Node& config) { string twist_topic = r.Get("twist_sub", "cmd_vel"); string odom_topic = r.Get("odom_pub", "odometry/filtered"); - string ground_truth_topic = - r.Get("ground_truth_pub", "odometry/ground_truth"); + string ground_truth_topic = r.Get("ground_truth_pub", "odometry/ground_truth"); // noise are in the form of linear x, linear y, angular variances - vector odom_twist_noise = - r.GetList("odom_twist_noise", {0, 0, 0}, 3, 3); - vector odom_pose_noise = - r.GetList("odom_pose_noise", {0, 0, 0}, 3, 3); + vector odom_twist_noise = r.GetList("odom_twist_noise", {0, 0, 0}, 3, 3); + vector odom_pose_noise = r.GetList("odom_pose_noise", {0, 0, 0}, 3, 3); - double pub_rate = - r.Get("pub_rate", numeric_limits::infinity()); + double pub_rate = r.Get("pub_rate", numeric_limits::infinity()); update_timer_.SetRate(pub_rate); // by default the covariance diagonal is the variance of actual noise @@ -96,10 +95,8 @@ void TricycleDrive::OnInitialize(const YAML::Node& config) { odom_twist_covar_default[7] = odom_twist_noise[1]; odom_twist_covar_default[35] = odom_twist_noise[2]; - auto odom_twist_covar = - r.GetArray("odom_twist_covariance", odom_twist_covar_default); - auto odom_pose_covar = - r.GetArray("odom_pose_covariance", odom_pose_covar_default); + auto odom_twist_covar = r.GetArray("odom_twist_covariance", odom_twist_covar_default); + auto odom_pose_covar = r.GetArray("odom_pose_covariance", odom_pose_covar_default); // Default max_steer_angle=0, max_angular_velocity=0, and // max_steer_acceleration=0 mean "unbounded" @@ -120,20 +117,17 @@ void TricycleDrive::OnInitialize(const YAML::Node& config) { front_wj_ = GetModel()->GetJoint(front_wj_name); if (front_wj_ == nullptr) { - throw YAMLException("Joint with name " + Q(front_wj_name) + - " does not exist"); + throw YAMLException("Joint with name " + Q(front_wj_name) + " does not exist"); } rear_left_wj_ = GetModel()->GetJoint(rear_left_wj_name); if (rear_left_wj_ == nullptr) { - throw YAMLException("Joint with name " + Q(rear_left_wj_name) + - " does not exist"); + throw YAMLException("Joint with name " + Q(rear_left_wj_name) + " does not exist"); } rear_right_wj_ = GetModel()->GetJoint(rear_right_wj_name); if (rear_right_wj_ == nullptr) { - throw YAMLException("Joint with name " + Q(rear_right_wj_name) + - " does not exist"); + throw YAMLException("Joint with name " + Q(rear_right_wj_name) + " does not exist"); } // validate the that joints fits the assumption of the robot model and @@ -149,8 +143,7 @@ void TricycleDrive::OnInitialize(const YAML::Node& config) { // init the values for the messages ground_truth_msg_.header.frame_id = odom_frame_id; - ground_truth_msg_.child_frame_id = - GetModel()->NameSpaceTF(body_->name_); + ground_truth_msg_.child_frame_id = GetModel()->NameSpaceTF(body_->name_); ground_truth_msg_.twist.covariance.fill(0); ground_truth_msg_.pose.covariance.fill(0); @@ -171,27 +164,26 @@ void TricycleDrive::OnInitialize(const YAML::Node& config) { } for (unsigned int i = 0; i < 3; i++) { - noise_gen_[i + 3] = - normal_distribution(0.0, sqrt(odom_twist_noise[i])); + noise_gen_[i + 3] = normal_distribution(0.0, sqrt(odom_twist_noise[i])); } - RCLCPP_DEBUG(rclcpp::get_logger("TricycleDrive"), - "Initialized with params body(%p %s) front_wj(%p %s) " - "rear_left_wj(%p %s) rear_right_wj(%p %s) " - "odom_frame_id(%s) twist_sub(%s) odom_pub(%s) " - "ground_truth_pub(%s) odom_pose_noise({%f,%f,%f}) " - "odom_twist_noise({%f,%f,%f}) pub_rate(%f)\n", - body_, body_->GetName().c_str(), front_wj_, front_wj_->GetName().c_str(), - rear_left_wj_, rear_left_wj_->GetName().c_str(), rear_right_wj_, - rear_right_wj_->GetName().c_str(), odom_frame_id.c_str(), - twist_topic.c_str(), odom_topic.c_str(), ground_truth_topic.c_str(), - odom_pose_noise[0], odom_pose_noise[1], odom_pose_noise[2], - odom_twist_noise[0], odom_twist_noise[1], odom_twist_noise[2], pub_rate); + RCLCPP_DEBUG( + rclcpp::get_logger("TricycleDrive"), + "Initialized with params body(%p %s) front_wj(%p %s) " + "rear_left_wj(%p %s) rear_right_wj(%p %s) " + "odom_frame_id(%s) twist_sub(%s) odom_pub(%s) " + "ground_truth_pub(%s) odom_pose_noise({%f,%f,%f}) " + "odom_twist_noise({%f,%f,%f}) pub_rate(%f)\n", + body_, body_->GetName().c_str(), front_wj_, front_wj_->GetName().c_str(), rear_left_wj_, + rear_left_wj_->GetName().c_str(), rear_right_wj_, rear_right_wj_->GetName().c_str(), + odom_frame_id.c_str(), twist_topic.c_str(), odom_topic.c_str(), ground_truth_topic.c_str(), + odom_pose_noise[0], odom_pose_noise[1], odom_pose_noise[2], odom_twist_noise[0], + odom_twist_noise[1], odom_twist_noise[2], pub_rate); } -void TricycleDrive::ComputeJoints() { - auto get_anchor = [&](Joint* joint, bool* is_inverted = nullptr) { - +void TricycleDrive::ComputeJoints() +{ + auto get_anchor = [&](Joint * joint, bool * is_inverted = nullptr) { b2Vec2 wheel_anchor; ///< wheel anchor point, must be (0,0) b2Vec2 body_anchor; ///< body anchor point bool inv = false; @@ -205,8 +197,8 @@ void TricycleDrive::ComputeJoints() { body_anchor = joint->physics_joint_->GetAnchorB(); inv = true; } else { - throw YAMLException("Joint " + Q(joint->GetName()) + - " does not anchor on body " + Q(body_->GetName())); + throw YAMLException( + "Joint " + Q(joint->GetName()) + " does not anchor on body " + Q(body_->GetName())); } // convert anchor is global coordinates to local body coordinates @@ -215,8 +207,8 @@ void TricycleDrive::ComputeJoints() { // ensure the joint is anchored at (0,0) of the wheel_body if (std::fabs(wheel_anchor.x) > 1e-5 || std::fabs(wheel_anchor.y) > 1e-5) { - throw YAMLException("Joint " + Q(joint->GetName()) + - " must be anchored at (0, 0) on the wheel"); + throw YAMLException( + "Joint " + Q(joint->GetName()) + " must be anchored at (0, 0) on the wheel"); } if (is_inverted) { @@ -240,8 +232,7 @@ void TricycleDrive::ComputeJoints() { } // enable limits for the front joint - b2RevoluteJoint* j = - dynamic_cast(front_wj_->physics_joint_); + b2RevoluteJoint * j = dynamic_cast(front_wj_->physics_joint_); j->EnableLimit(true); // positive joint angle goes counter clockwise from the perspective of BodyA, @@ -252,8 +243,7 @@ void TricycleDrive::ComputeJoints() { // the front wheel must be at (0,0) of the body if (std::fabs(front_anchor.x) > 1e-5 || std::fabs(front_anchor.y) > 1e-5) { - throw YAMLException( - "Front wheel joint must have its body anchored at (0, 0)"); + throw YAMLException("Front wheel joint must have its body anchored at (0, 0)"); } // calculate the wheelbase and axeltrack. We also need to verify that @@ -263,9 +253,8 @@ void TricycleDrive::ComputeJoints() { // find the perpendicular intersection between line segment given by (x1, y1) // and (x2, y2) and a point (x3, y3). - double x1 = rear_left_anchor.x, y1 = rear_left_anchor.y, - x2 = rear_right_anchor.x, y2 = rear_right_anchor.y, - x3 = front_anchor.x, y3 = front_anchor.y; + double x1 = rear_left_anchor.x, y1 = rear_left_anchor.y, x2 = rear_right_anchor.x, + y2 = rear_right_anchor.y, x3 = front_anchor.x, y3 = front_anchor.y; double k = ((y2 - y1) * (x3 - x1) - (x2 - x1) * (y3 - y1)) / ((y2 - y1) * (y2 - y1) + (x2 - x1) * (x2 - x1)); @@ -275,9 +264,9 @@ void TricycleDrive::ComputeJoints() { // check (x4, y4) equals to rear_center_ if (std::fabs(x4 - rear_center_.x) > 1e-5 || std::fabs(y4 - rear_center_.y) > 1e-5) { throw YAMLException( - "The mid point between the rear wheel anchors on the body must equal " - "the perpendicular intersection between the rear axel (line segment " - "between rear anchors) and the front wheel anchor"); + "The mid point between the rear wheel anchors on the body must equal " + "the perpendicular intersection between the rear axel (line segment " + "between rear anchors) and the front wheel anchor"); } // track is the separation between the rear two wheels, which is simply the @@ -289,10 +278,11 @@ void TricycleDrive::ComputeJoints() { wheelbase_ = sqrt((x4 - x3) * (x4 - x3) + (y4 - y3) * (y4 - y3)); } -void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { +void TricycleDrive::BeforePhysicsStep(const Timekeeper & timekeeper) +{ bool publish = update_timer_.CheckUpdate(timekeeper); - b2Body* b2body = body_->physics_body_; + b2Body * b2body = body_->physics_body_; b2Vec2 position = b2body->GetPosition(); float angle = b2body->GetAngle(); @@ -300,8 +290,7 @@ void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { if (publish) { // 1. get the state of the body and publish the data, // before the tricycle physics get updated - b2Vec2 linear_vel_local = - b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0)); + b2Vec2 linear_vel_local = b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0)); float angular_vel = b2body->GetAngularVelocity(); ground_truth_msg_.header.stamp = timekeeper.GetSimTime(); @@ -310,7 +299,7 @@ void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { ground_truth_msg_.pose.pose.position.z = 0; tf2::Quaternion q; q.setRPY(0, 0, angle); - ground_truth_msg_.pose.pose.orientation= tf2::toMsg(q); + ground_truth_msg_.pose.pose.orientation = tf2::toMsg(q); ground_truth_msg_.twist.twist.linear.x = linear_vel_local.x; ground_truth_msg_.twist.twist.linear.y = linear_vel_local.y; ground_truth_msg_.twist.twist.linear.z = 0; @@ -325,7 +314,7 @@ void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { odom_msg_.pose.pose.position.x += noise_gen_[0](rng_); odom_msg_.pose.pose.position.y += noise_gen_[1](rng_); q.setRPY(0, 0, angle + noise_gen_[2](rng_)); - odom_msg_.pose.pose.orientation= tf2::toMsg(q); + odom_msg_.pose.pose.orientation = tf2::toMsg(q); odom_msg_.twist.twist.linear.x += noise_gen_[3](rng_); odom_msg_.twist.twist.linear.y += noise_gen_[4](rng_); odom_msg_.twist.twist.angular.z += noise_gen_[5](rng_); @@ -357,7 +346,7 @@ void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { // twist message contains the speed and angle of the front wheel double v_f = twist_msg_->linear.x; // target velocity at front wheel delta_command_ = twist_msg_->angular.z; // target steering angle - double theta = angle; // angle of robot in map frame + double theta = angle; // angle of robot in map frame double dt = timekeeper.GetStepSize(); // In the simulation, the equations of motion have to be computed backwards @@ -374,15 +363,13 @@ void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { } if (max_steer_velocity_ != 0.0) { // Saturating the command also saturates the steering velocity - d_delta_command = - Saturate(d_delta_command, -max_steer_velocity_, max_steer_velocity_); + d_delta_command = Saturate(d_delta_command, -max_steer_velocity_, max_steer_velocity_); } // (3) Update the new steering acceleration double d2_delta = (d_delta_command - d_delta_) / dt; if (max_steer_acceleration_ != 0.0) { - d2_delta = - Saturate(d2_delta, -max_steer_acceleration_, max_steer_acceleration_); + d2_delta = Saturate(d2_delta, -max_steer_acceleration_, max_steer_acceleration_); } // (2) Update the new steering velocity @@ -395,16 +382,15 @@ void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { } rclcpp::Clock steady_clock = rclcpp::Clock(RCL_STEADY_TIME); - RCLCPP_DEBUG_THROTTLE(rclcpp::get_logger("TricycleDrive"), steady_clock, 500, - "Using new tricycle steering, d2_delta = %.4f, " - "d_delta = %.4f, twist.x = %.4f, twist.delta = %.4f", - d2_delta, d_delta_, twist_msg_->linear.x, - twist_msg_->angular.z); + RCLCPP_DEBUG_THROTTLE( + rclcpp::get_logger("TricycleDrive"), steady_clock, 500, + "Using new tricycle steering, d2_delta = %.4f, " + "d_delta = %.4f, twist.x = %.4f, twist.delta = %.4f", + d2_delta, d_delta_, twist_msg_->linear.x, twist_msg_->angular.z); // change angle of the front wheel for visualization - b2RevoluteJoint* j = - dynamic_cast(front_wj_->physics_joint_); + b2RevoluteJoint * j = dynamic_cast(front_wj_->physics_joint_); j->EnableLimit(true); if (invert_steering_angle_) { j->SetLimits(-delta_, -delta_); @@ -439,11 +425,13 @@ void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) { b2body->SetAngularVelocity(w); } -void TricycleDrive::TwistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { +void TricycleDrive::TwistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ twist_msg_ = msg; } -double TricycleDrive::Saturate(double in, double lower, double upper) { +double TricycleDrive::Saturate(double in, double lower, double upper) +{ if (lower > upper) { return in; } @@ -452,7 +440,6 @@ double TricycleDrive::Saturate(double in, double lower, double upper) { out = std::min(out, upper); return out; } -} +} // namespace flatland_plugins -PLUGINLIB_EXPORT_CLASS(flatland_plugins::TricycleDrive, - flatland_server::ModelPlugin) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(flatland_plugins::TricycleDrive, flatland_server::ModelPlugin) \ No newline at end of file diff --git a/flatland_plugins/src/tween.cpp b/flatland_plugins/src/tween.cpp index ba647bdf..afb97fcb 100644 --- a/flatland_plugins/src/tween.cpp +++ b/flatland_plugins/src/tween.cpp @@ -48,50 +48,53 @@ #include #include #include + #include #include //#include -namespace flatland_plugins { +namespace flatland_plugins +{ std::map Tween::mode_strings_ = { - {"yoyo", Tween::ModeType_::YOYO}, - {"loop", Tween::ModeType_::LOOP}, - {"once", Tween::ModeType_::ONCE}, - {"trigger", Tween::ModeType_::TRIGGER}}; + {"yoyo", Tween::ModeType_::YOYO}, + {"loop", Tween::ModeType_::LOOP}, + {"once", Tween::ModeType_::ONCE}, + {"trigger", Tween::ModeType_::TRIGGER}}; std::map Tween::easing_strings_ = { - {"linear", Tween::EasingType_::linear}, - {"quadraticIn", Tween::EasingType_::quadraticIn}, - {"quadraticOut", Tween::EasingType_::quadraticOut}, - {"quadraticInOut", Tween::EasingType_::quadraticInOut}, - {"cubicIn", Tween::EasingType_::cubicIn}, - {"cubicOut", Tween::EasingType_::cubicOut}, - {"cubicInOut", Tween::EasingType_::cubicInOut}, - {"quarticIn", Tween::EasingType_::quarticIn}, - {"quarticOut", Tween::EasingType_::quarticOut}, - {"quarticInOut", Tween::EasingType_::quarticInOut}, - {"quinticIn", Tween::EasingType_::quinticIn}, - {"quinticOut", Tween::EasingType_::quinticOut}, - {"quinticInOut", Tween::EasingType_::quinticInOut}, - // { "sinuisodal", Tween::EasingType_::sinuisodal }, - {"exponentialIn", Tween::EasingType_::exponentialIn}, - {"exponentialOut", Tween::EasingType_::exponentialOut}, - {"exponentialInOut", Tween::EasingType_::exponentialInOut}, - {"circularIn", Tween::EasingType_::circularIn}, - {"circularOut", Tween::EasingType_::circularOut}, - {"circularInOut", Tween::EasingType_::circularInOut}, - {"backIn", Tween::EasingType_::backIn}, - {"backOut", Tween::EasingType_::backOut}, - {"backInOut", Tween::EasingType_::backInOut}, - {"elasticIn", Tween::EasingType_::elasticIn}, - {"elasticOut", Tween::EasingType_::elasticOut}, - {"elasticInOut", Tween::EasingType_::elasticInOut}, - {"bounceIn", Tween::EasingType_::bounceIn}, - {"bounceOut", Tween::EasingType_::bounceOut}, - {"bounceInOut", Tween::EasingType_::bounceInOut}}; + {"linear", Tween::EasingType_::linear}, + {"quadraticIn", Tween::EasingType_::quadraticIn}, + {"quadraticOut", Tween::EasingType_::quadraticOut}, + {"quadraticInOut", Tween::EasingType_::quadraticInOut}, + {"cubicIn", Tween::EasingType_::cubicIn}, + {"cubicOut", Tween::EasingType_::cubicOut}, + {"cubicInOut", Tween::EasingType_::cubicInOut}, + {"quarticIn", Tween::EasingType_::quarticIn}, + {"quarticOut", Tween::EasingType_::quarticOut}, + {"quarticInOut", Tween::EasingType_::quarticInOut}, + {"quinticIn", Tween::EasingType_::quinticIn}, + {"quinticOut", Tween::EasingType_::quinticOut}, + {"quinticInOut", Tween::EasingType_::quinticInOut}, + // { "sinuisodal", Tween::EasingType_::sinuisodal }, + {"exponentialIn", Tween::EasingType_::exponentialIn}, + {"exponentialOut", Tween::EasingType_::exponentialOut}, + {"exponentialInOut", Tween::EasingType_::exponentialInOut}, + {"circularIn", Tween::EasingType_::circularIn}, + {"circularOut", Tween::EasingType_::circularOut}, + {"circularInOut", Tween::EasingType_::circularInOut}, + {"backIn", Tween::EasingType_::backIn}, + {"backOut", Tween::EasingType_::backOut}, + {"backInOut", Tween::EasingType_::backInOut}, + {"elasticIn", Tween::EasingType_::elasticIn}, + {"elasticOut", Tween::EasingType_::elasticOut}, + {"elasticInOut", Tween::EasingType_::elasticInOut}, + {"bounceIn", Tween::EasingType_::bounceIn}, + {"bounceOut", Tween::EasingType_::bounceOut}, + {"bounceInOut", Tween::EasingType_::bounceInOut}}; -void Tween::OnInitialize(const YAML::Node& config) { +void Tween::OnInitialize(const YAML::Node & config) +{ YamlReader reader(node_, config); std::string body_name = reader.Get("body"); @@ -112,9 +115,9 @@ void Tween::OnInitialize(const YAML::Node& config) { if (body_ == nullptr) { throw YAMLException("Body with name " + Q(body_name) + " does not exist"); } - start_ = Pose(body_->physics_body_->GetPosition().x, - body_->physics_body_->GetPosition().y, - body_->physics_body_->GetAngle()); + start_ = Pose( + body_->physics_body_->GetPosition().x, body_->physics_body_->GetPosition().y, + body_->physics_body_->GetAngle()); // Validate the mode selection if (!Tween::mode_strings_.count(mode)) { @@ -123,8 +126,8 @@ void Tween::OnInitialize(const YAML::Node& config) { mode_ = Tween::mode_strings_.at(mode); tween_ = tweeny::from(0.0, 0.0, 0.0) - .to(delta_.x, delta_.y, delta_.theta) - .during((uint32)(duration_ * 1000.0)); + .to(delta_.x, delta_.y, delta_.theta) + .during((uint32)(duration_ * 1000.0)); Tween::EasingType_ easing_type; std::string easing = reader.Get("easing", "linear"); @@ -231,31 +234,28 @@ void Tween::OnInitialize(const YAML::Node& config) { // Make sure there are no unused keys reader.EnsureAccessedAllKeys(); - RCLCPP_DEBUG(rclcpp::get_logger("Tween"), - "Initialized with params body(%p %s) " - "start ({%f,%f,%f}) " - "end ({%f,%f,%f}) " - "duration %f " - "mode: %s [%d] " - "easing: %s\n", - body_, body_->name_.c_str(), start_.x, start_.y, start_.theta, - delta_.x, delta_.y, delta_.theta, duration_, mode.c_str(), - (int)mode_, easing.c_str()); + RCLCPP_DEBUG( + rclcpp::get_logger("Tween"), + "Initialized with params body(%p %s) " + "start ({%f,%f,%f}) " + "end ({%f,%f,%f}) " + "duration %f " + "mode: %s [%d] " + "easing: %s\n", + body_, body_->name_.c_str(), start_.x, start_.y, start_.theta, delta_.x, delta_.y, delta_.theta, + duration_, mode.c_str(), (int)mode_, easing.c_str()); } -void Tween::TriggerCallback(const std_msgs::msg::Bool::SharedPtr msg) { - triggered_ = msg->data; -} +void Tween::TriggerCallback(const std_msgs::msg::Bool::SharedPtr msg) { triggered_ = msg->data; } -void Tween::BeforePhysicsStep(const Timekeeper& timekeeper) { - std::array v = - tween_.step((uint32)(timekeeper.GetStepSize() * 1000.0)); +void Tween::BeforePhysicsStep(const Timekeeper & timekeeper) +{ + std::array v = tween_.step((uint32)(timekeeper.GetStepSize() * 1000.0)); rclcpp::Clock steady_clock = rclcpp::Clock(RCL_STEADY_TIME); - RCLCPP_DEBUG_THROTTLE(rclcpp::get_logger("Tween"), steady_clock, 1000, "value %f,%f,%f step %f progress %f", - v[0], v[1], v[2], timekeeper.GetStepSize(), - tween_.progress()); - body_->physics_body_->SetTransform(b2Vec2(start_.x + v[0], start_.y + v[1]), - start_.theta + v[2]); + RCLCPP_DEBUG_THROTTLE( + rclcpp::get_logger("Tween"), steady_clock, 1000, "value %f,%f,%f step %f progress %f", v[0], + v[1], v[2], timekeeper.GetStepSize(), tween_.progress()); + body_->physics_body_->SetTransform(b2Vec2(start_.x + v[0], start_.y + v[1]), start_.theta + v[2]); // Tell Box2D to update the AABB and check for collisions for this object body_->physics_body_->SetAwake(true); @@ -284,6 +284,6 @@ void Tween::BeforePhysicsStep(const Timekeeper& timekeeper) { } } } -} +} // namespace flatland_plugins PLUGINLIB_EXPORT_CLASS(flatland_plugins::Tween, flatland_server::ModelPlugin) \ No newline at end of file diff --git a/flatland_plugins/src/update_timer.cpp b/flatland_plugins/src/update_timer.cpp index d73c9acc..0c562b76 100644 --- a/flatland_plugins/src/update_timer.cpp +++ b/flatland_plugins/src/update_timer.cpp @@ -45,21 +45,26 @@ */ #include + #include -namespace flatland_plugins { +namespace flatland_plugins +{ -UpdateTimer::UpdateTimer() - : period_(rclcpp::Duration(0,0)), last_update_time_(rclcpp::Time(0, 0)) {} +UpdateTimer::UpdateTimer() : period_(rclcpp::Duration(0, 0)), last_update_time_(rclcpp::Time(0, 0)) +{ +} -void UpdateTimer::SetRate(double rate) { +void UpdateTimer::SetRate(double rate) +{ if (rate == 0.0) period_ = rclcpp::Duration(INT32_MAX, 0); // 1000 hours is infinity right? else period_ = rclcpp::Duration::from_seconds(1.0 / rate); } -bool UpdateTimer::CheckUpdate(const flatland_server::Timekeeper &timekeeper) { +bool UpdateTimer::CheckUpdate(const flatland_server::Timekeeper & timekeeper) +{ if (std::fabs(period_.seconds()) < 1e-5) { return true; } @@ -78,8 +83,7 @@ bool UpdateTimer::CheckUpdate(const flatland_server::Timekeeper &timekeeper) { // is stable and close to max step size. // hector_gazebo/hector_gazebo_plugins/include/hector_gazebo_plugins/update_timer.h double step = timekeeper.GetMaxStepSize(); - double fraction = - std::fmod(timekeeper.GetSimTime().seconds() + (step / 2.0), period_.seconds()); + double fraction = std::fmod(timekeeper.GetSimTime().seconds() + (step / 2.0), period_.seconds()); if ((fraction >= 0.0) && (fraction < step)) { last_update_time_ = timekeeper.GetSimTime(); @@ -88,4 +92,4 @@ bool UpdateTimer::CheckUpdate(const flatland_server::Timekeeper &timekeeper) { return false; } -}; +}; // namespace flatland_plugins diff --git a/flatland_plugins/src/world_modifier.cpp b/flatland_plugins/src/world_modifier.cpp index 690a98c5..6a20cc43 100644 --- a/flatland_plugins/src/world_modifier.cpp +++ b/flatland_plugins/src/world_modifier.cpp @@ -44,20 +44,19 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include - #include #include #include #include #include #include -#include #include +#include #include #include #include +#include #include #include @@ -66,10 +65,12 @@ using std::endl; using namespace std::placeholders; using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ -float RayTrace::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, - const b2Vec2 &normal, float fraction) { +float RayTrace::ReportFixture( + b2Fixture * fixture, const b2Vec2 & point, const b2Vec2 & normal, float fraction) +{ // only register hit in the specified layers if (!(fixture->GetFilterData().categoryBits & category_bits_)) { // cout << "hit others " << endl; @@ -80,17 +81,20 @@ float RayTrace::ReportFixture(b2Fixture *fixture, const b2Vec2 &point, return fraction; } -WorldModifier::WorldModifier(flatland_server::World *world, - std::string layer_name, double wall_wall_dist, - bool double_wall, Pose robot_ini_pose) - : world_(world), - layer_name_(layer_name), - wall_wall_dist_(wall_wall_dist), - double_wall_(double_wall), - robot_ini_pose_(robot_ini_pose) {} +WorldModifier::WorldModifier( + flatland_server::World * world, std::string layer_name, double wall_wall_dist, bool double_wall, + Pose robot_ini_pose) +: world_(world), + layer_name_(layer_name), + wall_wall_dist_(wall_wall_dist), + double_wall_(double_wall), + robot_ini_pose_(robot_ini_pose) +{ +} -void WorldModifier::CalculateNewWall(double d, b2Vec2 vertex1, b2Vec2 vertex2, - b2EdgeShape &new_wall) { +void WorldModifier::CalculateNewWall( + double d, b2Vec2 vertex1, b2Vec2 vertex2, b2EdgeShape & new_wall) +{ b2Vec2 new_wall_v1; b2Vec2 new_wall_v2; if (d == 0) { // if distance towards the robot is 0 @@ -121,11 +125,12 @@ void WorldModifier::CalculateNewWall(double d, b2Vec2 vertex1, b2Vec2 vertex2, new_wall.Set(new_wall_v1, new_wall_v2); } -void WorldModifier::AddWall(b2EdgeShape &new_wall) { - Layer *layer = NULL; +void WorldModifier::AddWall(b2EdgeShape & new_wall) +{ + Layer * layer = NULL; std::vector cfr_names; - for (auto &it : world_->layers_name_map_) { - for (auto &v_it : it.first) { + for (auto & it : world_->layers_name_map_) { + for (auto & v_it : it.first) { if (v_it == layer_name_) { layer = it.second; cfr_names = it.first; @@ -145,17 +150,17 @@ void WorldModifier::AddWall(b2EdgeShape &new_wall) { layer->body_->physics_body_->CreateFixture(&fixture_def); } -void WorldModifier::AddSideWall(b2EdgeShape &old_wall, b2EdgeShape &new_wall) { +void WorldModifier::AddSideWall(b2EdgeShape & old_wall, b2EdgeShape & new_wall) +{ b2Vec2 old_wall_v1 = old_wall.m_vertex1; b2Vec2 old_wall_v2 = old_wall.m_vertex2; b2Vec2 new_wall_v1 = new_wall.m_vertex1; b2Vec2 new_wall_v2 = new_wall.m_vertex2; // first side double k = - ((old_wall_v2.y - old_wall_v1.y) * (new_wall_v1.x - old_wall_v1.x) - - (old_wall_v2.x - old_wall_v1.x) * (new_wall_v1.y - old_wall_v1.y)) / - (std::pow((old_wall_v2.y - old_wall_v1.y), 2) + - std::pow((old_wall_v2.x - old_wall_v1.x), 2)); + ((old_wall_v2.y - old_wall_v1.y) * (new_wall_v1.x - old_wall_v1.x) - + (old_wall_v2.x - old_wall_v1.x) * (new_wall_v1.y - old_wall_v1.y)) / + (std::pow((old_wall_v2.y - old_wall_v1.y), 2) + std::pow((old_wall_v2.x - old_wall_v1.x), 2)); double x = new_wall_v1.x - k * (old_wall_v2.y - old_wall_v1.y); double y = new_wall_v1.y + k * (old_wall_v2.x - old_wall_v1.x); b2EdgeShape first_wall; @@ -165,8 +170,7 @@ void WorldModifier::AddSideWall(b2EdgeShape &old_wall, b2EdgeShape &new_wall) { // second side k = ((old_wall_v2.y - old_wall_v1.y) * (new_wall_v2.x - old_wall_v1.x) - (old_wall_v2.x - old_wall_v1.x) * (new_wall_v2.y - old_wall_v1.y)) / - (std::pow((old_wall_v2.y - old_wall_v1.y), 2) + - std::pow((old_wall_v2.x - old_wall_v1.x), 2)); + (std::pow((old_wall_v2.y - old_wall_v1.y), 2) + std::pow((old_wall_v2.x - old_wall_v1.x), 2)); x = new_wall_v2.x - k * (old_wall_v2.y - old_wall_v1.y); y = new_wall_v2.y + k * (old_wall_v2.x - old_wall_v1.x); b2EdgeShape second_wall; @@ -174,7 +178,8 @@ void WorldModifier::AddSideWall(b2EdgeShape &old_wall, b2EdgeShape &new_wall) { AddWall(second_wall); } -void WorldModifier::AddFullWall(b2EdgeShape *wall) { +void WorldModifier::AddFullWall(b2EdgeShape * wall) +{ b2Vec2 vertex1 = wall->m_vertex1; b2Vec2 vertex2 = wall->m_vertex2; double d = (robot_ini_pose_.x - vertex1.x) * (vertex2.y - vertex1.y) - @@ -194,4 +199,4 @@ void WorldModifier::AddFullWall(b2EdgeShape *wall) { AddSideWall(*wall, new_wall); } } -}; // namespace \ No newline at end of file +}; // namespace flatland_plugins \ No newline at end of file diff --git a/flatland_plugins/src/world_random_wall.cpp b/flatland_plugins/src/world_random_wall.cpp index d244c29d..dcf6239b 100644 --- a/flatland_plugins/src/world_random_wall.cpp +++ b/flatland_plugins/src/world_random_wall.cpp @@ -49,29 +49,31 @@ #include #include #include -#include -#include #include + #include #include +#include +#include #include using namespace flatland_server; using std::cout; using std::endl; -namespace flatland_plugins { -void RandomWall::OnInitialize(const YAML::Node &config, YamlReader &world_config) { +namespace flatland_plugins +{ +void RandomWall::OnInitialize(const YAML::Node & config, YamlReader & world_config) +{ // read in the plugin config YamlReader plugin_reader(node_, config); std::string layer_name = plugin_reader.Get("layer", ""); - unsigned int num_of_walls = - plugin_reader.Get("num_of_walls", 0); + unsigned int num_of_walls = plugin_reader.Get("num_of_walls", 0); double wall_wall_dist = plugin_reader.Get("wall_wall_dist", 1); bool double_wall = plugin_reader.Get("double_wall", false); std::string robot_name = plugin_reader.Get("robot_name", ""); - Layer *layer = NULL; - for (auto &it : world_->layers_name_map_) { - for (auto &v_it : it.first) { + Layer * layer = NULL; + for (auto & it : world_->layers_name_map_) { + for (auto & v_it : it.first) { if (v_it == layer_name) { layer = it.second; break; @@ -84,22 +86,19 @@ void RandomWall::OnInitialize(const YAML::Node &config, YamlReader &world_config // read in the robot location from the world.yaml Pose robot_ini_pose; - YamlReader models_reader = - world_config.SubnodeOpt("models", YamlReader::LIST); + YamlReader models_reader = world_config.SubnodeOpt("models", YamlReader::LIST); if (models_reader.IsNodeNull()) { throw("no robot specified!"); } for (int i = 0; i < models_reader.NodeSize(); i++) { YamlReader reader = models_reader.Subnode(i, YamlReader::MAP); - if (i + 1 >= models_reader.NodeSize() && - reader.Get("name") != robot_name) { + if (i + 1 >= models_reader.NodeSize() && reader.Get("name") != robot_name) { throw("cannot find specified robot name of " + robot_name); } if (reader.Get("name") == robot_name) { robot_ini_pose = reader.Get("pose", Pose(0, 0, 0)); b2Transform tran = layer->body_->physics_body_->GetTransform(); - b2Vec2 ini_pose = - b2MulT(tran, b2Vec2(robot_ini_pose.x, robot_ini_pose.y)); + b2Vec2 ini_pose = b2MulT(tran, b2Vec2(robot_ini_pose.x, robot_ini_pose.y)); robot_ini_pose.x = ini_pose.x; robot_ini_pose.y = ini_pose.y; break; @@ -107,13 +106,11 @@ void RandomWall::OnInitialize(const YAML::Node &config, YamlReader &world_config } // create the world modifiyer cout << "robot location read" << endl; - WorldModifier modifier(world_, layer_name, wall_wall_dist, double_wall, - robot_ini_pose); + WorldModifier modifier(world_, layer_name, wall_wall_dist, double_wall, robot_ini_pose); // get all walls std::vector Wall_List; - for (b2Fixture *f = layer->body_->physics_body_->GetFixtureList(); f; - f = f->GetNext()) { + for (b2Fixture * f = layer->body_->physics_body_->GetFixtureList(); f; f = f->GetNext()) { Wall_List.push_back(static_cast(f->GetShape())); } std::srand(std::time(0)); @@ -126,7 +123,6 @@ void RandomWall::OnInitialize(const YAML::Node &config, YamlReader &world_config throw e; } } -}; // namespace +}; // namespace flatland_plugins -PLUGINLIB_EXPORT_CLASS(flatland_plugins::RandomWall, - flatland_server::WorldPlugin) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(flatland_plugins::RandomWall, flatland_server::WorldPlugin) \ No newline at end of file diff --git a/flatland_plugins/test/bumper_test.cpp b/flatland_plugins/test/bumper_test.cpp index 5da53a54..1d69d73a 100644 --- a/flatland_plugins/test/bumper_test.cpp +++ b/flatland_plugins/test/bumper_test.cpp @@ -60,28 +60,32 @@ using namespace flatland_plugins; using namespace flatland_msgs::msg; using std::placeholders::_1; -class BumperPluginTest : public ::testing::Test { - public: +class BumperPluginTest : public ::testing::Test +{ +public: boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; flatland_msgs::msg::Collisions msg1, msg2; - World* w; + World * w; std::shared_ptr node; BumperPluginTest() : node(rclcpp::Node::make_shared("test_bumper_plugin")) {} - void SetUp() override { + void SetUp() override + { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); w = nullptr; } - void TearDown() override { + void TearDown() override + { if (w != nullptr) { delete w; } } - static bool fltcmp(const float& n1, const float& n2, float epsilon = 1e-5) { + static bool fltcmp(const float & n1, const float & n2, float epsilon = 1e-5) + { if (std::isinf(n1) && std::isinf(n2)) { return true; } @@ -94,35 +98,36 @@ class BumperPluginTest : public ::testing::Test { return ret; } - bool StringEq(const std::string& name, const std::string& actual, - const std::string& expected) { + bool StringEq(const std::string & name, const std::string & actual, const std::string & expected) + { if (actual != expected) { - printf("%s Actual:%s != Expected:%s\n", name.c_str(), actual.c_str(), - expected.c_str()); + printf("%s Actual:%s != Expected:%s\n", name.c_str(), actual.c_str(), expected.c_str()); return false; } return true; } - bool Vector2Eq(const std::string& name, const Vector2& actual, - const std::pair expected) { - if (!fltcmp(actual.x, expected.first) || - !fltcmp(actual.y, expected.second)) { - printf("%s Actual:(%f,%f) != Expected:(%f,%f)\n", name.c_str(), actual.x, - actual.y, expected.first, expected.second); + bool Vector2Eq( + const std::string & name, const Vector2 & actual, const std::pair expected) + { + if (!fltcmp(actual.x, expected.first) || !fltcmp(actual.y, expected.second)) { + printf( + "%s Actual:(%f,%f) != Expected:(%f,%f)\n", name.c_str(), actual.x, actual.y, expected.first, + expected.second); return false; } return true; } - bool CollisionsEq(const Collisions& collisions, const std::string& frame_id, - size_t num_collisions) { - if (!StringEq("frame_id", collisions.header.frame_id, frame_id)) - return false; + bool CollisionsEq( + const Collisions & collisions, const std::string & frame_id, size_t num_collisions) + { + if (!StringEq("frame_id", collisions.header.frame_id, frame_id)) return false; if (num_collisions != collisions.collisions.size()) { - printf("Num collisions Actual:%lu != Expected:%lu\n", - collisions.collisions.size(), num_collisions); + printf( + "Num collisions Actual:%lu != Expected:%lu\n", collisions.collisions.size(), + num_collisions); return false; } @@ -130,26 +135,26 @@ class BumperPluginTest : public ::testing::Test { } // check the received scan data is as expected - bool CollisionEq(const Collision& collision, const std::string& entity_a, - const std::string& body_A, const std::string& entity_b, - const std::string& body_B, size_t return_size, - const std::pair& normal) { + bool CollisionEq( + const Collision & collision, const std::string & entity_a, const std::string & body_A, + const std::string & entity_b, const std::string & body_B, size_t return_size, + const std::pair & normal) + { if (!StringEq("entity_a", collision.entity_a, entity_a)) return false; if (!StringEq("body_A", collision.body_a, body_A)) return false; if (!StringEq("entity_b", collision.entity_b, entity_b)) return false; if (!StringEq("body_B", collision.body_b, body_B)) return false; - if (!(collision.magnitude_forces.size() <= 2 && - collision.contact_positions.size() <= 2 && + if (!(collision.magnitude_forces.size() <= 2 && collision.contact_positions.size() <= 2 && collision.contact_normals.size() <= 2 && collision.magnitude_forces.size() == return_size && collision.contact_positions.size() == return_size && collision.contact_normals.size() == return_size)) { printf( - "Vector sizes are expected to be all the same and have sizes %lu, " - "magnitude_forces=%lu contact_positions=%lu contact_normals=%lu\n", - return_size, collision.magnitude_forces.size(), - collision.contact_positions.size(), collision.contact_normals.size()); + "Vector sizes are expected to be all the same and have sizes %lu, " + "magnitude_forces=%lu contact_positions=%lu contact_normals=%lu\n", + return_size, collision.magnitude_forces.size(), collision.contact_positions.size(), + collision.contact_normals.size()); return false; } @@ -157,8 +162,7 @@ class BumperPluginTest : public ::testing::Test { std::string idx = "[" + std::to_string(i) + "]"; if (collision.magnitude_forces[i] <= 0) { - printf("forces%s Actual:%f != Expected: >0\n", idx.c_str(), - collision.magnitude_forces[i]); + printf("forces%s Actual:%f != Expected: >0\n", idx.c_str(), collision.magnitude_forces[i]); return false; } @@ -169,11 +173,12 @@ class BumperPluginTest : public ::testing::Test { return true; } - void CollisionCb_A(const Collisions& msg) { msg1 = msg; } + void CollisionCb_A(const Collisions & msg) { msg1 = msg; } - void CollisionCb_B(const Collisions& msg) { msg2 = msg; } + void CollisionCb_B(const Collisions & msg) { msg2 = msg; } - void SpinRos(float hz, unsigned iterations) { + void SpinRos(float hz, unsigned iterations) + { rclcpp::WallRate rate(hz); for (unsigned int i = 0; i < iterations; i++) { rclcpp::spin_some(node); @@ -185,25 +190,25 @@ class BumperPluginTest : public ::testing::Test { /** * Test the bumper plugin for a given model and plugin configuration */ -TEST_F(BumperPluginTest, collision_test) { - world_yaml = - this_file_dir / fs::path("bumper_tests/collision_test/world.yaml"); +TEST_F(BumperPluginTest, collision_test) +{ + world_yaml = this_file_dir / fs::path("bumper_tests/collision_test/world.yaml"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.01); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); - BumperPluginTest* obj = dynamic_cast(this); + BumperPluginTest * obj = dynamic_cast(this); auto sub_1 = node->create_subscription( - "collisions", 1, std::bind(&BumperPluginTest::CollisionCb_A, obj, _1)); + "collisions", 1, std::bind(&BumperPluginTest::CollisionCb_A, obj, _1)); auto sub_2 = node->create_subscription( - "collisions_B", 1, std::bind(&BumperPluginTest::CollisionCb_B, obj, _1)); + "collisions_B", 1, std::bind(&BumperPluginTest::CollisionCb_B, obj, _1)); - Bumper* p = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + Bumper * p = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - Body* b0 = p->GetModel()->bodies_[0]; - Body* b1 = p->GetModel()->bodies_[1]; + Body * b0 = p->GetModel()->bodies_[0]; + Body * b1 = p->GetModel()->bodies_[1]; // check that there are no collision at the beginning for (unsigned int i = 0; i < 100; i++) { @@ -228,8 +233,8 @@ TEST_F(BumperPluginTest, collision_test) { SpinRos(500, 10); // makes sure the ros message gets through ASSERT_TRUE(CollisionsEq(msg1, "map", 1)); - EXPECT_TRUE(CollisionEq(msg1.collisions[0], "robot1", "base_link_1", - "layer_1", "layer_1", 0, {})); + EXPECT_TRUE( + CollisionEq(msg1.collisions[0], "robot1", "base_link_1", "layer_1", "layer_1", 0, {})); EXPECT_TRUE(CollisionsEq(msg2, "world", 0)); // step another 5 times which moves 0.5 meters colliding base_link_2 as well @@ -240,13 +245,13 @@ TEST_F(BumperPluginTest, collision_test) { } SpinRos(500, 10); ASSERT_TRUE(CollisionsEq(msg1, "map", 2)); - EXPECT_TRUE(CollisionEq(msg1.collisions[0], "robot1", "base_link_1", - "layer_1", "layer_1", 0, {})); - EXPECT_TRUE(CollisionEq(msg1.collisions[1], "robot1", "base_link_2", - "layer_1", "layer_1", 1, {1, 0})); + EXPECT_TRUE( + CollisionEq(msg1.collisions[0], "robot1", "base_link_1", "layer_1", "layer_1", 0, {})); + EXPECT_TRUE( + CollisionEq(msg1.collisions[1], "robot1", "base_link_2", "layer_1", "layer_1", 1, {1, 0})); ASSERT_TRUE(CollisionsEq(msg2, "world", 1)); - EXPECT_TRUE(CollisionEq(msg2.collisions[0], "robot1", "base_link_2", - "layer_1", "layer_1", 1, {1, 0})); + EXPECT_TRUE( + CollisionEq(msg2.collisions[0], "robot1", "base_link_2", "layer_1", "layer_1", 1, {1, 0})); // Now move backward far away from the wall, there collisions should clear for (unsigned int i = 0; i < 300; i++) { @@ -272,13 +277,13 @@ TEST_F(BumperPluginTest, collision_test) { SpinRos(500, 10); ASSERT_TRUE(CollisionsEq(msg1, "map", 2)); - EXPECT_TRUE(CollisionEq(msg1.collisions[0], "robot1", "base_link_1", - "layer_1", "layer_1", 0, {})); - EXPECT_TRUE(CollisionEq(msg1.collisions[1], "robot1", "base_link_2", - "layer_1", "layer_1", 1, {-1, 0})); + EXPECT_TRUE( + CollisionEq(msg1.collisions[0], "robot1", "base_link_1", "layer_1", "layer_1", 0, {})); + EXPECT_TRUE( + CollisionEq(msg1.collisions[1], "robot1", "base_link_2", "layer_1", "layer_1", 1, {-1, 0})); ASSERT_TRUE(CollisionsEq(msg2, "world", 1)); - EXPECT_TRUE(CollisionEq(msg2.collisions[0], "robot1", "base_link_2", - "layer_1", "layer_1", 1, {-1, 0})); + EXPECT_TRUE( + CollisionEq(msg2.collisions[0], "robot1", "base_link_2", "layer_1", "layer_1", 1, {-1, 0})); // w->DebugVisualize(); // DebugVisualization::Get().Publish(); // ros::spin(); @@ -287,21 +292,22 @@ TEST_F(BumperPluginTest, collision_test) { /** * Test with a invalid body specified in the exclude list */ -TEST_F(BumperPluginTest, invalid_A) { +TEST_F(BumperPluginTest, invalid_A) +{ world_yaml = this_file_dir / fs::path("bumper_tests/invalid_A/world.yaml"); try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { + } catch (const PluginException & e) { std::cmatch match; std::string regex_str = ".*Body with name \"random_body\" does not exist.*"; std::regex regex(regex_str); EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { + << "Exception Message '" + std::string(e.what()) + "'" + " did not match against regex '" + + regex_str + "'"; + } catch (const std::exception & e) { ADD_FAILURE() << "Was expecting a PluginException, another exception was " "caught instead: " << e.what(); @@ -309,7 +315,8 @@ TEST_F(BumperPluginTest, invalid_A) { } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_plugins/test/diff_drive_test.cpp b/flatland_plugins/test/diff_drive_test.cpp index 1c156446..f091777e 100644 --- a/flatland_plugins/test/diff_drive_test.cpp +++ b/flatland_plugins/test/diff_drive_test.cpp @@ -50,22 +50,23 @@ #include #include -TEST(DiffDrivePluginTest, load_test) { - std::shared_ptr node = - rclcpp::Node::make_shared("test_diff_drive_plugin"); +TEST(DiffDrivePluginTest, load_test) +{ + std::shared_ptr node = rclcpp::Node::make_shared("test_diff_drive_plugin"); pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::ModelPlugin"); + "flatland_server", "flatland_server::ModelPlugin"); try { std::shared_ptr plugin = - loader.createSharedInstance("flatland_plugins::DiffDrive"); - } catch (pluginlib::PluginlibException& e) { + loader.createSharedInstance("flatland_plugins::DiffDrive"); + } catch (pluginlib::PluginlibException & e) { FAIL() << "Failed to load diff drive Drive plugin. " << e.what(); } } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_plugins/test/gps_test.cpp b/flatland_plugins/test/gps_test.cpp index f4e78056..8d823ec4 100644 --- a/flatland_plugins/test/gps_test.cpp +++ b/flatland_plugins/test/gps_test.cpp @@ -4,22 +4,23 @@ #include #include -TEST(GpsPluginTest, load_test) { - std::shared_ptr node = - rclcpp::Node::make_shared("test_gps_plugin"); +TEST(GpsPluginTest, load_test) +{ + std::shared_ptr node = rclcpp::Node::make_shared("test_gps_plugin"); pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::ModelPlugin"); + "flatland_server", "flatland_server::ModelPlugin"); try { std::shared_ptr plugin = - loader.createSharedInstance("flatland_plugins::Gps"); - } catch (pluginlib::PluginlibException& e) { + loader.createSharedInstance("flatland_plugins::Gps"); + } catch (pluginlib::PluginlibException & e) { FAIL() << "Failed to load GPS plugin. " << e.what(); } } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_plugins/test/laser_test.cpp b/flatland_plugins/test/laser_test.cpp index d9a34f2d..04997a77 100644 --- a/flatland_plugins/test/laser_test.cpp +++ b/flatland_plugins/test/laser_test.cpp @@ -58,24 +58,27 @@ using namespace flatland_server; using namespace flatland_plugins; using std::placeholders::_1; -class LaserPluginTest : public ::testing::Test { - public: +class LaserPluginTest : public ::testing::Test +{ +public: boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; sensor_msgs::msg::LaserScan scan_front, scan_center, scan_back; - World* w; + World * w; std::shared_ptr node; LaserPluginTest() : node(rclcpp::Node::make_shared("test_laser_plugin")) {} - void SetUp() override { + void SetUp() override + { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); w = nullptr; } void TearDown() override { delete w; } - static bool fltcmp(const double& n1, const double& n2) { + static bool fltcmp(const double & n1, const double & n2) + { if (std::isinf(n1) && std::isinf(n2)) { return true; } @@ -89,16 +92,18 @@ class LaserPluginTest : public ::testing::Test { } // print content of the vector for debugging - void print_flt_vec(const std::vector& v) { + void print_flt_vec(const std::vector & v) + { printf("{"); - for (const auto& e : v) { + for (const auto & e : v) { printf("%f,", e); } printf("}"); } // check the float values equals and print message for debugging - bool FloatEq(const char* name, float actual, float expected) { + bool FloatEq(const char * name, float actual, float expected) + { if (actual != expected) { printf("%s Actual:%f != Expected %f", name, actual, expected); return false; @@ -107,30 +112,27 @@ class LaserPluginTest : public ::testing::Test { } // check the received scan data is as expected - bool ScanEq(const sensor_msgs::msg::LaserScan& scan, - const std::string& frame_id, float angle_min, float angle_max, - float angle_increment, float time_increment, float scan_time, - float range_min, float range_max, std::vector ranges, - std::vector intensities) { + bool ScanEq( + const sensor_msgs::msg::LaserScan & scan, const std::string & frame_id, float angle_min, + float angle_max, float angle_increment, float time_increment, float scan_time, float range_min, + float range_max, std::vector ranges, std::vector intensities) + { if (scan.header.frame_id != frame_id) { - printf("frame_id Actual:%s != Expected:%s\n", - scan.header.frame_id.c_str(), frame_id.c_str()); + printf("frame_id Actual:%s != Expected:%s\n", scan.header.frame_id.c_str(), frame_id.c_str()); return false; } if (!FloatEq("angle_min", scan.angle_min, angle_min)) return false; if (!FloatEq("angle_max", scan.angle_max, angle_max)) return false; - if (!FloatEq("angle_increment", scan.angle_increment, angle_increment)) - return false; - if (!FloatEq("time_increment", scan.time_increment, time_increment)) - return false; + if (!FloatEq("angle_increment", scan.angle_increment, angle_increment)) return false; + if (!FloatEq("time_increment", scan.time_increment, time_increment)) return false; if (!FloatEq("scan_time", scan.scan_time, scan_time)) return false; if (!FloatEq("range_min", scan.range_min, range_min)) return false; if (!FloatEq("range_max", scan.range_max, range_max)) return false; - if (ranges.size() != scan.ranges.size() || - !std::equal(ranges.begin(), ranges.end(), scan.ranges.begin(), - fltcmp)) { + if ( + ranges.size() != scan.ranges.size() || + !std::equal(ranges.begin(), ranges.end(), scan.ranges.begin(), fltcmp)) { printf("\"ranges\" does not match\n"); printf("Actual: "); print_flt_vec(scan.ranges); @@ -141,9 +143,9 @@ class LaserPluginTest : public ::testing::Test { return false; } - if (intensities.size() != scan.intensities.size() || - !std::equal(intensities.begin(), intensities.end(), - scan.intensities.begin(), fltcmp)) { + if ( + intensities.size() != scan.intensities.size() || + !std::equal(intensities.begin(), intensities.end(), scan.intensities.begin(), fltcmp)) { printf("\"intensities\" does not math"); printf("Actual: "); print_flt_vec(scan.intensities); @@ -157,19 +159,16 @@ class LaserPluginTest : public ::testing::Test { return true; } - void ScanFrontCb(const sensor_msgs::msg::LaserScan& msg) { - scan_front = msg; - }; - void ScanCenterCb(const sensor_msgs::msg::LaserScan& msg) { - scan_center = msg; - }; - void ScanBackCb(const sensor_msgs::msg::LaserScan& msg) { scan_back = msg; }; + void ScanFrontCb(const sensor_msgs::msg::LaserScan & msg) { scan_front = msg; }; + void ScanCenterCb(const sensor_msgs::msg::LaserScan & msg) { scan_center = msg; }; + void ScanBackCb(const sensor_msgs::msg::LaserScan & msg) { scan_back = msg; }; }; /** * Test the laser plugin for a given model and plugin configuration */ -TEST_F(LaserPluginTest, range_test) { +TEST_F(LaserPluginTest, range_test) +{ world_yaml = this_file_dir / fs::path("laser_tests/range_test/world.yaml"); Timekeeper timekeeper(node); @@ -177,17 +176,17 @@ TEST_F(LaserPluginTest, range_test) { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); - auto* obj = dynamic_cast(this); + auto * obj = dynamic_cast(this); auto sub_1 = node->create_subscription( - "scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); + "scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); auto sub_2 = node->create_subscription( - "scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); + "scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); auto sub_3 = node->create_subscription( - "scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); + "scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); - auto* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - auto* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); - auto* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); + auto * p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + auto * p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); + auto * p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); // let it spin for 10 times to make sure the message gets through rclcpp::WallRate rate(500); @@ -198,45 +197,48 @@ TEST_F(LaserPluginTest, range_test) { } // check scan returns - EXPECT_TRUE(ScanEq(scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2, - 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, {})); + EXPECT_TRUE(ScanEq( + scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2, 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, + {})); EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits::infinity())) - << "Actual: " << p1->update_rate_; + << "Actual: " << p1->update_rate_; EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]); - EXPECT_TRUE(ScanEq(scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0, - 0.0, 0.0, 5.0, {4.8, 4.7, 4.6, 4.9, 4.8}, {})); + EXPECT_TRUE(ScanEq( + scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, 0.0, 5.0, + {4.8, 4.7, 4.6, 4.9, 4.8}, {})); EXPECT_TRUE(fltcmp(p2->update_rate_, 5000)) << "Actual: " << p2->update_rate_; EXPECT_EQ(p2->body_, w->models_[0]->bodies_[0]); - EXPECT_TRUE(ScanEq(scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, - 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {})); + EXPECT_TRUE(ScanEq( + scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, + {})); EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p3->update_rate_; EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); } /** * Test the laser plugin for intensity configuration */ -TEST_F(LaserPluginTest, intensity_test) { - world_yaml = - this_file_dir / fs::path("laser_tests/intensity_test/world.yaml"); +TEST_F(LaserPluginTest, intensity_test) +{ + world_yaml = this_file_dir / fs::path("laser_tests/intensity_test/world.yaml"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); - auto* obj = dynamic_cast(this); + auto * obj = dynamic_cast(this); auto sub_1 = node->create_subscription( - "scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); + "scan", 1, std::bind(&LaserPluginTest::ScanFrontCb, obj, _1)); auto sub_2 = node->create_subscription( - "scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); + "scan_center", 1, std::bind(&LaserPluginTest::ScanCenterCb, obj, _1)); auto sub_3 = node->create_subscription( - "scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); + "scan_back", 1, std::bind(&LaserPluginTest::ScanBackCb, obj, _1)); - auto* p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - auto* p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); - auto* p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); + auto * p1 = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + auto * p2 = dynamic_cast(w->plugin_manager_.model_plugins_[1].get()); + auto * p3 = dynamic_cast(w->plugin_manager_.model_plugins_[2].get()); // let it spin for 10 times to make sure the message gets through rclcpp::WallRate rate(500); @@ -247,18 +249,20 @@ TEST_F(LaserPluginTest, intensity_test) { } // check scan returns - EXPECT_TRUE(ScanEq(scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2, - 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, {0, 0, 0})); + EXPECT_TRUE(ScanEq( + scan_front, "r_laser_front", -M_PI / 2, M_PI / 2, M_PI / 2, 0.0, 0.0, 0.0, 5.0, {4.5, 4.4, 4.3}, + {0, 0, 0})); EXPECT_TRUE(fltcmp(p1->update_rate_, std::numeric_limits::infinity())) - << "Actual: " << p1->update_rate_; + << "Actual: " << p1->update_rate_; EXPECT_EQ(p1->body_, w->models_[0]->bodies_[0]); - EXPECT_TRUE(ScanEq(scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0, - 0.0, 0.0, 5.0, {4.8, 4.7, 4.6, 4.9, 4.8}, - {0, 255, 0, 0, 0})); + EXPECT_TRUE(ScanEq( + scan_center, "r_center_laser", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, 0.0, 5.0, + {4.8, 4.7, 4.6, 4.9, 4.8}, {0, 255, 0, 0, 0})); EXPECT_TRUE(fltcmp(p2->update_rate_, 5000)) << "Actual: " << p2->update_rate_; EXPECT_EQ(p2->body_, w->models_[0]->bodies_[0]); - EXPECT_TRUE(ScanEq(scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, - 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, {0, 0, 0, 0, 0})); + EXPECT_TRUE(ScanEq( + scan_back, "r_laser_back", 0, 2 * M_PI, M_PI / 2, 0.0, 0.0, 0.0, 4, {NAN, 3.2, 3.5, NAN, NAN}, + {0, 0, 0, 0, 0})); EXPECT_TRUE(fltcmp(p3->update_rate_, 1)) << "Actual: " << p3->update_rate_; EXPECT_EQ(p3->body_, w->models_[0]->bodies_[0]); } @@ -267,7 +271,8 @@ TEST_F(LaserPluginTest, intensity_test) { * Checks the laser plugin will throw correct exception for invalid * configurations */ -TEST_F(LaserPluginTest, invalid_A) { +TEST_F(LaserPluginTest, invalid_A) +{ world_yaml = this_file_dir / fs::path("laser_tests/invalid_A/world.yaml"); try { @@ -275,14 +280,14 @@ TEST_F(LaserPluginTest, invalid_A) { w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { + } catch (const PluginException & e) { std::cmatch match; std::string regex_str = ".*Flatland YAML: Entry \"range\" does not exist.*"; std::regex regex(regex_str); EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { + << "Exception Message '" + std::string(e.what()) + "'" + " did not match against regex '" + + regex_str + "'"; + } catch (const std::exception & e) { ADD_FAILURE() << "Was expecting a PluginException, another exception was " "caught instead: " << e.what(); @@ -293,7 +298,8 @@ TEST_F(LaserPluginTest, invalid_A) { * Checks the laser plugin will throw correct exception for invalid * configurations */ -TEST_F(LaserPluginTest, invalid_B) { +TEST_F(LaserPluginTest, invalid_B) +{ world_yaml = this_file_dir / fs::path("laser_tests/invalid_B/world.yaml"); try { @@ -301,14 +307,14 @@ TEST_F(LaserPluginTest, invalid_B) { w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { + } catch (const PluginException & e) { std::cmatch match; std::string regex_str = ".*Invalid \"angle\" params, must have max > min.*"; std::regex regex(regex_str); EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { + << "Exception Message '" + std::string(e.what()) + "'" + " did not match against regex '" + + regex_str + "'"; + } catch (const std::exception & e) { ADD_FAILURE() << "Was expecting a PluginException, another exception was " "caught instead: " << e.what(); @@ -316,7 +322,8 @@ TEST_F(LaserPluginTest, invalid_B) { } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_plugins/test/model_tf_publisher_test.cpp b/flatland_plugins/test/model_tf_publisher_test.cpp index 74dbda7a..b4271e18 100644 --- a/flatland_plugins/test/model_tf_publisher_test.cpp +++ b/flatland_plugins/test/model_tf_publisher_test.cpp @@ -61,20 +61,23 @@ namespace fs = boost::filesystem; using namespace flatland_server; using namespace flatland_plugins; -class ModelTfPublisherTest : public ::testing::Test { - public: +class ModelTfPublisherTest : public ::testing::Test +{ +public: boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; - World* w; + World * w; - void SetUp() override { + void SetUp() override + { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); w = nullptr; } void TearDown() override { delete w; } - static bool fltcmp(const double& n1, const double& n2) { + static bool fltcmp(const double & n1, const double & n2) + { if (std::isinf(n1) && std::isinf(n2)) { return true; } @@ -88,24 +91,23 @@ class ModelTfPublisherTest : public ::testing::Test { } // Test if transform equals to expected - bool TfEq(const geometry_msgs::msg::TransformStamped& tf, float x, float y, - float a) { + bool TfEq(const geometry_msgs::msg::TransformStamped & tf, float x, float y, float a) + { tf2::Quaternion q; tf2::fromMsg(tf.transform.rotation, q); tf2::Matrix3x3 rot_matrix(q); double roll, pitch, yaw; rot_matrix.getRPY(roll, pitch, yaw); - if (!fltcmp(x, tf.transform.translation.x) || - !fltcmp(y, tf.transform.translation.y) || - !fltcmp(0, tf.transform.translation.z) || !fltcmp(roll, 0) || - !fltcmp(pitch, 0) || !fltcmp(yaw, a)) { + if ( + !fltcmp(x, tf.transform.translation.x) || !fltcmp(y, tf.transform.translation.y) || + !fltcmp(0, tf.transform.translation.z) || !fltcmp(roll, 0) || !fltcmp(pitch, 0) || + !fltcmp(yaw, a)) { printf("Transformation\n"); - printf("Actual: x=%f y=%f z=%f, roll=%f pitch=%f yaw=%f\n", - tf.transform.translation.x, tf.transform.translation.y, - tf.transform.translation.z, roll, pitch, yaw); - printf("Expected: x=%f y=%f z=%f, roll=%f pitch=%f yaw=%f\n", x, y, 0.0, - 0.0, 0.0, a); + printf( + "Actual: x=%f y=%f z=%f, roll=%f pitch=%f yaw=%f\n", tf.transform.translation.x, + tf.transform.translation.y, tf.transform.translation.z, roll, pitch, yaw); + printf("Expected: x=%f y=%f z=%f, roll=%f pitch=%f yaw=%f\n", x, y, 0.0, 0.0, 0.0, a); return false; } @@ -116,18 +118,16 @@ class ModelTfPublisherTest : public ::testing::Test { /** * Test the transformation for the model robot in a given plugin configuration */ -TEST_F(ModelTfPublisherTest, tf_publish_test_A) { - world_yaml = - this_file_dir / - fs::path("model_tf_publisher_tests/tf_publish_test_A/world.yaml"); +TEST_F(ModelTfPublisherTest, tf_publish_test_A) +{ + world_yaml = this_file_dir / fs::path("model_tf_publisher_tests/tf_publish_test_A/world.yaml"); std::shared_ptr node = - rclcpp::Node::make_shared("test_tf_publisher_tf_publish_test_A"); + rclcpp::Node::make_shared("test_tf_publisher_tf_publish_test_A"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); w = World::MakeWorld(node, world_yaml.string()); - auto* p = dynamic_cast( - w->plugin_manager_.model_plugins_[0].get()); + auto * p = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); EXPECT_DOUBLE_EQ(5000.0, p->update_rate_); EXPECT_STREQ("antenna", p->reference_body_->name_.c_str()); @@ -150,36 +150,34 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_A) { } // check for the transformations that should exist - tf_world_to_base = - tf_buffer.lookupTransform("world", "my_robot_base", rclcpp::Time(0)); - tf_world_to_antenna = - tf_buffer.lookupTransform("world", "my_robot_antenna", rclcpp::Time(0)); - tf_base_to_left_wheel = tf_buffer.lookupTransform( - "my_robot_base", "my_robot_left_wheel", rclcpp::Time(0)); - tf_base_to_right_wheel = tf_buffer.lookupTransform( - "my_robot_base", "my_robot_right_wheel", rclcpp::Time(0)); + tf_world_to_base = tf_buffer.lookupTransform("world", "my_robot_base", rclcpp::Time(0)); + tf_world_to_antenna = tf_buffer.lookupTransform("world", "my_robot_antenna", rclcpp::Time(0)); + tf_base_to_left_wheel = + tf_buffer.lookupTransform("my_robot_base", "my_robot_left_wheel", rclcpp::Time(0)); + tf_base_to_right_wheel = + tf_buffer.lookupTransform("my_robot_base", "my_robot_right_wheel", rclcpp::Time(0)); // check for the transformations that should not exist try { - tf_base_to_front_bumper = tf_buffer.lookupTransform( - "my_robot_base", "my_robot_front_bumper", rclcpp::Time(0)); + tf_base_to_front_bumper = + tf_buffer.lookupTransform("my_robot_base", "my_robot_front_bumper", rclcpp::Time(0)); ADD_FAILURE() << "Expected an exception, but none were raised"; - } catch (const tf2::TransformException& e) { + } catch (const tf2::TransformException & e) { EXPECT_STREQ( - "\"my_robot_front_bumper\" passed to lookupTransform argument " - "source_frame does not exist. ", - e.what()); + "\"my_robot_front_bumper\" passed to lookupTransform argument " + "source_frame does not exist. ", + e.what()); } try { - tf_base_to_rear_bumper = tf_buffer.lookupTransform( - "my_robot_base", "my_robot_rear_bumper", rclcpp::Time(0)); + tf_base_to_rear_bumper = + tf_buffer.lookupTransform("my_robot_base", "my_robot_rear_bumper", rclcpp::Time(0)); ADD_FAILURE() << "Expected an exception, but none were raised"; - } catch (const tf2::TransformException& e) { + } catch (const tf2::TransformException & e) { EXPECT_STREQ( - "\"my_robot_rear_bumper\" passed to lookupTransform argument " - "source_frame does not exist. ", - e.what()); + "\"my_robot_rear_bumper\" passed to lookupTransform argument " + "source_frame does not exist. ", + e.what()); } // check transformations are correct @@ -192,18 +190,17 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_A) { /** * Test the transformation for the model robot in another plugin configuration */ -TEST_F(ModelTfPublisherTest, tf_publish_test_B) { - world_yaml = - this_file_dir / - fs::path("model_tf_publisher_tests/tf_publish_test_B/world.yaml"); +TEST_F(ModelTfPublisherTest, tf_publish_test_B) +{ + world_yaml = this_file_dir / fs::path("model_tf_publisher_tests/tf_publish_test_B/world.yaml"); std::shared_ptr node = - rclcpp::Node::make_shared("test_tf_publisher_tf_publish_test_B"); + rclcpp::Node::make_shared("test_tf_publisher_tf_publish_test_B"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); w = World::MakeWorld(node, world_yaml.string()); - ModelTfPublisher* p = dynamic_cast( - w->plugin_manager_.model_plugins_[0].get()); + ModelTfPublisher * p = + dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); EXPECT_DOUBLE_EQ(std::numeric_limits::infinity(), p->update_rate_); EXPECT_STREQ("base", p->reference_body_->name_.c_str()); @@ -225,25 +222,20 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_B) { rate.sleep(); } - tf_base_to_antenna = - tf_buffer.lookupTransform("base", "antenna", rclcpp::Time(0)); - tf_base_to_left_wheel = - tf_buffer.lookupTransform("base", "left_wheel", rclcpp::Time(0)); - tf_base_to_right_wheel = - tf_buffer.lookupTransform("base", "right_wheel", rclcpp::Time(0)); - tf_base_to_front_bumper = - tf_buffer.lookupTransform("base", "front_bumper", rclcpp::Time(0)); - tf_base_to_rear_bumper = - tf_buffer.lookupTransform("base", "rear_bumper", rclcpp::Time(0)); + tf_base_to_antenna = tf_buffer.lookupTransform("base", "antenna", rclcpp::Time(0)); + tf_base_to_left_wheel = tf_buffer.lookupTransform("base", "left_wheel", rclcpp::Time(0)); + tf_base_to_right_wheel = tf_buffer.lookupTransform("base", "right_wheel", rclcpp::Time(0)); + tf_base_to_front_bumper = tf_buffer.lookupTransform("base", "front_bumper", rclcpp::Time(0)); + tf_base_to_rear_bumper = tf_buffer.lookupTransform("base", "rear_bumper", rclcpp::Time(0)); try { tf_map_to_base = tf_buffer.lookupTransform("map", "base", rclcpp::Time(0)); ADD_FAILURE() << "Expected an exception, but none were raised"; - } catch (const tf2::TransformException& e) { + } catch (const tf2::TransformException & e) { EXPECT_STREQ( - "\"map\" passed to lookupTransform argument target_frame does not " - "exist. ", - e.what()); + "\"map\" passed to lookupTransform argument target_frame does not " + "exist. ", + e.what()); } EXPECT_TRUE(TfEq(tf_base_to_antenna, 0, 0, 0)); @@ -257,23 +249,23 @@ TEST_F(ModelTfPublisherTest, tf_publish_test_B) { * Test the transformation for the provided model yaml, which will fail due * to a nonexistent reference body */ -TEST_F(ModelTfPublisherTest, invalid_A) { - world_yaml = - this_file_dir / fs::path("model_tf_publisher_tests/invalid_A/world.yaml"); +TEST_F(ModelTfPublisherTest, invalid_A) +{ + world_yaml = this_file_dir / fs::path("model_tf_publisher_tests/invalid_A/world.yaml"); try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { + } catch (const PluginException & e) { std::cmatch match; std::string regex_str = ".*Body with name \"random_body\" does not exist.*"; std::regex regex(regex_str); EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { + << "Exception Message '" + std::string(e.what()) + "'" + " did not match against regex '" + + regex_str + "'"; + } catch (const std::exception & e) { ADD_FAILURE() << "Was expecting a PluginException, another exception was " "caught instead: " << e.what(); @@ -284,24 +276,23 @@ TEST_F(ModelTfPublisherTest, invalid_A) { * Test the transformation for the provided model yaml, which will fail due * to a nonexistent body specified in the exclude list */ -TEST_F(ModelTfPublisherTest, invalid_B) { - world_yaml = - this_file_dir / fs::path("model_tf_publisher_tests/invalid_B/world.yaml"); +TEST_F(ModelTfPublisherTest, invalid_B) +{ + world_yaml = this_file_dir / fs::path("model_tf_publisher_tests/invalid_B/world.yaml"); try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException& e) { + } catch (const PluginException & e) { std::cmatch match; - std::string regex_str = - ".*Body with name \"random_body_1\" does not exist.*"; + std::string regex_str = ".*Body with name \"random_body_1\" does not exist.*"; std::regex regex(regex_str); EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception& e) { + << "Exception Message '" + std::string(e.what()) + "'" + " did not match against regex '" + + regex_str + "'"; + } catch (const std::exception & e) { ADD_FAILURE() << "Was expecting a PluginException, another exception was " "caught instead: " << e.what(); @@ -309,7 +300,8 @@ TEST_F(ModelTfPublisherTest, invalid_B) { } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_plugins/test/tricycle_drive_test.cpp b/flatland_plugins/test/tricycle_drive_test.cpp index 43f47d23..0019bb40 100644 --- a/flatland_plugins/test/tricycle_drive_test.cpp +++ b/flatland_plugins/test/tricycle_drive_test.cpp @@ -50,22 +50,23 @@ #include #include -TEST(TricycleDrivePluginTest, load_test) { - std::shared_ptr node = - rclcpp::Node::make_shared("test_tricycle_drive_plugin"); +TEST(TricycleDrivePluginTest, load_test) +{ + std::shared_ptr node = rclcpp::Node::make_shared("test_tricycle_drive_plugin"); pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::ModelPlugin"); + "flatland_server", "flatland_server::ModelPlugin"); try { std::shared_ptr plugin = - loader.createSharedInstance("flatland_plugins::TricycleDrive"); - } catch (pluginlib::PluginlibException& e) { + loader.createSharedInstance("flatland_plugins::TricycleDrive"); + } catch (pluginlib::PluginlibException & e) { FAIL() << "Failed to load Tricycle Drive plugin. " << e.what(); } } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_plugins/test/tween_test.cpp b/flatland_plugins/test/tween_test.cpp index 111d8bdd..5b6828ce 100644 --- a/flatland_plugins/test/tween_test.cpp +++ b/flatland_plugins/test/tween_test.cpp @@ -56,16 +56,16 @@ namespace fs = boost::filesystem; using namespace flatland_server; using namespace flatland_plugins; -class TweenPluginTest : public ::testing::Test { - public: +class TweenPluginTest : public ::testing::Test +{ +public: boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; - void SetUp() override { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - } + void SetUp() override { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); } - static bool fltcmp(const float& n1, const float& n2, float epsilon = 1e-5) { + static bool fltcmp(const float & n1, const float & n2, float epsilon = 1e-5) + { if (std::isinf(n1) && std::isinf(n2)) { return true; } @@ -82,19 +82,18 @@ class TweenPluginTest : public ::testing::Test { /** * Test the tween plugin handles oneshot */ -TEST_F(TweenPluginTest, once_test) { +TEST_F(TweenPluginTest, once_test) +{ world_yaml = this_file_dir / fs::path("tween_tests/once.world.yaml"); - std::shared_ptr node = - rclcpp::Node::make_shared("test_tween_once_test"); + std::shared_ptr node = rclcpp::Node::make_shared("test_tween_once_test"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(node, world_yaml.string()); + World * w = World::MakeWorld(node, world_yaml.string()); - Tween* tween = - dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + Tween * tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - Body* b = tween->body_; + Body * b = tween->body_; ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 2.0)); ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 1.0)); @@ -118,19 +117,18 @@ TEST_F(TweenPluginTest, once_test) { /** * Test that the tween plugin yoyos */ -TEST_F(TweenPluginTest, yoyo_test) { +TEST_F(TweenPluginTest, yoyo_test) +{ world_yaml = this_file_dir / fs::path("tween_tests/yoyo.world.yaml"); - std::shared_ptr node = - rclcpp::Node::make_shared("test_tween_yoyo_test"); + std::shared_ptr node = rclcpp::Node::make_shared("test_tween_yoyo_test"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(node, world_yaml.string()); + World * w = World::MakeWorld(node, world_yaml.string()); - Tween* tween = - dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + Tween * tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - Body* b = tween->body_; + Body * b = tween->body_; ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 0.0)); ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 0.0)); @@ -163,19 +161,18 @@ TEST_F(TweenPluginTest, yoyo_test) { /** * Test that the tween plugin loops */ -TEST_F(TweenPluginTest, loop_test) { +TEST_F(TweenPluginTest, loop_test) +{ world_yaml = this_file_dir / fs::path("tween_tests/loop.world.yaml"); - std::shared_ptr node = - rclcpp::Node::make_shared("test_tween_loop_test"); + std::shared_ptr node = rclcpp::Node::make_shared("test_tween_loop_test"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.5); - World* w = World::MakeWorld(node, world_yaml.string()); + World * w = World::MakeWorld(node, world_yaml.string()); - Tween* tween = - dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); + Tween * tween = dynamic_cast(w->plugin_manager_.model_plugins_[0].get()); - Body* b = tween->body_; + Body * b = tween->body_; ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().x, 0.0)); ASSERT_TRUE(fltcmp(b->physics_body_->GetPosition().y, 0.0)); @@ -207,7 +204,8 @@ TEST_F(TweenPluginTest, loop_test) { } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_plugins/test/update_timer_test.cpp b/flatland_plugins/test/update_timer_test.cpp index b7e98be9..84bd38f5 100644 --- a/flatland_plugins/test/update_timer_test.cpp +++ b/flatland_plugins/test/update_timer_test.cpp @@ -57,17 +57,20 @@ using namespace flatland_server; using namespace flatland_plugins; using namespace std::chrono_literals; -class TestPlugin : public ModelPlugin { - public: +class TestPlugin : public ModelPlugin +{ +public: UpdateTimer update_timer_; int update_counter_; - void OnInitialize(const YAML::Node &config) override { + void OnInitialize(const YAML::Node & config) override + { update_timer_.SetRate(0); update_counter_ = 0; } - void BeforePhysicsStep(const Timekeeper &timekeeper) override { + void BeforePhysicsStep(const Timekeeper & timekeeper) override + { // keeps this function updating at a specific rate if (!update_timer_.CheckUpdate(timekeeper)) { return; @@ -76,8 +79,9 @@ class TestPlugin : public ModelPlugin { } }; -class UpdateTimerTest : public ::testing::Test { - public: +class UpdateTimerTest : public ::testing::Test +{ +public: boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; double set_rate; @@ -86,26 +90,27 @@ class UpdateTimerTest : public ::testing::Test { double wall_rate; double step_size; int64_t sim_test_time; - World *w; + World * w; std::shared_ptr node; UpdateTimerTest() : node(rclcpp::Node::make_shared("test_update_timer")) {} - void SetUp() override { + void SetUp() override + { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); w = nullptr; } void TearDown() override { delete w; } - void ExecuteRateTest() { + void ExecuteRateTest() + { Timekeeper timekeeper(node); w = World::MakeWorld(node, world_yaml.string()); // artificially load a plugin std::shared_ptr p(new TestPlugin()); - p->Initialize(node, "TestPlugin", "test_plugin", w->models_[0], - YAML::Node()); + p->Initialize(node, "TestPlugin", "test_plugin", w->models_[0], YAML::Node()); w->plugin_manager_.model_plugins_.push_back(p); p->update_timer_.SetRate(set_rate); @@ -130,7 +135,8 @@ class UpdateTimerTest : public ::testing::Test { /** * Test update rate at real time factor > 1 */ -TEST_F(UpdateTimerTest, rate_test_A) { +TEST_F(UpdateTimerTest, rate_test_A) +{ world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); set_rate = 141.56; expected_rate = set_rate; @@ -148,7 +154,8 @@ TEST_F(UpdateTimerTest, rate_test_A) { /** * Test update rate at real time factor < 1 */ -TEST_F(UpdateTimerTest, rate_test_B) { +TEST_F(UpdateTimerTest, rate_test_B) +{ world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); set_rate = 564.56; expected_rate = set_rate; @@ -166,7 +173,8 @@ TEST_F(UpdateTimerTest, rate_test_B) { /** * Test update rate at real time factor >> 1 */ -TEST_F(UpdateTimerTest, rate_test_C) { +TEST_F(UpdateTimerTest, rate_test_C) +{ world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); set_rate = 47.4; expected_rate = set_rate; @@ -184,7 +192,8 @@ TEST_F(UpdateTimerTest, rate_test_C) { /** * Test update rate at update rate = inf, which will update as fast as possible */ -TEST_F(UpdateTimerTest, rate_test_D) { +TEST_F(UpdateTimerTest, rate_test_D) +{ world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); set_rate = std::numeric_limits::infinity(); expected_rate = 100.0; @@ -202,7 +211,8 @@ TEST_F(UpdateTimerTest, rate_test_D) { /** * Test update rate at update rate = 0, which will never update */ -TEST_F(UpdateTimerTest, rate_test_E) { +TEST_F(UpdateTimerTest, rate_test_E) +{ world_yaml = this_file_dir / fs::path("update_timer_test/world.yaml"); set_rate = 0; expected_rate = 0; @@ -218,7 +228,8 @@ TEST_F(UpdateTimerTest, rate_test_E) { } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp index ba867cb0..570abfba 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_tool.hpp @@ -1,14 +1,19 @@ /* - * @copyright Copyright 2022 FEUP - * @name change_rate_tool.hpp - * @brief changes simulation rate - * @author Ana Barros - * @author Henrique Ribeiro - * @author João Costa + * ______ __ __ __ + * /\ _ \ __ /\ \/\ \ /\ \__ + * \ \ \L\ \ __ __ /\_\ \_\ \ \ \____ ___\ \ ,_\ ____ + * \ \ __ \/\ \/\ \\/\ \ /'_` \ \ '__`\ / __`\ \ \/ /',__\ + * \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\ + * \ \_\ \_\ \___/ \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/ + * \/_/\/_/\/__/ \/_/\/__,_ /\/___/ \/___/ \/__/\/___/ + * @copyright Copyright 2017 Avidbots Corp. + * @name joint.h + * @brief Defines Joint + * @author Chunshang Li * * Software License Agreement (BSD License) * - * Copyright (c) 2022, FEUP + * Copyright (c) 2017, Avidbots Corp. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -39,7 +44,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ #define FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_TOOL_HPP_ @@ -52,12 +56,14 @@ #include "rviz_default_plugins/visibility_control.hpp" -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ -class RVIZ_DEFAULT_PLUGINS_PUBLIC ChangeRateTool : public rviz_common::Tool { +class RVIZ_DEFAULT_PLUGINS_PUBLIC ChangeRateTool : public rviz_common::Tool +{ Q_OBJECT - public: +public: ChangeRateTool(); ~ChangeRateTool() override; @@ -68,10 +74,9 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC ChangeRateTool : public rviz_common::Tool { void setRate(double rate); - private: +private: std::shared_ptr node_; - rclcpp::Client::SharedPtr - change_rate_service_; + rclcpp::Client::SharedPtr change_rate_service_; }; } // namespace flatland_rviz_plugins diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp index d4a5e58e..ca28a818 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/interactive_tool.hpp @@ -46,10 +46,12 @@ #include #include -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ class RVIZ_DEFAULT_PLUGINS_PUBLIC InteractiveTool - : public rviz_default_plugins::tools::InteractionTool { - public: + : public rviz_default_plugins::tools::InteractionTool +{ +public: InteractiveTool(); void onInitialize() override; @@ -57,7 +59,7 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC InteractiveTool void activate() override; void deactivate() override; - private: +private: void enableMarkers(bool is_enabled); }; } // namespace flatland_rviz_plugins diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp index 1dbf0bee..22a41d63 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/pause_tool.hpp @@ -50,12 +50,14 @@ #include "rviz_default_plugins/visibility_control.hpp" -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ -class RVIZ_DEFAULT_PLUGINS_PUBLIC PauseTool : public rviz_common::Tool { +class RVIZ_DEFAULT_PLUGINS_PUBLIC PauseTool : public rviz_common::Tool +{ Q_OBJECT - public: +public: PauseTool(); ~PauseTool() override; @@ -64,7 +66,7 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC PauseTool : public rviz_common::Tool { void activate() override; void deactivate() override; - private: +private: std::shared_ptr node_; rclcpp::Client::SharedPtr pause_service_; }; diff --git a/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp index 371e002d..48c8b188 100644 --- a/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/spawn_model_tool.hpp @@ -62,16 +62,18 @@ #include #include -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ /** * @name SpawnModelTool * @brief Every tool which can be added to the tool bar is a * subclass of rviz_common::Tool. */ -class SpawnModelTool : public rviz_common::Tool { +class SpawnModelTool : public rviz_common::Tool +{ Q_OBJECT - public: +public: SpawnModelTool(); ~SpawnModelTool() override; @@ -97,7 +99,7 @@ class SpawnModelTool : public rviz_common::Tool { */ void SpawnModelInFlatland(); - private: +private: /** * @name onInitialize * @brief Initializes tools currently loaded when rviz starts @@ -119,7 +121,7 @@ class SpawnModelTool : public rviz_common::Tool { * @brief Main loop of tool * @param event Mouse event */ - int processMouseEvent(rviz_common::ViewportMouseEvent &event) override; + int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; /** * @name SetMovingModelColor * @brief Set the color of the moving model @@ -136,30 +138,30 @@ class SpawnModelTool : public rviz_common::Tool { * @brief Load a vector preview of the model's polygon footprint * @param footprint The footprint yaml node */ - void LoadPolygonFootprint(flatland_server::YamlReader &footprint); + void LoadPolygonFootprint(flatland_server::YamlReader & footprint); /** * @name LoadCircleFootprint * @brief Load a vector preview of the model's circle footprint * @param footprint The footprint yaml node */ - void LoadCircleFootprint(flatland_server::YamlReader &footprint); + void LoadCircleFootprint(flatland_server::YamlReader & footprint); - Ogre::Vector3 intersection; // location cursor intersects ground plane, ie - // the location to create the model - float initial_angle; // the angle to create the model at - Ogre::SceneNode *moving_model_node_; // the node for the 3D object + Ogre::Vector3 intersection; // location cursor intersects ground plane, ie + // the location to create the model + float initial_angle; // the angle to create the model at + Ogre::SceneNode * moving_model_node_; // the node for the 3D object enum ModelState { m_hidden, m_dragging, m_rotating }; - ModelState model_state; // model state, first hidden, then dragging to - // intersection point, then rotating to desired angle + ModelState model_state; // model state, first hidden, then dragging to + // intersection point, then rotating to desired angle static QString path_to_model_file_; // full path to model file (yaml) - static QString model_name; // base file name with path and extension removed + static QString model_name; // base file name with path and extension removed - protected: - rviz_rendering::Arrow *arrow_; // Rviz 3d arrow to show axis of rotation +protected: + rviz_rendering::Arrow * arrow_; // Rviz 3d arrow to show axis of rotation rclcpp::Client::SharedPtr client; std::vector> lines_list_; }; } // end namespace flatland_rviz_plugins -#endif // FLATLAND_RVIZ_PLUGINS__SPAWN_MODEL_TOOL_HPP_ \ No newline at end of file +#endif // FLATLAND_RVIZ_PLUGINS__SPAWN_MODEL_TOOL_HPP_ diff --git a/flatland_rviz_plugins/src/change_rate_dialog.cpp b/flatland_rviz_plugins/src/change_rate_dialog.cpp index 10a2ff18..f0faff24 100644 --- a/flatland_rviz_plugins/src/change_rate_dialog.cpp +++ b/flatland_rviz_plugins/src/change_rate_dialog.cpp @@ -43,34 +43,33 @@ #include -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ -ChangeRateDialog::ChangeRateDialog(QWidget *parent, - rviz_common::DisplayContext *context, - ChangeRateTool *tool) - : QDialog(parent), context_(context), tool_(tool) { +ChangeRateDialog::ChangeRateDialog( + QWidget * parent, rviz_common::DisplayContext * context, ChangeRateTool * tool) +: QDialog(parent), context_(context), tool_(tool) +{ r_edit = new QLineEdit; - auto *okButton = new QPushButton("ok"); - auto *cancelButton = new QPushButton("cancel"); + auto * okButton = new QPushButton("ok"); + auto * cancelButton = new QPushButton("cancel"); r_edit->setFocusPolicy(Qt::ClickFocus); // only name gets focus okButton->setFocusPolicy(Qt::NoFocus); cancelButton->setFocusPolicy(Qt::NoFocus); - connect(okButton, &QAbstractButton::clicked, this, - &ChangeRateDialog::OkButtonClicked); - connect(cancelButton, &QAbstractButton::clicked, this, - &ChangeRateDialog::CancelButtonClicked); + connect(okButton, &QAbstractButton::clicked, this, &ChangeRateDialog::OkButtonClicked); + connect(cancelButton, &QAbstractButton::clicked, this, &ChangeRateDialog::CancelButtonClicked); - auto *h0_layout = new QHBoxLayout; + auto * h0_layout = new QHBoxLayout; h0_layout->addWidget(new QLabel("rate:")); h0_layout->addWidget(r_edit); - auto *h1_layout = new QHBoxLayout; + auto * h1_layout = new QHBoxLayout; h1_layout->addWidget(okButton); h1_layout->addWidget(cancelButton); - auto *v_layout = new QVBoxLayout; + auto * v_layout = new QVBoxLayout; v_layout->addLayout(h0_layout); v_layout->addLayout(h1_layout); @@ -80,9 +79,9 @@ ChangeRateDialog::ChangeRateDialog(QWidget *parent, this->setAttribute(Qt::WA_DeleteOnClose, true); } -void ChangeRateDialog::CancelButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), - "LoadModelDialog::CancelButtonClicked"); +void ChangeRateDialog::CancelButtonClicked() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::CancelButtonClicked"); auto tool_man = context_->getToolManager(); auto dflt_tool = tool_man->getDefaultTool(); @@ -91,9 +90,9 @@ void ChangeRateDialog::CancelButtonClicked() { this->close(); } -void ChangeRateDialog::OkButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), - "LoadModelDialog::OkButtonClicked"); +void ChangeRateDialog::OkButtonClicked() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::OkButtonClicked"); bool ok(false); double rate = r_edit->text().toDouble(&ok); if (ok) { @@ -106,8 +105,7 @@ void ChangeRateDialog::OkButtonClicked() { this->close(); } else { QMessageBox msgBox; - msgBox.setText( - "Error: The rate needs to be a valid double (float 64-bit) value."); + msgBox.setText("Error: The rate needs to be a valid double (float 64-bit) value."); msgBox.exec(); } } diff --git a/flatland_rviz_plugins/src/change_rate_dialog.hpp b/flatland_rviz_plugins/src/change_rate_dialog.hpp index 11438031..377747fe 100644 --- a/flatland_rviz_plugins/src/change_rate_dialog.hpp +++ b/flatland_rviz_plugins/src/change_rate_dialog.hpp @@ -57,21 +57,22 @@ #include "flatland_rviz_plugins/change_rate_tool.hpp" -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ -class ChangeRateDialog : public QDialog { - public: - ChangeRateDialog(QWidget *parent, rviz_common::DisplayContext *context, - ChangeRateTool *tool); +class ChangeRateDialog : public QDialog +{ +public: + ChangeRateDialog(QWidget * parent, rviz_common::DisplayContext * context, ChangeRateTool * tool); - private: - rviz_common::DisplayContext *context_; +private: + rviz_common::DisplayContext * context_; - QLineEdit *r_edit; // name lineEdit widget + QLineEdit * r_edit; // name lineEdit widget - ChangeRateTool *tool_; + ChangeRateTool * tool_; - public Q_SLOTS: +public Q_SLOTS: /** * @name CancelButtonClicked * @brief Cancel button was clicked, dismiss dialog diff --git a/flatland_rviz_plugins/src/change_rate_tool.cpp b/flatland_rviz_plugins/src/change_rate_tool.cpp index dfdb7e01..488b2ecd 100644 --- a/flatland_rviz_plugins/src/change_rate_tool.cpp +++ b/flatland_rviz_plugins/src/change_rate_tool.cpp @@ -49,31 +49,33 @@ #include "change_rate_dialog.hpp" -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ -ChangeRateTool::ChangeRateTool() { shortcut_key_ = 'R'; } +ChangeRateTool::ChangeRateTool() {shortcut_key_ = 'R';} ChangeRateTool::~ChangeRateTool() {} -void ChangeRateTool::onInitialize() { +void ChangeRateTool::onInitialize() +{ node_ = rclcpp::Node::make_shared("change_rate_tool"); - change_rate_service_ = - node_->create_client("change_rate"); + change_rate_service_ = node_->create_client("change_rate"); setName("Change Simulation Rate"); - setIcon(rviz_common::loadPixmap( - "package://flatland_rviz_plugins/icons/time.png")); + setIcon(rviz_common::loadPixmap("package://flatland_rviz_plugins/icons/time.png")); } -void ChangeRateTool::setRate(double rate) { +void ChangeRateTool::setRate(double rate) +{ auto request = std::make_shared(); request->rate = rate; auto result = change_rate_service_->async_send_request(request); rclcpp::spin_until_future_complete(node_, result); } -void ChangeRateTool::activate() { - auto *model_dialog = new ChangeRateDialog(nullptr, context_, this); +void ChangeRateTool::activate() +{ + auto * model_dialog = new ChangeRateDialog(nullptr, context_, this); model_dialog->setModal(true); model_dialog->show(); } diff --git a/flatland_rviz_plugins/src/interactive_tool.cpp b/flatland_rviz_plugins/src/interactive_tool.cpp index aa41a208..8f8ff460 100644 --- a/flatland_rviz_plugins/src/interactive_tool.cpp +++ b/flatland_rviz_plugins/src/interactive_tool.cpp @@ -39,7 +39,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #include "include/flatland_rviz_plugins/interactive_tool.hpp" #include @@ -47,12 +46,13 @@ #include #include -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ -InteractiveTool::InteractiveTool() - : rviz_default_plugins::tools::InteractionTool() {} +InteractiveTool::InteractiveTool() : rviz_default_plugins::tools::InteractionTool() {} -void InteractiveTool::enableMarkers(bool is_enabled) { +void InteractiveTool::enableMarkers(bool is_enabled) +{ auto root_display = context_->getRootDisplayGroup(); for (int i = 0; i < root_display->numDisplays(); ++i) { auto display = root_display->getDisplayAt(i); @@ -62,23 +62,25 @@ void InteractiveTool::enableMarkers(bool is_enabled) { } } -void InteractiveTool::onInitialize() { +void InteractiveTool::onInitialize() +{ rviz_default_plugins::tools::InteractionTool::onInitialize(); this->enableMarkers(false); setName("Interact"); - setIcon(rviz_common::loadPixmap( - "package://rviz_default_plugins/icons/classes/Interact.png")); + setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/Interact.png")); } -void InteractiveTool::activate() { +void InteractiveTool::activate() +{ rviz_default_plugins::tools::InteractionTool::activate(); this->enableMarkers(true); } -void InteractiveTool::deactivate() { +void InteractiveTool::deactivate() +{ rviz_default_plugins::tools::InteractionTool::deactivate(); this->enableMarkers(false); @@ -87,5 +89,4 @@ void InteractiveTool::deactivate() { } // namespace flatland_rviz_plugins #include // NOLINT -PLUGINLIB_EXPORT_CLASS(flatland_rviz_plugins::InteractiveTool, - rviz_common::Tool) +PLUGINLIB_EXPORT_CLASS(flatland_rviz_plugins::InteractiveTool, rviz_common::Tool) diff --git a/flatland_rviz_plugins/src/load_model_dialog.cpp b/flatland_rviz_plugins/src/load_model_dialog.cpp index 8592b75b..54b28d0f 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.cpp +++ b/flatland_rviz_plugins/src/load_model_dialog.cpp @@ -65,34 +65,34 @@ #include #include -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ QString LoadModelDialog::path_to_model_file; int LoadModelDialog::count; bool LoadModelDialog::numbering; -LoadModelDialog::LoadModelDialog(QWidget *parent, - rviz_common::DisplayContext *context, - SpawnModelTool *tool) - : QDialog(parent), context_(context), tool_(tool) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), - "ModelDialog::ModelDialog"); - auto *v_layout = new QVBoxLayout; +LoadModelDialog::LoadModelDialog( + QWidget * parent, rviz_common::DisplayContext * context, SpawnModelTool * tool) +: QDialog(parent), context_(context), tool_(tool) +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "ModelDialog::ModelDialog"); + auto * v_layout = new QVBoxLayout; setLayout(v_layout); // we are injecting horizontal layouts into the master vertical layout - auto *h0_layout = new QHBoxLayout; - auto *h1_layout = new QHBoxLayout; - auto *h2_layout = new QHBoxLayout; - auto *h3_layout = new QHBoxLayout; + auto * h0_layout = new QHBoxLayout; + auto * h1_layout = new QHBoxLayout; + auto * h2_layout = new QHBoxLayout; + auto * h3_layout = new QHBoxLayout; // create widgets - auto *pathButton = new QPushButton("choose file"); + auto * pathButton = new QPushButton("choose file"); p_label = new QLabel; n_checkbox = new QCheckBox; n_edit = new QLineEdit; - auto *okButton = new QPushButton("ok"); - auto *cancelButton = new QPushButton("cancel"); + auto * okButton = new QPushButton("ok"); + auto * cancelButton = new QPushButton("cancel"); // set focus policy, otherwise cr in textfield triggers all the slots pathButton->setFocusPolicy(Qt::NoFocus); @@ -102,14 +102,10 @@ LoadModelDialog::LoadModelDialog(QWidget *parent, okButton->setFocusPolicy(Qt::NoFocus); cancelButton->setFocusPolicy(Qt::NoFocus); - connect(pathButton, &QAbstractButton::clicked, this, - &LoadModelDialog::on_PathButtonClicked); - connect(okButton, &QAbstractButton::clicked, this, - &LoadModelDialog::OkButtonClicked); - connect(cancelButton, &QAbstractButton::clicked, this, - &LoadModelDialog::CancelButtonClicked); - connect(n_checkbox, &QAbstractButton::clicked, this, - &LoadModelDialog::NumberCheckBoxChanged); + connect(pathButton, &QAbstractButton::clicked, this, &LoadModelDialog::on_PathButtonClicked); + connect(okButton, &QAbstractButton::clicked, this, &LoadModelDialog::OkButtonClicked); + connect(cancelButton, &QAbstractButton::clicked, this, &LoadModelDialog::CancelButtonClicked); + connect(n_checkbox, &QAbstractButton::clicked, this, &LoadModelDialog::NumberCheckBoxChanged); // path button h0_layout->addWidget(pathButton); @@ -148,9 +144,9 @@ LoadModelDialog::LoadModelDialog(QWidget *parent, this->setAttribute(Qt::WA_DeleteOnClose, true); } -void LoadModelDialog::CancelButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), - "LoadModelDialog::CancelButtonClicked"); +void LoadModelDialog::CancelButtonClicked() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::CancelButtonClicked"); auto tool_man = context_->getToolManager(); auto dflt_tool = tool_man->getDefaultTool(); @@ -159,9 +155,9 @@ void LoadModelDialog::CancelButtonClicked() { this->close(); } -void LoadModelDialog::OkButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), - "LoadModelDialog::OkButtonClicked"); +void LoadModelDialog::OkButtonClicked() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::OkButtonClicked"); QString name = n_edit->displayText(); @@ -172,9 +168,9 @@ void LoadModelDialog::OkButtonClicked() { this->close(); } -void LoadModelDialog::AddNumberAndUpdateName() { - std::string bsfn = - boost::filesystem::basename(path_to_model_file.toStdString()); +void LoadModelDialog::AddNumberAndUpdateName() +{ + std::string bsfn = boost::filesystem::basename(path_to_model_file.toStdString()); QString name = QString::fromStdString(bsfn); if (numbering) { @@ -185,9 +181,9 @@ void LoadModelDialog::AddNumberAndUpdateName() { n_edit->setText(name); } -void LoadModelDialog::on_PathButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), - "LoadModelDialog::on_PathButtonClicked"); +void LoadModelDialog::on_PathButtonClicked() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::on_PathButtonClicked"); path_to_model_file = ChooseFile(); AddNumberAndUpdateName(); @@ -195,24 +191,23 @@ void LoadModelDialog::on_PathButtonClicked() { n_edit->setFocus(); } -void LoadModelDialog::NumberCheckBoxChanged(bool i) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), - "NumberCheckBoxChanged " << i); +void LoadModelDialog::NumberCheckBoxChanged(bool i) +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "NumberCheckBoxChanged " << i); numbering = !numbering; AddNumberAndUpdateName(); } -QString LoadModelDialog::ChooseFile() { - QString fileName = - QFileDialog::getOpenFileName(NULL, tr("Open model file"), "", ""); - if (fileName.isEmpty()) +QString LoadModelDialog::ChooseFile() +{ + QString fileName = QFileDialog::getOpenFileName(NULL, tr("Open model file"), "", ""); + if (fileName.isEmpty()) { return fileName; - else { + } else { QFile file(fileName); if (!file.open(QIODevice::ReadOnly)) { - QMessageBox::information(NULL, tr("Unable to open file"), - file.errorString()); + QMessageBox::information(NULL, tr("Unable to open file"), file.errorString()); return fileName; } file.close(); diff --git a/flatland_rviz_plugins/src/load_model_dialog.hpp b/flatland_rviz_plugins/src/load_model_dialog.hpp index fd7a339d..5fc99318 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.hpp +++ b/flatland_rviz_plugins/src/load_model_dialog.hpp @@ -72,21 +72,22 @@ #include "flatland_rviz_plugins/spawn_model_tool.hpp" -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ -class LoadModelDialog : public QDialog { - public: +class LoadModelDialog : public QDialog +{ +public: /** * @name LoadModelDialog * @brief Launch load model dialog * @param parent, parent widget pointer * @param tool, pointer to this so dialog can call methods */ - LoadModelDialog(QWidget *parent, rviz_common::DisplayContext *context, - SpawnModelTool *tool); + LoadModelDialog(QWidget * parent, rviz_common::DisplayContext * context, SpawnModelTool * tool); - private: - rviz_common::DisplayContext *context_; +private: + rviz_common::DisplayContext * context_; /** * @name ChooseFile @@ -100,15 +101,15 @@ class LoadModelDialog : public QDialog { void AddNumberAndUpdateName(); static QString path_to_model_file; // full path to model file - static int count; // counter for adding unique number to filename - static bool numbering; // flag to use unique numbering + static int count; // counter for adding unique number to filename + static bool numbering; // flag to use unique numbering - SpawnModelTool *tool_; - QLineEdit *n_edit; // name lineEdit widget - QLabel *p_label; // path label widget - QCheckBox *n_checkbox; // checkbox widget + SpawnModelTool * tool_; + QLineEdit * n_edit; // name lineEdit widget + QLabel * p_label; // path label widget + QCheckBox * n_checkbox; // checkbox widget - public Q_SLOTS: +public Q_SLOTS: /** * @name NumberCheckBoxChanged * @brief Checkbox was clicked, toggle numbering of names diff --git a/flatland_rviz_plugins/src/pause_tool.cpp b/flatland_rviz_plugins/src/pause_tool.cpp index 3abc129a..52d88842 100644 --- a/flatland_rviz_plugins/src/pause_tool.cpp +++ b/flatland_rviz_plugins/src/pause_tool.cpp @@ -46,22 +46,24 @@ #include #include -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ -PauseTool::PauseTool() { shortcut_key_ = ' '; } +PauseTool::PauseTool() {shortcut_key_ = ' ';} PauseTool::~PauseTool() {} -void PauseTool::onInitialize() { +void PauseTool::onInitialize() +{ node_ = rclcpp::Node::make_shared("pause_sim_tool"); pause_service_ = node_->create_client("toggle_pause"); setName("Toggle Pause"); - setIcon(rviz_common::loadPixmap( - "package://flatland_rviz_plugins/icons/pause.svg")); + setIcon(rviz_common::loadPixmap("package://flatland_rviz_plugins/icons/pause.svg")); } -void PauseTool::activate() { +void PauseTool::activate() +{ auto request = std::make_shared(); auto result = pause_service_->async_send_request(request); rclcpp::spin_until_future_complete(node_, result); diff --git a/flatland_rviz_plugins/src/spawn_model_tool.cpp b/flatland_rviz_plugins/src/spawn_model_tool.cpp index b526bde8..8ef8f3f9 100644 --- a/flatland_rviz_plugins/src/spawn_model_tool.cpp +++ b/flatland_rviz_plugins/src/spawn_model_tool.cpp @@ -72,21 +72,22 @@ #include "load_model_dialog.hpp" -namespace flatland_rviz_plugins { +namespace flatland_rviz_plugins +{ QString SpawnModelTool::path_to_model_file_; QString SpawnModelTool::model_name; // Set the "shortcut_key_" member variable defined in the // superclass to declare which key will activate the tool. -SpawnModelTool::SpawnModelTool() : moving_model_node_(nullptr) { - shortcut_key_ = 'm'; -} +SpawnModelTool::SpawnModelTool() +: moving_model_node_(nullptr) {shortcut_key_ = 'm';} // The destructor destroys the Ogre scene nodes for the models so they // disappear from the 3D scene. The destructor for a Tool subclass is // only called when the tool is removed from the toolbar with the "-" // button. -SpawnModelTool::~SpawnModelTool() { +SpawnModelTool::~SpawnModelTool() +{ scene_manager_->destroySceneNode(arrow_->getSceneNode()); scene_manager_->destroySceneNode(moving_model_node_); } @@ -102,13 +103,13 @@ SpawnModelTool::~SpawnModelTool() { // In this case we load a mesh object with the shape and appearance of // the model, create an Ogre::SceneNode for the moving model, and then // set it invisible. -void SpawnModelTool::onInitialize() { +void SpawnModelTool::onInitialize() +{ // hide 3d model until the user clicks the span model tool button model_state = m_hidden; // make an arrow to show axis of rotation - arrow_ = - new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); + arrow_ = new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); arrow_->setColor(0.0f, 0.0f, 1.0f, 1.0f); // blue // will only be visible during rotation phase arrow_->getSceneNode()->setVisible(false); @@ -118,15 +119,13 @@ void SpawnModelTool::onInitialize() { arrow_->setOrientation(orientation); // create an Ogre child scene node for rendering the preview outline - moving_model_node_ = - scene_manager_->getRootSceneNode()->createChildSceneNode(); + moving_model_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); moving_model_node_->setVisible(false); SetMovingModelColor(Qt::green); setName("Load Model"); - setIcon(rviz_common::loadPixmap( - "package://rviz_default_plugins/icons/classes/RobotModel.png")); + setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/RobotModel.png")); } // Activate() is called when the tool is started by the user, either @@ -143,18 +142,18 @@ void SpawnModelTool::onInitialize() { // if it were writable the model should really change position when the // user edits the property. This is a fine idea, and is possible, but // is left as an exercise for the reader. -void SpawnModelTool::activate() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), - "SpawnModelTool::activate "); +void SpawnModelTool::activate() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::activate "); - LoadModelDialog *model_dialog = new LoadModelDialog(nullptr, context_, this); + LoadModelDialog * model_dialog = new LoadModelDialog(nullptr, context_, this); model_dialog->setModal(true); model_dialog->show(); } -void SpawnModelTool::BeginPlacement() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), - "SpawnModelTool::BeginPlacement"); +void SpawnModelTool::BeginPlacement() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::BeginPlacement"); model_state = m_dragging; if (moving_model_node_) { @@ -170,16 +169,18 @@ void SpawnModelTool::BeginPlacement() { // property, so that doesn't need to be done in a separate step. If // we didn't delete it here, it would stay in the list of models when // we switch to another tool. -void SpawnModelTool::deactivate() { +void SpawnModelTool::deactivate() +{ if (moving_model_node_) { moving_model_node_->setVisible(false); } } -void SpawnModelTool::SpawnModelInFlatland() { +void SpawnModelTool::SpawnModelInFlatland() +{ RCLCPP_INFO_STREAM( - rclcpp::get_logger("SpawnModelTool"), - "SpawnModelTool::SpawnModelInFlatland name:" << model_name.toStdString()); + rclcpp::get_logger("SpawnModelTool"), + "SpawnModelTool::SpawnModelInFlatland name:" << model_name.toStdString()); auto srv = std::make_shared(); // model names can not have embeded period char @@ -193,14 +194,12 @@ void SpawnModelTool::SpawnModelInFlatland() { srv->pose.y = intersection[1]; srv->pose.theta = initial_angle; - std::shared_ptr node = - rclcpp::Node::make_shared("spawn_model_tool"); // TODO + std::shared_ptr node = rclcpp::Node::make_shared("spawn_model_tool"); // TODO client = node->create_client("spawn_model"); // make ros service call auto result = client->async_send_request(srv); - if (rclcpp::spin_until_future_complete(node, result) == - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { auto response = result.get(); if (!response->success) { QMessageBox msgBox; @@ -214,18 +213,16 @@ void SpawnModelTool::SpawnModelInFlatland() { } } -void SpawnModelTool::SetMovingModelColor(QColor c) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), - "SpawnModelTool::SetMovingModelColor"); +void SpawnModelTool::SetMovingModelColor(QColor c) +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::SetMovingModelColor"); try { - auto *m_pEntity = - dynamic_cast(moving_model_node_->getAttachedObject(0)); + auto * m_pEntity = dynamic_cast(moving_model_node_->getAttachedObject(0)); const Ogre::MaterialPtr m_pMat = m_pEntity->getSubEntity(0)->getMaterial(); m_pMat->getTechnique(0)->getPass(0)->setAmbient(1, 0, 0); - m_pMat->getTechnique(0)->getPass(0)->setDiffuse(c.redF(), c.greenF(), - c.blueF(), 0); - } catch (Ogre::InvalidParametersException &e) { + m_pMat->getTechnique(0)->getPass(0)->setDiffuse(c.redF(), c.greenF(), c.blueF(), 0); + } catch (Ogre::InvalidParametersException & e) { // RCLCPP_WARN("Invalid preview model"); } } @@ -242,7 +239,8 @@ void SpawnModelTool::SetMovingModelColor(QColor c) { // place and drop the pointer to the VectorProperty. Dropping the // pointer means when the tool is deactivated the VectorProperty won't // be deleted, which is what we want. -int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { +int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ if (!moving_model_node_) { return Render; } @@ -251,9 +249,8 @@ int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { if (model_state == m_dragging) { auto intersectionPair = - rviz_rendering::ViewportProjectionFinder() - .getViewportPointProjectionOnXYPlane(event.panel->getRenderWindow(), - event.x, event.y); + rviz_rendering::ViewportProjectionFinder().getViewportPointProjectionOnXYPlane( + event.panel->getRenderWindow(), event.x, event.y); intersection = intersectionPair.second; if (intersectionPair.first) { moving_model_node_->setVisible(true); @@ -267,16 +264,14 @@ int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { return Render; } } else { - moving_model_node_->setVisible( - false); // If the mouse is not pointing at the - // ground plane, don't show the model. + moving_model_node_->setVisible(false); // If the mouse is not pointing at the + // ground plane, don't show the model. } } if (model_state == m_rotating) { // model_state is m_rotating auto intersectionPair = - rviz_rendering::ViewportProjectionFinder() - .getViewportPointProjectionOnXYPlane(event.panel->getRenderWindow(), - event.x, event.y); + rviz_rendering::ViewportProjectionFinder().getViewportPointProjectionOnXYPlane( + event.panel->getRenderWindow(), event.x, event.y); Ogre::Vector3 intersection2 = intersectionPair.second; if (intersectionPair.first) { if (event.leftDown()) { @@ -295,32 +290,31 @@ int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { moving_model_node_->setPosition(intersection); Ogre::Vector3 dir = intersection2 - intersection; initial_angle = atan2(dir.y, dir.x); - Ogre::Quaternion orientation(Ogre::Radian(initial_angle), - Ogre::Vector3(0, 0, 1)); + Ogre::Quaternion orientation(Ogre::Radian(initial_angle), Ogre::Vector3(0, 0, 1)); moving_model_node_->setOrientation(orientation); } } return Render; } -void SpawnModelTool::LoadPreview() { +void SpawnModelTool::LoadPreview() +{ // Clear the old preview, if there is one moving_model_node_->removeAllChildren(); lines_list_.clear(); - std::shared_ptr node = - rclcpp::Node::make_shared("spawn_model_tool"); // TODO + std::shared_ptr node = rclcpp::Node::make_shared("spawn_model_tool"); // TODO // Load the bodies list into a model object flatland_server::YamlReader reader(node, path_to_model_file_.toStdString()); try { flatland_server::YamlReader bodies_reader = - reader.Subnode("bodies", flatland_server::YamlReader::LIST); + reader.Subnode("bodies", flatland_server::YamlReader::LIST); // Iterate each body and add to the preview for (int i = 0; i < bodies_reader.NodeSize(); i++) { flatland_server::YamlReader body_reader = - bodies_reader.Subnode(i, flatland_server::YamlReader::MAP); + bodies_reader.Subnode(i, flatland_server::YamlReader::MAP); if (!body_reader.Get("enabled", "true")) { // skip if disabled continue; } @@ -328,18 +322,18 @@ void SpawnModelTool::LoadPreview() { auto pose = body_reader.GetPose("pose", flatland_server::Pose()); flatland_server::YamlReader footprints_node = - body_reader.Subnode("footprints", flatland_server::YamlReader::LIST); + body_reader.Subnode("footprints", flatland_server::YamlReader::LIST); for (int j = 0; j < footprints_node.NodeSize(); j++) { flatland_server::YamlReader footprint = - footprints_node.Subnode(j, flatland_server::YamlReader::MAP); + footprints_node.Subnode(j, flatland_server::YamlReader::MAP); - lines_list_.push_back(std::make_shared( + lines_list_.push_back( + std::make_shared( context_->getSceneManager(), moving_model_node_)); auto lines = lines_list_.back(); lines->setColor(0.0, 1.0, 0.0, 0.75); // Green lines->setLineWidth(0.05); - lines->setOrientation( - Ogre::Quaternion(Ogre::Radian(pose.theta), Ogre::Vector3(0, 0, 1))); + lines->setOrientation(Ogre::Quaternion(Ogre::Radian(pose.theta), Ogre::Vector3(0, 0, 1))); lines->setPosition(Ogre::Vector3(pose.x, pose.y, 0)); auto type = footprint.Get("type"); @@ -352,46 +346,43 @@ void SpawnModelTool::LoadPreview() { } } } - } catch (const flatland_server::YAMLException &e) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("SpawnModelTool"), - "Couldn't load model bodies for preview" << e.what()); + } catch (const flatland_server::YAMLException & e) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("SpawnModelTool"), "Couldn't load model bodies for preview" << e.what()); } } -void SpawnModelTool::LoadPolygonFootprint( - flatland_server::YamlReader &footprint) { +void SpawnModelTool::LoadPolygonFootprint(flatland_server::YamlReader & footprint) +{ auto lines = lines_list_.back(); - auto points = footprint.GetList("points", 3, - b2_maxPolygonVertices); + auto points = footprint.GetList("points", 3, b2_maxPolygonVertices); for (auto p : points) { lines->addPoint(Ogre::Vector3(p.x, p.y, 0.)); } if (points.size() > 0) { - lines->addPoint( - Ogre::Vector3(points.at(0).x, points.at(0).y, 0.)); // Close the box + lines->addPoint(Ogre::Vector3(points.at(0).x, points.at(0).y, 0.)); // Close the box } } -void SpawnModelTool::LoadCircleFootprint( - flatland_server::YamlReader &footprint) { +void SpawnModelTool::LoadCircleFootprint(flatland_server::YamlReader & footprint) +{ auto lines = lines_list_.back(); auto center = footprint.GetVec2("center", flatland_server::Vec2()); auto radius = footprint.Get("radius", 1.0); for (float a = 0.; a < M_PI * 2.0; a += M_PI / 8.) { // 16 point circle - lines->addPoint(Ogre::Vector3(center.x + radius * cos(a), - center.y + radius * sin(a), 0.)); + lines->addPoint(Ogre::Vector3(center.x + radius * cos(a), center.y + radius * sin(a), 0.)); } - lines->addPoint( - Ogre::Vector3(center.x + radius, center.y, 0.)); // close the loop + lines->addPoint(Ogre::Vector3(center.x + radius, center.y, 0.)); // close the loop } -void SpawnModelTool::SavePath(QString p) { +void SpawnModelTool::SavePath(QString p) +{ path_to_model_file_ = p; LoadPreview(); } -void SpawnModelTool::SaveName(QString n) { model_name = n; } +void SpawnModelTool::SaveName(QString n) {model_name = n;} // At the end of every plugin class implementation, we end the // namespace and then tell pluginlib about the class. It is important diff --git a/flatland_server/include/flatland_server/body.h b/flatland_server/include/flatland_server/body.h index ccc66361..bc7effc1 100644 --- a/flatland_server/include/flatland_server/body.h +++ b/flatland_server/include/flatland_server/body.h @@ -52,17 +52,19 @@ #include #include -namespace flatland_server { +namespace flatland_server +{ /** * This class defines a body in the simulation. It wraps around the Box2D * physics body providing extra data and useful methods */ -class Body { - public: - Entity *entity_; ///< The entity the body belongs to +class Body +{ +public: + Entity * entity_; ///< The entity the body belongs to std::string name_; ///< name of the body, unique within a model - b2Body *physics_body_; ///< Box2D physics body + b2Body * physics_body_; ///< Box2D physics body Color color_; ///< color, for visualization YAML::Node properties_; ///< Properties document for plugins to use @@ -78,10 +80,10 @@ class Body { * @param[in] linear_damping Box2D body linear damping * @param[in] angular_damping Box2D body angular damping */ - Body(b2World *physics_world, Entity *entity, const std::string &name, - const Color &color, const Pose &pose, b2BodyType body_type, - const YAML::Node &properties, double linear_damping = 0, - double angular_damping = 0); + Body( + b2World * physics_world, Entity * entity, const std::string & name, const Color & color, + const Pose & pose, b2BodyType body_type, const YAML::Node & properties, + double linear_damping = 0, double angular_damping = 0); /** * @brief logs the debugging information for the body @@ -91,19 +93,19 @@ class Body { /** * @return entity associated with the body */ - Entity *GetEntity(); + Entity * GetEntity(); /** * @return name of the body */ - const std::string &GetName() const; + const std::string & GetName() const; /** * @brief Get the Box2D body, use this to manipulate the body in physics * through the Box2D methods * @return Pointer to Box2D physics body */ - b2Body *GetPhysicsBody(); + b2Body * GetPhysicsBody(); /** * @brief Count the number of fixtures @@ -114,12 +116,12 @@ class Body { /** * @return Color of the body */ - const Color &GetColor() const; + const Color & GetColor() const; /** * @brief Set of the color of the body */ - void SetColor(const Color &color); + void SetColor(const Color & color); /** * Destructor for the body @@ -131,7 +133,7 @@ class Body { * and destructing bodies */ Body(const Body &) = delete; - Body &operator=(const Body &) = delete; + Body & operator=(const Body &) = delete; }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_MODEL_BODY_H diff --git a/flatland_server/include/flatland_server/collision_filter_registry.h b/flatland_server/include/flatland_server/collision_filter_registry.h index 934b01e2..4f93af28 100644 --- a/flatland_server/include/flatland_server/collision_filter_registry.h +++ b/flatland_server/include/flatland_server/collision_filter_registry.h @@ -51,7 +51,8 @@ #include #include -namespace flatland_server { +namespace flatland_server +{ /** * This class defines a collision filter registry. It allows layers to register @@ -60,12 +61,13 @@ namespace flatland_server { * the correct collision category. It also hands out unique collide (positive * numbers) and no collide (negative numbers) Box2D collision groups. */ -class CollisionFilterRegistry { - public: +class CollisionFilterRegistry +{ +public: static const int LAYER_NOT_EXIST = -1; ///< No such layer static const int LAYER_ALREADY_EXIST = -2; ///< Layer exists static const int LAYERS_FULL = -3; ///< Cannot add more layers - static const int MAX_LAYERS = 16; ///< 16 is the maximum as defined by Box2D + static const int MAX_LAYERS = 16; ///< 16 is the maximum as defined by Box2D /// internal counter to keep track of no collides groups int no_collide_group_cnt_; @@ -129,8 +131,8 @@ class CollisionFilterRegistry { * this list, optional */ uint16_t GetCategoryBits( - const std::vector &layers, - std::vector *invalid_layers = nullptr) const; + const std::vector & layers, + std::vector * invalid_layers = nullptr) const; }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_COLLISION_FILTER_REGISTRY_H diff --git a/flatland_server/include/flatland_server/debug_visualization.h b/flatland_server/include/flatland_server/debug_visualization.h index 80732f77..8d30bef2 100644 --- a/flatland_server/include/flatland_server/debug_visualization.h +++ b/flatland_server/include/flatland_server/debug_visualization.h @@ -48,30 +48,33 @@ #define FLATLAND_SERVER_DEBUG_VISUALIZATION_H #include + #include -#include -#include #include +#include #include #include +#include #include "flatland_server/body.h" #include "flatland_server/timekeeper.h" -namespace flatland_server { -struct DebugTopic { +namespace flatland_server +{ +struct DebugTopic +{ rclcpp::Publisher::SharedPtr publisher; bool needs_publishing; visualization_msgs::msg::MarkerArray markers; }; -class DebugVisualization { - - public: +class DebugVisualization +{ +public: DebugVisualization(rclcpp::Node::SharedPtr node); std::map topics_; rclcpp::Node::SharedPtr node_; - rclcpp::Publisher::SharedPtr topic_list_publisher_; + rclcpp::Publisher::SharedPtr topic_list_publisher_; /** * @brief Return the singleton object @@ -82,7 +85,7 @@ class DebugVisualization { * @brief Publish all marker array topics_ that need publishing * @param[in] timekeeper The time object to use for header timestamps */ - void Publish(const Timekeeper& timekeeper); + void Publish(const Timekeeper & timekeeper); /** * @brief Visualize body @@ -93,8 +96,7 @@ class DebugVisualization { * @param[in] b blue color 0.0->1.0 * @param[in] a alpha color 0.0->1.0 */ - void Visualize(std::string name, b2Body* body, float r, float g, float b, - float a); + void Visualize(std::string name, b2Body * body, float r, float g, float b, float a); /** * @brief Visualize body @@ -105,8 +107,7 @@ class DebugVisualization { * @param[in] b blue color 0.0->1.0 * @param[in] a alpha color 0.0->1.0 */ - void Visualize(std::string name, b2Joint* joint, float r, float g, float b, - float a); + void Visualize(std::string name, b2Joint * joint, float r, float g, float b, float a); /** * @brief Visualize a layer in 2.5d @@ -117,7 +118,7 @@ class DebugVisualization { * @param[in] b blue color 0.0->1.0 * @param[in] a alpha color 0.0->1.0 */ - void VisualizeLayer(std::string name, Body* body); + void VisualizeLayer(std::string name, Body * body); /** * @brief Remove all elements in a visualization topic @@ -134,8 +135,9 @@ class DebugVisualization { * @param[in] b blue color 0.0->1.0 * @param[in] a alpha color 0.0->1.0 */ - void BodyToMarkers(visualization_msgs::msg::MarkerArray& markers, b2Body* body, - float r, float g, float b, float a); + void BodyToMarkers( + visualization_msgs::msg::MarkerArray & markers, b2Body * body, float r, float g, float b, + float a); /** * @brief Append a joint as a marker on the marker array @@ -146,19 +148,20 @@ class DebugVisualization { * @param[in] b blue color 0.0->1.0 * @param[in] a alpha color 0.0->1.0 */ - void JointToMarkers(visualization_msgs::msg::MarkerArray& markers, b2Joint* joint, - float r, float g, float b, float a); + void JointToMarkers( + visualization_msgs::msg::MarkerArray & markers, b2Joint * joint, float r, float g, float b, + float a); /** * @brief Ensure that a topic name is being broadcasted * @param[in] name Name of the topic */ - void AddTopicIfNotExist(const std::string& name); + void AddTopicIfNotExist(const std::string & name); /** * @brief Publish topics list */ void PublishTopicList(); }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_DEBUG_VISUALIZATION_H diff --git a/flatland_server/include/flatland_server/dummy_model_plugin.h b/flatland_server/include/flatland_server/dummy_model_plugin.h index 7e22b661..fe5eef8c 100644 --- a/flatland_server/include/flatland_server/dummy_model_plugin.h +++ b/flatland_server/include/flatland_server/dummy_model_plugin.h @@ -56,13 +56,15 @@ using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ /** * This is a dummy plugin of type model plugin, used completely for testing * purposes */ -class DummyModelPlugin : public ModelPlugin { - public: +class DummyModelPlugin : public ModelPlugin +{ +public: int dummy_param_int_; ///< Iteger variable for testing std::string dummy_param_string_; ///< String variable for testing double dummy_param_float_; ///< float variable for testing @@ -71,8 +73,8 @@ class DummyModelPlugin : public ModelPlugin { * @brief Initialization for the plugin * @param[in] config Plugin YAML Node */ - void OnInitialize(const YAML::Node &config) override; -}; + void OnInitialize(const YAML::Node & config) override; }; +}; // namespace flatland_plugins #endif \ No newline at end of file diff --git a/flatland_server/include/flatland_server/dummy_world_plugin.h b/flatland_server/include/flatland_server/dummy_world_plugin.h index bbfe806d..be7d2a62 100644 --- a/flatland_server/include/flatland_server/dummy_world_plugin.h +++ b/flatland_server/include/flatland_server/dummy_world_plugin.h @@ -53,20 +53,22 @@ using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ /** * This is a dummy plugin of type world plugin, used completely for testing * purposes */ -class DummyWorldPlugin : public WorldPlugin { - public: +class DummyWorldPlugin : public WorldPlugin +{ +public: /** * @brief Initialization for the plugin * @param[in] config Plugin YAML Node * @param[in] world_config The world configuration file */ - void OnInitialize(const YAML::Node &plugin_reader, YamlReader &world_config) override; + void OnInitialize(const YAML::Node & plugin_reader, YamlReader & world_config) override; }; -} +} // namespace flatland_plugins #endif \ No newline at end of file diff --git a/flatland_server/include/flatland_server/entity.h b/flatland_server/include/flatland_server/entity.h index a08f28ef..cb6e0f31 100644 --- a/flatland_server/include/flatland_server/entity.h +++ b/flatland_server/include/flatland_server/entity.h @@ -50,23 +50,26 @@ #include #include #include + #include -namespace flatland_server { +namespace flatland_server +{ /** * This class defines a entity in the simulation world. It provides a class * for high level physical things in the world to inherit from (layers and * models) */ -class Entity { - public: +class Entity +{ +public: /// Defines the type of entity enum EntityType { LAYER, MODEL }; std::shared_ptr node_; - b2World *physics_world_; ///< Box2D physics world - std::string name_; ///< name of the entity + b2World * physics_world_; ///< Box2D physics world + std::string name_; ///< name of the entity /** * @brief Constructor for the entity @@ -74,25 +77,25 @@ class Entity { * @param[in] physics_world Box2D physics_world * @param[in] name name of the entity */ - Entity(std::shared_ptr node, b2World *physics_world, const std::string &name); + Entity(std::shared_ptr node, b2World * physics_world, const std::string & name); virtual ~Entity() = default; /** * @return name of the entity */ - const std::string &GetName() const; + const std::string & GetName() const; /** * @brief Get Box2D physics world * @return Pointer to Box2D physics world, use this to call Box2D world * methods */ - b2World *GetPhysicsWorld(); + b2World * GetPhysicsWorld(); /// This class should be non-copyable. This will cause the destructor to be /// called twice for a given b2Body Entity(const Entity &) = delete; - Entity &operator=(const Entity &) = delete; + Entity & operator=(const Entity &) = delete; /** * @brief Get the type of entity, subclasses must override @@ -110,5 +113,5 @@ class Entity { */ virtual void DebugOutput() const = 0; }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_ENTITY_H diff --git a/flatland_server/include/flatland_server/exceptions.h b/flatland_server/include/flatland_server/exceptions.h index 2af2153b..d281afd1 100644 --- a/flatland_server/include/flatland_server/exceptions.h +++ b/flatland_server/include/flatland_server/exceptions.h @@ -48,34 +48,39 @@ #define FLATLAND_SERVER_EXCEPTIONS_H #include + #include #include #include -namespace flatland_server { +namespace flatland_server +{ -class Exception : public std::runtime_error { - public: +class Exception : public std::runtime_error +{ +public: /** - * @brief Constructor for the Exception class - */ - Exception(const std::string &msg) : runtime_error(msg) {} + * @brief Constructor for the Exception class + */ + Exception(const std::string & msg) : runtime_error(msg) {} }; -class PluginException : public Exception { - public: +class PluginException : public Exception +{ +public: /** * @brief Constructor for PluginException * @param[in] msg custom message */ - PluginException(const std::string &msg) : Exception(ErrorMsg(msg)) {} + PluginException(const std::string & msg) : Exception(ErrorMsg(msg)) {} - private: +private: /** * @brief Generates exception message for plugin exception * @param[in] msg Custom error message */ - static const std::string ErrorMsg(const std::string &msg) { + static const std::string ErrorMsg(const std::string & msg) + { std::stringstream output; output << "Flatland plugin: "; output << msg; @@ -83,44 +88,43 @@ class PluginException : public Exception { } }; -class YAMLException : public Exception { - public: +class YAMLException : public Exception +{ +public: /** * @brief Constructor for the YAMLException class, stores and generates * exception message using yaml cpp exceptions * @param[in] msg Exception message * @param[in] yaml_cpp_exception Exception generated from YAML cpp */ - YAMLException(const std::string &msg, - const YAML::Exception &yaml_cpp_exception) - : Exception( - ErrorMsg(msg, yaml_cpp_exception.msg, yaml_cpp_exception.mark)) {} + YAMLException(const std::string & msg, const YAML::Exception & yaml_cpp_exception) + : Exception(ErrorMsg(msg, yaml_cpp_exception.msg, yaml_cpp_exception.mark)) + { + } /** * @brief Constructor for the YAMLException class, stores and generates * exception message using just a message * @param[in] msg Exception message */ - YAMLException(const std::string &msg) : Exception("Flatland YAML: " + msg) {} + YAMLException(const std::string & msg) : Exception("Flatland YAML: " + msg) {} - private: +private: /** * @brief Generates exception message from yaml cpp messages * @param[in] msg Exception message * @param[in] yaml_cpp_msg Exception message generated by yaml cpp * @param[in] yaml_cpp_mark Mark generated by yaml cpp */ - static const std::string ErrorMsg(const std::string &msg, - const std::string &yaml_cpp_msg, - const YAML::Mark &yaml_cpp_mark) { + static const std::string ErrorMsg( + const std::string & msg, const std::string & yaml_cpp_msg, const YAML::Mark & yaml_cpp_mark) + { std::stringstream output; output << "Flatland YAML: "; output << msg; - if (!(yaml_cpp_mark.pos == -1 && yaml_cpp_mark.line == -1 && - yaml_cpp_mark.column == -1)) { - output << ", line " << yaml_cpp_mark.line + 1 << " col " - << yaml_cpp_mark.column + 1; + if (!(yaml_cpp_mark.pos == -1 && yaml_cpp_mark.line == -1 && yaml_cpp_mark.column == -1)) { + output << ", line " << yaml_cpp_mark.line + 1 << " col " << yaml_cpp_mark.column + 1; } if (yaml_cpp_msg.size() > 0) { @@ -130,6 +134,6 @@ class YAMLException : public Exception { return output.str(); } }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_WORLD_H diff --git a/flatland_server/include/flatland_server/flatland_plugin.h b/flatland_server/include/flatland_server/flatland_plugin.h index be8532e9..19a4481c 100644 --- a/flatland_server/include/flatland_server/flatland_plugin.h +++ b/flatland_server/include/flatland_server/flatland_plugin.h @@ -52,13 +52,16 @@ #include #include -#include #include + +#include #include -namespace flatland_server { -class FlatlandPlugin { - public: +namespace flatland_server +{ +class FlatlandPlugin +{ +public: enum class PluginType { Invalid, Model, World }; // Different plugin Types std::string type_; ///< type of the plugin std::string name_; ///< name of the plugin @@ -66,64 +69,64 @@ class FlatlandPlugin { PluginType plugin_type_; /* - * @brief Get PluginType - */ + * @brief Get PluginType + */ PluginType Type() { return plugin_type_; } /** - * @brief Get plugin name - */ - const std::string &GetName() const { return name_; } + * @brief Get plugin name + */ + const std::string & GetName() const { return name_; } /** - * @brief Get type of plugin - */ - const std::string &GetType() const { return type_; } + * @brief Get type of plugin + */ + const std::string & GetType() const { return type_; } /** * @brief This method is called before the Box2D physics step * @param[in] timekeeper provide time related information */ - virtual void BeforePhysicsStep(const Timekeeper &timekeeper) {} + virtual void BeforePhysicsStep(const Timekeeper & timekeeper) {} /** * @brief This method is called after the Box2D physics step * @param[in] timekeeper provide time related information */ - virtual void AfterPhysicsStep(const Timekeeper &timekeeper) {} + virtual void AfterPhysicsStep(const Timekeeper & timekeeper) {} /** * @brief A method that is called for all Box2D begin contacts * @param[in] contact Box2D contact */ - virtual void BeginContact(b2Contact *contact) {} + virtual void BeginContact(b2Contact * contact) {} /** * @brief A method that is called for all Box2D end contacts * @param[in] contact Box2D contact */ - virtual void EndContact(b2Contact *contact) {} + virtual void EndContact(b2Contact * contact) {} /** * @brief A method that is called for Box2D presolve * @param[in] contact Box2D contact * @param[in] oldManifold Manifold from the previous iteration */ - virtual void PreSolve(b2Contact *contact, const b2Manifold *oldManifold) {} + virtual void PreSolve(b2Contact * contact, const b2Manifold * oldManifold) {} /** * @brief A method that is called for Box2D postsolve * @param[in] contact Box2D contact * @param[in] impulse Impulse from the collision resolution */ - virtual void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) {} + virtual void PostSolve(b2Contact * contact, const b2ContactImpulse * impulse) {} /** * @brief Flatland plugin destructor */ virtual ~FlatlandPlugin() = default; }; -} //namespace +} // namespace flatland_server #pragma GCC diagnostic pop diff --git a/flatland_server/include/flatland_server/geometry.h b/flatland_server/include/flatland_server/geometry.h index d53881d4..b0b3345d 100644 --- a/flatland_server/include/flatland_server/geometry.h +++ b/flatland_server/include/flatland_server/geometry.h @@ -49,19 +49,22 @@ #include -namespace flatland_server { +namespace flatland_server +{ /** * A struct to store precalculated 2D homogenous transformation values */ -struct RotateTranslate { +struct RotateTranslate +{ double dx, dy; double cos; double sin; }; -class Geometry { - public: +class Geometry +{ +public: /** * @brief Creates a transform for given position and yaw delta * @param[in] dx Delta x @@ -77,7 +80,7 @@ class Geometry { * @param[in] rt Defined transformation * @return Transformed point */ - static b2Vec2 Transform(const b2Vec2& in, const RotateTranslate& rt); + static b2Vec2 Transform(const b2Vec2 & in, const RotateTranslate & rt); /** * @brief Inverse transform a point for given transformation @@ -85,8 +88,8 @@ class Geometry { * @param[in] rt Defined transformation * @return Inverse transformed point */ - static b2Vec2 InverseTransform(const b2Vec2& in, const RotateTranslate& rt); + static b2Vec2 InverseTransform(const b2Vec2 & in, const RotateTranslate & rt); }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_GEOMETRY_H diff --git a/flatland_server/include/flatland_server/interactive_marker_manager.h b/flatland_server/include/flatland_server/interactive_marker_manager.h index b070476e..44cbdd4a 100644 --- a/flatland_server/include/flatland_server/interactive_marker_manager.h +++ b/flatland_server/include/flatland_server/interactive_marker_manager.h @@ -5,25 +5,28 @@ #include #include #include + +#include #include -#include -#include #include +#include +#include #include -#include -namespace flatland_server { +namespace flatland_server +{ -class InteractiveMarkerManager { - public: +class InteractiveMarkerManager +{ +public: /** * @brief Constructor for the interactive marker manager class * @param[in] model_list_ptr Pointer to the list of models in the World class * @param[in] plugin_manager_ptr Pointer to the plugin manager in the World * class */ - InteractiveMarkerManager(std::vector* model_list_ptr, - PluginManager* plugin_manager_ptr); + InteractiveMarkerManager( + std::vector * model_list_ptr, PluginManager * plugin_manager_ptr); /** * @brief Destructor for the interactive marker manager class @@ -37,15 +40,15 @@ class InteractiveMarkerManager { * @param[in] body_markers Marker array corresponding to new model bodies */ void createInteractiveMarker( - const std::string& model_name, const Pose& pose, - const visualization_msgs::msg::MarkerArray& body_markers); + const std::string & model_name, const Pose & pose, + const visualization_msgs::msg::MarkerArray & body_markers); /** * @brief Remove interactive marker corresponding to a given model when * deleting it from the simulation * @param[in] model_name Name of the model being deleted */ - void deleteInteractiveMarker(const std::string& model_name); + void deleteInteractiveMarker(const std::string & model_name); /** * @brief Update the interactive marker poses after running @@ -55,16 +58,14 @@ class InteractiveMarkerManager { bool isManipulating() { return manipulating_model_; } - private: +private: interactive_markers::MenuHandler - menu_handler_; ///< Handler for the interactive marker context menus - std::shared_ptr - interactive_marker_server_ = nullptr; ///< Interactive marker server - std::vector* - models_; ///< Pointer to the model list in the World class - PluginManager* - plugin_manager_; ///< Pointer to the plugin manager in the World class - bool manipulating_model_; ///< Boolean flag indicating if the user is + menu_handler_; ///< Handler for the interactive marker context menus + std::shared_ptr interactive_marker_server_ = + nullptr; ///< Interactive marker server + std::vector * models_; ///< Pointer to the model list in the World class + PluginManager * plugin_manager_; ///< Pointer to the plugin manager in the World class + bool manipulating_model_; ///< Boolean flag indicating if the user is /// manipulating a model with its interactive marker rclcpp::Time pose_update_stamp_; ///< Timestamp of the last received pose /// update feedback. Used to handle when the @@ -73,42 +74,42 @@ class InteractiveMarkerManager { /// MOUSE_UP event. /** - * @brief Process interactive feedback on a MOUSE_UP event and use it - * to move the appropriate model to the new pose - * @param[in] feedback The feedback structure containing the name of the - * manipulated model and the new pose - */ + * @brief Process interactive feedback on a MOUSE_UP event and use it + * to move the appropriate model to the new pose + * @param[in] feedback The feedback structure containing the name of the + * manipulated model and the new pose + */ void processMouseUpFeedback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); /** - * @brief Process interactive feedback on a MOUSE_DOWN event and use it - * to set a flag indicating that the user is manipulating a model with - * its interactive marker - * @param[in] feedback The feedback structure containing the name of the - * manipulated model and the current pose. Not used in this callback - */ + * @brief Process interactive feedback on a MOUSE_DOWN event and use it + * to set a flag indicating that the user is manipulating a model with + * its interactive marker + * @param[in] feedback The feedback structure containing the name of the + * manipulated model and the current pose. Not used in this callback + */ void processMouseDownFeedback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); /** - * @brief Process interactive feedback on a POSE_UPDATE event and record - * the current wall time to detect a timeout in the update method - * @param[in] feedback The feedback structure containing the name of the - * manipulated model and the current pose. Not used in this method - */ + * @brief Process interactive feedback on a POSE_UPDATE event and record + * the current wall time to detect a timeout in the update method + * @param[in] feedback The feedback structure containing the name of the + * manipulated model and the current pose. Not used in this method + */ void processPoseUpdateFeedback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); /** - * @brief Process feedback from the context menu of the interactive marker to - * delete the appropriate model - * @param[in] feedback The feedback structure containing the name of the model - * to be deleted - */ + * @brief Process feedback from the context menu of the interactive marker to + * delete the appropriate model + * @param[in] feedback The feedback structure containing the name of the model + * to be deleted + */ void deleteModelMenuCallback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); }; -} +} // namespace flatland_server #endif // INTERACTIVEMARKERMANAGER_H diff --git a/flatland_server/include/flatland_server/joint.h b/flatland_server/include/flatland_server/joint.h index 45d4b86f..f53d6b1d 100644 --- a/flatland_server/include/flatland_server/joint.h +++ b/flatland_server/include/flatland_server/joint.h @@ -52,7 +52,8 @@ #include #include -namespace flatland_server { +namespace flatland_server +{ class Model; @@ -60,13 +61,14 @@ class Model; * This class defines a joint in the simulation world. It wraps around the Box2D * physics joints providing extra data and useful methods */ -class Joint { - public: - Model *model_; ///< Model the joint belongs to - std::string name_; ///< Name of the joint - b2World *physics_world_; ///< Box2D physics world - Color color_; ///< Color for visualization - b2Joint *physics_joint_; ///< Box2D physics joint +class Joint +{ +public: + Model * model_; ///< Model the joint belongs to + std::string name_; ///< Name of the joint + b2World * physics_world_; ///< Box2D physics world + Color color_; ///< Color for visualization + b2Joint * physics_joint_; ///< Box2D physics joint /** * @brief Constructor for the joint @@ -76,13 +78,14 @@ class Joint { * @param[in] color Color to visualize the joint * @param[in] joint_def Box2D joint definition */ - Joint(b2World *physics_world, Model *model, const std::string &name, - const Color &color, const b2JointDef &joint_def); + Joint( + b2World * physics_world, Model * model, const std::string & name, const Color & color, + const b2JointDef & joint_def); ~Joint(); /// Disallow copying of joints, problematic for constructors and destructors Joint(const Joint &) = delete; - Joint &operator=(const Joint &) = delete; + Joint & operator=(const Joint &) = delete; /** * @brief logs the debugging information for the joint @@ -92,32 +95,32 @@ class Joint { /** * @return Pointer to the model that contains the joint */ - Model *GetModel(); + Model * GetModel(); /** * @return Name of the the joint */ - const std::string &GetName() const; + const std::string & GetName() const; /** * @return Color of the joint for visualization */ - const Color &GetColor() const; + const Color & GetColor() const; /** * @return Color of the joint for visualization */ - void SetColor(const Color &color); + void SetColor(const Color & color); /** * @return Get pointer to the Box2D physics joint */ - b2Joint *GetPhysicsJoint(); + b2Joint * GetPhysicsJoint(); /** * @return Get pointer of the Box2D physics world */ - b2World *GetphysicsWorld(); + b2World * GetphysicsWorld(); /** * @brief Creates a joint for the given params, throws exceptions upon failure @@ -127,8 +130,7 @@ class Joint { * information * @return A new joint as defined by the input data */ - static Joint *MakeJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader); + static Joint * MakeJoint(b2World * physics_world, Model * model, YamlReader & joint_reader); /** * @brief Creates a revolute joint for the given params, throws exceptions @@ -147,12 +149,10 @@ class Joint { * collide * @return A new revolute joint as defined by the input data */ - static Joint *MakeRevoluteJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader, - const std::string &name, const Color &color, - b2Body *body_A, b2Vec2 anchor_A, - b2Body *body_B, b2Vec2 anchor_B, - bool collide_connected); + static Joint * MakeRevoluteJoint( + b2World * physics_world, Model * model, YamlReader & joint_reader, const std::string & name, + const Color & color, b2Body * body_A, b2Vec2 anchor_A, b2Body * body_B, b2Vec2 anchor_B, + bool collide_connected); /** * @brief Creates a weld joint for the given params, throws exceptions upon @@ -171,11 +171,10 @@ class Joint { * collide * @return A new weld joint as defined by the input data */ - static Joint *MakeWeldJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader, const std::string &name, - const Color &color, b2Body *body_A, - b2Vec2 anchor_A, b2Body *body_B, b2Vec2 anchor_B, - bool collide_connected); + static Joint * MakeWeldJoint( + b2World * physics_world, Model * model, YamlReader & joint_reader, const std::string & name, + const Color & color, b2Body * body_A, b2Vec2 anchor_A, b2Body * body_B, b2Vec2 anchor_B, + bool collide_connected); }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_MODEL_JOINT_H diff --git a/flatland_server/include/flatland_server/layer.h b/flatland_server/include/flatland_server/layer.h index 383b13f4..730eda20 100644 --- a/flatland_server/include/flatland_server/layer.h +++ b/flatland_server/include/flatland_server/layer.h @@ -53,23 +53,26 @@ #include #include #include + #include #include -namespace flatland_server { +namespace flatland_server +{ /** * This class defines a layer in the simulation world which simulates the * environment in the world */ -class Layer : public Entity { - public: +class Layer : public Entity +{ +public: std::vector names_; ///< list of layer names std::shared_ptr node_; - Body *body_ = nullptr; - CollisionFilterRegistry *cfr_; ///< collision filter registry - std::string viz_name_; ///< for visualization + Body * body_ = nullptr; + CollisionFilterRegistry * cfr_; ///< collision filter registry + std::string viz_name_; ///< for visualization /** * @brief Constructor for the Layer class for initialization using a image @@ -87,11 +90,11 @@ class Layer : public Entity { * @param[in] resolution Resolution of the map image in meters per pixel * @param[in] properties A YAML node containing properties for plugins to use */ - Layer(std::shared_ptr node, - b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const Pose &origin, const cv::Mat &bitmap, double occupied_thresh, - double resolution, const YAML::Node &properties); + Layer( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::vector & names, const Color & color, const Pose & origin, + const cv::Mat & bitmap, double occupied_thresh, double resolution, + const YAML::Node & properties); /** * @brief Constructor for the Layer class for initialization using line @@ -109,24 +112,24 @@ class Layer : public Entity { * the same way as resolution * @param[in] properties A YAML node containing properties for plugins to use */ - Layer(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const Pose &origin, const std::vector &line_segments, - double scale, const YAML::Node &properties); + Layer( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::vector & names, const Color & color, const Pose & origin, + const std::vector & line_segments, double scale, const YAML::Node & properties); /** - * @brief Constructor for the Layer class for initialization with no static - * map in it - * @param[in] rclcpp::Node shared pointer to the current ROS node - * @param[in] physics_world Pointer to the box2d physics world - * @param[in] cfr Collision filter registry - * @param[in] names A list of names for the layer, the first name is used - * for the name of the body - * @param[in] properties A YAML node containing properties for plugins to use - */ - Layer(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const YAML::Node &properties); + * @brief Constructor for the Layer class for initialization with no static + * map in it + * @param[in] rclcpp::Node shared pointer to the current ROS node + * @param[in] physics_world Pointer to the box2d physics world + * @param[in] cfr Collision filter registry + * @param[in] names A list of names for the layer, the first name is used + * for the name of the body + * @param[in] properties A YAML node containing properties for plugins to use + */ + Layer( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::vector & names, const Color & color, const YAML::Node & properties); /** * @brief Destructor for the layer class @@ -136,14 +139,14 @@ class Layer : public Entity { /** * @return The list of names the layer has */ - const std::vector &GetNames() const; + const std::vector & GetNames() const; /** * @return The collision filter registrar */ - const CollisionFilterRegistry *GetCfr() const; + const CollisionFilterRegistry * GetCfr() const; - Body *GetBody(); + Body * GetBody(); /** * @brief Return the type of entity @@ -157,8 +160,7 @@ class Layer : public Entity { * @param[in] occupied_thresh Threshold indicating obstacle if above * @param[in] resolution Resolution of the map image in meters per pixel */ - void LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, - double resolution); + void LoadFromBitmap(const cv::Mat & bitmap, double occupied_thresh, double resolution); /** * @brief Visualize layer for debugging purposes @@ -176,8 +178,8 @@ class Layer : public Entity { * @param[in] file_path Path to the file * @param[out] line_segments Line segments obtained from the file */ - static void ReadLineSegmentsFile(const std::string &file_path, - std::vector &line_segments); + static void ReadLineSegmentsFile( + const std::string & file_path, std::vector & line_segments); /** * @brief Factory method to instantiate a layer, throws exceptions upon @@ -194,10 +196,10 @@ class Layer : public Entity { * @param[in] properties A YAML node containing properties for plugins to use * @return A new layer */ - static Layer *MakeLayer(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &map_path, - const std::vector &names, - const Color &color, const YAML::Node &properties); + static Layer * MakeLayer( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::string & map_path, const std::vector & names, const Color & color, + const YAML::Node & properties); }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_WORLD_H diff --git a/flatland_server/include/flatland_server/model.h b/flatland_server/include/flatland_server/model.h index dfa366f9..1d0974ad 100644 --- a/flatland_server/include/flatland_server/model.h +++ b/flatland_server/include/flatland_server/model.h @@ -53,9 +53,11 @@ #include #include #include + #include -namespace flatland_server { +namespace flatland_server +{ class ModelBody; class Joint; @@ -64,13 +66,14 @@ class Joint; * This class defines a Model. It can be used to repsent any object in the * environment such robots, chairs, deskes etc. */ -class Model : public Entity { - public: +class Model : public Entity +{ +public: std::string namespace_; ///< namespace of the model std::vector bodies_; ///< list of bodies in the model std::vector joints_; ///< list of joints in the model YamlReader plugins_reader_; ///< for storing plugins when paring YAML - CollisionFilterRegistry *cfr_; ///< Collision filter registry + CollisionFilterRegistry * cfr_; ///< Collision filter registry std::string viz_name_; ///< used for visualization /** @@ -80,8 +83,9 @@ class Model : public Entity { * @param[in] cfr Collision filter registry * @param[in] name Name of the model */ - Model (std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &ns, const std::string &name); + Model( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::string & ns, const std::string & name); /** * @brief Destructor for the layer class @@ -98,62 +102,62 @@ class Model : public Entity { * @brief load bodies to this model, throws exceptions upon failure * @param[in] bodies_reader YAML reader for node containing the list of bodies */ - void LoadBodies(YamlReader &bodies_reader); + void LoadBodies(YamlReader & bodies_reader); /** * @brief load joints to this model, throws exceptions upon failure * @param[in] joints_reader YAML reader for node containing the list of joints */ - void LoadJoints(YamlReader &joints_reader); + void LoadJoints(YamlReader & joints_reader); /** * @brief Get a body in the model using its name * @param[in] name Name of the body * @return pointer to the body, nullptr indicates body cannot be found */ - ModelBody *GetBody(const std::string &name); + ModelBody * GetBody(const std::string & name); /** * @brief Get a body in the model using its name * @param[in] name Name of the joint * @return pointer to the joint, nullptr if the joint does not exist */ - Joint *GetJoint(const std::string &name); + Joint * GetJoint(const std::string & name); /** * @return List of bodies the model has */ - const std::vector &GetBodies(); + const std::vector & GetBodies(); /** * @return List of joints the model has */ - const std::vector &GetJoints(); + const std::vector & GetJoints(); /** * @return The namespace of the model */ - const std::string &GetNameSpace() const; + const std::string & GetNameSpace() const; /** * @return prepend the tf with namespace_ */ - std::string NameSpaceTF(const std::string &frame_id) const; + std::string NameSpaceTF(const std::string & frame_id) const; /** * @return prepend the topic with namespace/ */ - std::string NameSpaceTopic(const std::string &topic) const; + std::string NameSpaceTopic(const std::string & topic) const; /** * @return The name of the model */ - const std::string &GetName() const; + const std::string & GetName() const; /** * @return The collision filter registrar */ - const CollisionFilterRegistry *GetCfr() const; + const CollisionFilterRegistry * GetCfr() const; /** * @brief Publish debug visualizations for model @@ -174,13 +178,13 @@ class Model : public Entity { * @brief transform all bodies in the model * @param[in] pose_delta dx, dy, dyaw */ - void TransformAll(const Pose &pose_delta); + void TransformAll(const Pose & pose_delta); /** * @brief Explicitly set the model pose in world coordinates * @param[in] pose world x, world y, world yaw */ - void SetPose(const Pose &pose); + void SetPose(const Pose & pose); /** * @brief Create a model, throws exceptions upon failure @@ -192,9 +196,9 @@ class Model : public Entity { * @param[in] name Name of the model * @return A new model */ - static Model *MakeModel(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &model_yaml_path, - const std::string &ns, const std::string &name); + static Model * MakeModel( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::string & model_yaml_path, const std::string & ns, const std::string & name); }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_MODEL_H diff --git a/flatland_server/include/flatland_server/model_body.h b/flatland_server/include/flatland_server/model_body.h index 0ec5e363..ed001304 100644 --- a/flatland_server/include/flatland_server/model_body.h +++ b/flatland_server/include/flatland_server/model_body.h @@ -53,7 +53,8 @@ #include #include -namespace flatland_server { +namespace flatland_server +{ class Model; @@ -62,9 +63,10 @@ class Model; * with a model. It contains members and useful methods that is specfic for a * body in a model */ -class ModelBody : public Body { - public: - CollisionFilterRegistry *cfr_; ///< collision filter registry +class ModelBody : public Body +{ +public: + CollisionFilterRegistry * cfr_; ///< collision filter registry /** * @brief Constructor for the Model Body @@ -80,22 +82,22 @@ class ModelBody : public Body { * @param[in] linear_damping Box2D body linear damping * @param[in] angular_damping Box2D body angular damping */ - ModelBody(b2World *physics_world, CollisionFilterRegistry *cfr, Model *model, - const std::string &name, const Color &color, const Pose &pose, - b2BodyType body_type, const YAML::Node &properties, - double linear_damping, double angular_damping); + ModelBody( + b2World * physics_world, CollisionFilterRegistry * cfr, Model * model, const std::string & name, + const Color & color, const Pose & pose, b2BodyType body_type, const YAML::Node & properties, + double linear_damping, double angular_damping); /** * @return The collision filter registry */ - const CollisionFilterRegistry *GetCfr() const; + const CollisionFilterRegistry * GetCfr() const; /** * @brief Load footprints (Box2D fixtures) into the body * @param[in] footprints_reader YAML reader for node containing the footprints * parameters */ - void LoadFootprints(YamlReader &footprints_reader); + void LoadFootprints(YamlReader & footprints_reader); /** * @brief Configures the common properties of footprints @@ -103,22 +105,21 @@ class ModelBody : public Body { * parameters * @param[out] fixture_def Box2D fixture definition */ - void ConfigFootprintDef(YamlReader &footprint_reader, - b2FixtureDef &fixture_def); + void ConfigFootprintDef(YamlReader & footprint_reader, b2FixtureDef & fixture_def); /** * @brief Loads a circle footprint * @param[in] footprint_reader YAML reader for node containing the footprint * parameters */ - void LoadCircleFootprint(YamlReader &footprint_reader); + void LoadCircleFootprint(YamlReader & footprint_reader); /** * @brief Loads a circle footprint * @param[in] footprint_reader YAML reader for node containing the footprint * parameters */ - void LoadPolygonFootprint(YamlReader &footprint_reader); + void LoadPolygonFootprint(YamlReader & footprint_reader); /** * @brief Factory method to create a model body @@ -127,10 +128,9 @@ class ModelBody : public Body { * @param[in] model The model this model body belongs to * @param[in] body_node YAML reader for node containing the body parameters */ - static ModelBody *MakeBody(b2World *physics_world, - CollisionFilterRegistry *cfr, Model *model, - YamlReader &body_node); + static ModelBody * MakeBody( + b2World * physics_world, CollisionFilterRegistry * cfr, Model * model, YamlReader & body_node); }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_MODEL_BODY_H diff --git a/flatland_server/include/flatland_server/model_plugin.h b/flatland_server/include/flatland_server/model_plugin.h index 9fa6b13d..24f5d78e 100644 --- a/flatland_server/include/flatland_server/model_plugin.h +++ b/flatland_server/include/flatland_server/model_plugin.h @@ -47,40 +47,41 @@ #ifndef FLATLAND_SERVER_MODEL_PLUGIN_H #define FLATLAND_SERVER_MODEL_PLUGIN_H - - #include #include #include #include -#include #include -namespace flatland_server { +#include + +namespace flatland_server +{ /** * This class defines a model plugin. All implemented model plugins will inherit * from it A model plugin is a plugin that is directly tied to a single model in * the world */ -class ModelPlugin : public FlatlandPlugin { - private: - Model *model_; ///< model this plugin is tied to +class ModelPlugin : public FlatlandPlugin +{ +private: + Model * model_; ///< model this plugin is tied to - public: +public: rclcpp::Node::SharedPtr node_; ///< ROS node /** * @brief Get model */ - Model *GetModel(); + Model * GetModel(); /** * @brief The method for the particular model plugin to override and provide * its own initialization * @param[in] config The plugin YAML node */ - virtual void OnInitialize(const YAML::Node &config) = 0; + virtual void OnInitialize(const YAML::Node & config) = 0; /** * @brief The method to initialize the ModelPlugin, required since Pluginlib @@ -91,8 +92,9 @@ class ModelPlugin : public FlatlandPlugin { * @param[in] model The model associated with this model plugin * @param[in] config The plugin YAML node */ - void Initialize(rclcpp::Node::SharedPtr node, const std::string &type, const std::string &name, - Model *model, const YAML::Node &config); + void Initialize( + rclcpp::Node::SharedPtr node, const std::string & type, const std::string & name, Model * model, + const YAML::Node & config); /** * @brief Helper function check if this model is part of the contact, and @@ -107,22 +109,22 @@ class ModelPlugin : public FlatlandPlugin { * is returned, none of the entity, this_fixture, other_fixture pointers will * be populated */ - bool FilterContact(b2Contact *contact, Entity *&entity, - b2Fixture *&this_fixture, b2Fixture *&other_fixture); + bool FilterContact( + b2Contact * contact, Entity *& entity, b2Fixture *& this_fixture, b2Fixture *& other_fixture); /** * @brief Helper function check if this model is part of the contact * @param[in] contact Box2D contact * @return True or false depending on if this model is involved */ - bool FilterContact(b2Contact *contact); + bool FilterContact(b2Contact * contact); - protected: +protected: /** * @brief Model plugin default constructor */ ModelPlugin() = default; }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_MODEL_PLUGIN_H diff --git a/flatland_server/include/flatland_server/plugin_manager.h b/flatland_server/include/flatland_server/plugin_manager.h index 8e2c70f6..aa908c47 100644 --- a/flatland_server/include/flatland_server/plugin_manager.h +++ b/flatland_server/include/flatland_server/plugin_manager.h @@ -53,22 +53,25 @@ #include #include #include -#include #include -namespace flatland_server { +#include + +namespace flatland_server +{ // forward declaration class World; -class PluginManager { - public: +class PluginManager +{ +public: rclcpp::Node::SharedPtr node_; std::vector> model_plugins_; - pluginlib::ClassLoader *model_plugin_loader_; + pluginlib::ClassLoader * model_plugin_loader_; std::vector> world_plugins_; - pluginlib::ClassLoader *world_plugin_loader_; + pluginlib::ClassLoader * world_plugin_loader_; /** * @brief Plugin manager constructor */ @@ -83,19 +86,19 @@ class PluginManager { * @brief This method is called before the Box2D physics step * @param[in] timekeeper provide time related information */ - void BeforePhysicsStep(const Timekeeper &timekeeper); + void BeforePhysicsStep(const Timekeeper & timekeeper); /** * @brief This method is called after the Box2D physics step * @param[in] timekeeper provide time related information */ - void AfterPhysicsStep(const Timekeeper &timekeeper); + void AfterPhysicsStep(const Timekeeper & timekeeper); /** * @brief This method removes all model plugins associated with a given mode * @param[in] The model plugins is associated to */ - void DeleteModelPlugin(Model *model); + void DeleteModelPlugin(Model * model); /** * @brief Load model plugins @@ -103,42 +106,41 @@ class PluginManager { * @param[in] plugin_reader The YAML reader with node containing the plugin * parameter */ - void LoadModelPlugin(Model *model, YamlReader &plugin_reader); + void LoadModelPlugin(Model * model, YamlReader & plugin_reader); /* * @brief load world plugins * @param[in] world, the world that thsi plugin is tied to * @param[in] plugin_reader, the YAML reader with node containing the plugin * @param[in] world_config, the yaml reader of world.yaml - */ - void LoadWorldPlugin(World *world, YamlReader &plugin_reader, - YamlReader &world_config); + */ + void LoadWorldPlugin(World * world, YamlReader & plugin_reader, YamlReader & world_config); /** * @brief Method called for a box2D begin contact * @param[in] contact Box2D contact information */ - void BeginContact(b2Contact *contact); + void BeginContact(b2Contact * contact); /** * @brief Method called for a box2D end contact * @param[in] contact Box2D contact information */ - void EndContact(b2Contact *contact); + void EndContact(b2Contact * contact); /** * @brief Method called for Box2D presolve * @param[in] contact Box2D contact information * @param[in] oldManifold The manifold from the previous timestep */ - void PreSolve(b2Contact *contact, const b2Manifold *oldManifold); + void PreSolve(b2Contact * contact, const b2Manifold * oldManifold); /** * @brief Method called for Box2D Postsolve * @param[in] contact Box2D contact information * @param[in] impulse The calculated impulse from the collision resolute */ - void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse); + void PostSolve(b2Contact * contact, const b2ContactImpulse * impulse); }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_PLUGIN_MANAGER_H diff --git a/flatland_server/include/flatland_server/service_manager.h b/flatland_server/include/flatland_server/service_manager.h index 31ab616c..d054eac5 100644 --- a/flatland_server/include/flatland_server/service_manager.h +++ b/flatland_server/include/flatland_server/service_manager.h @@ -57,7 +57,8 @@ #ifndef FLATLAND_PLUGIN_SERVICE_MANAGER_H #define FLATLAND_PLUGIN_SERVICE_MANAGER_H -namespace flatland_server { +namespace flatland_server +{ class SimulationManager; @@ -65,34 +66,35 @@ class SimulationManager; * This class contains a collection of ROS services that the user may use * to work with the simulation */ -class ServiceManager { - public: - World *world_; ///< a handle to the simulation world +class ServiceManager +{ +public: + World * world_; ///< a handle to the simulation world rclcpp::Node::SharedPtr node_; - SimulationManager *sim_man_; ///< a handle to the simulation manager + SimulationManager * sim_man_; ///< a handle to the simulation manager rclcpp::Service::SharedPtr - change_rate_service_; ///< service for changing the simulation rate + change_rate_service_; ///< service for changing the simulation rate rclcpp::Service::SharedPtr - spawn_model_service_; ///< service for spawning models + spawn_model_service_; ///< service for spawning models rclcpp::Service::SharedPtr - delete_model_service_; ///< service for deleting models + delete_model_service_; ///< service for deleting models rclcpp::Service::SharedPtr - move_model_service_; ///< service for moving models + move_model_service_; ///< service for moving models rclcpp::Service::SharedPtr - pause_service_; ///< service for pausing the simulation + pause_service_; ///< service for pausing the simulation rclcpp::Service::SharedPtr - resume_service_; ///< service for resuming the simulation + resume_service_; ///< service for resuming the simulation rclcpp::Service::SharedPtr - toggle_pause_service_; ///< service for toggling the - /// pause state of the simulation + toggle_pause_service_; ///< service for toggling the + /// pause state of the simulation /** * @brief Service manager constructor * @param[in] sim_man A handle to the simulation manager * @param[in] world A handle to the simulation world */ - ServiceManager(SimulationManager *sim_man, World *world); + ServiceManager(SimulationManager * sim_man, World * world); /** * @brief Callback for the change rate service @@ -101,9 +103,9 @@ class ServiceManager { * @param[in/out] response Contains the response for the service */ bool ChangeRate( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the spawn model service @@ -112,9 +114,9 @@ class ServiceManager { * @param[in/out] response Contains the response for the service */ bool SpawnModel( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the delete model service @@ -123,9 +125,9 @@ class ServiceManager { * @param[in/out] response Contains the response for the service */ bool DeleteModel( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the move model service @@ -134,30 +136,33 @@ class ServiceManager { * @param[in/out] response Contains the response for the service */ bool MoveModel( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the pause service */ - bool Pause(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + bool Pause( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the resume service */ - bool Resume(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + bool Resume( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); /** * @brief Callback for the pause toggle service */ - bool TogglePause(const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response); + bool TogglePause( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); }; } // namespace flatland_server #endif diff --git a/flatland_server/include/flatland_server/simulation_manager.h b/flatland_server/include/flatland_server/simulation_manager.h index 229a75d9..eef4e6d8 100644 --- a/flatland_server/include/flatland_server/simulation_manager.h +++ b/flatland_server/include/flatland_server/simulation_manager.h @@ -55,13 +55,15 @@ #include #include -namespace flatland_server { +namespace flatland_server +{ -class SimulationManager { - public: +class SimulationManager +{ +public: std::shared_ptr node_; bool run_simulator_; ///< While true, keep running the sim loop - World* world_; ///< Simulation world + World * world_; ///< Simulation world double update_rate_; ///< sim loop rate double step_size_; ///< step size bool show_viz_; ///< flag to determine if to show visualization @@ -77,9 +79,9 @@ class SimulationManager { * @param[in] viz_pub_rate rate to publish visualization * behaving ones */ - SimulationManager(std::shared_ptr node, - std::string world_yaml_file, double update_rate, - double step_size, bool show_viz, double viz_pub_rate); + SimulationManager( + std::shared_ptr node, std::string world_yaml_file, double update_rate, + double step_size, bool show_viz, double viz_pub_rate); ~SimulationManager(); @@ -95,8 +97,8 @@ class SimulationManager { void setUpdateRate(double update_rate); - private: - rclcpp::WallRate* rate_; +private: + rclcpp::WallRate * rate_; }; } // namespace flatland_server #endif // FLATLAND_SERVER_SIMULATION_MANAGER_H diff --git a/flatland_server/include/flatland_server/timekeeper.h b/flatland_server/include/flatland_server/timekeeper.h index b2416d6d..533a485a 100644 --- a/flatland_server/include/flatland_server/timekeeper.h +++ b/flatland_server/include/flatland_server/timekeeper.h @@ -47,20 +47,23 @@ #ifndef FLATLAND_SERVER_TIME_KEEPER_H #define FLATLAND_SERVER_TIME_KEEPER_H -#include #include +#include #include #include -namespace flatland_server { +namespace flatland_server +{ -class Timekeeper { - public: - rclcpp::Publisher::SharedPtr clock_pub_; ///< the topic to publish the clock - rclcpp::Node::SharedPtr node_; /// ROS node parent - rclcpp::Time time_; ///< simulation time - double max_step_size_; ///< maximum step size - const std::string clock_topic_; ///< the name of the clock topic +class Timekeeper +{ +public: + rclcpp::Publisher::SharedPtr + clock_pub_; ///< the topic to publish the clock + rclcpp::Node::SharedPtr node_; /// ROS node parent + rclcpp::Time time_; ///< simulation time + double max_step_size_; ///< maximum step size + const std::string clock_topic_; ///< the name of the clock topic /** * @brief constructor @@ -86,7 +89,7 @@ class Timekeeper { /** * @return The current simulation time */ - const rclcpp::Time& GetSimTime() const; + const rclcpp::Time & GetSimTime() const; /** * @return The current step size used for the world @@ -98,5 +101,5 @@ class Timekeeper { */ double GetMaxStepSize() const; }; -} //namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_TIME_KEEPER_H diff --git a/flatland_server/include/flatland_server/types.h b/flatland_server/include/flatland_server/types.h index 433c7545..f5112581 100644 --- a/flatland_server/include/flatland_server/types.h +++ b/flatland_server/include/flatland_server/types.h @@ -50,13 +50,16 @@ #ifndef FLATLAND_SERVER_TYPES_H #define FLATLAND_SERVER_TYPES_H -namespace flatland_server { +namespace flatland_server +{ -struct Vec2 { +struct Vec2 +{ double x; double y; - Vec2(double x, double y) { + Vec2(double x, double y) + { this->x = x; this->y = y; } @@ -66,33 +69,39 @@ struct Vec2 { b2Vec2 Box2D() const { return b2Vec2(x, y); } }; -struct LineSegment { +struct LineSegment +{ Vec2 start; Vec2 end; - LineSegment(const Vec2 &start, const Vec2 &end) { + LineSegment(const Vec2 & start, const Vec2 & end) + { this->start = start; this->end = end; } - LineSegment() { + LineSegment() + { this->start = Vec2(0, 0); this->end = Vec2(0, 0); } }; -struct Pose { +struct Pose +{ double x; double y; double theta; ///< theta - Pose(double x, double y, double theta) { + Pose(double x, double y, double theta) + { this->x = x; this->y = y; this->theta = theta; } - Pose(const std::array &p) { + Pose(const std::array & p) + { this->x = p[0]; this->y = p[1]; this->theta = p[3]; @@ -100,38 +109,37 @@ struct Pose { Pose() : x(0), y(0), theta(0) {} - bool operator==(const Pose &p) const { - return x == p.x && y == p.y && theta == p.theta; - } + bool operator==(const Pose & p) const { return x == p.x && y == p.y && theta == p.theta; } - bool operator!=(const Pose &p) const { return !operator==(p); } + bool operator!=(const Pose & p) const { return !operator==(p); } }; -struct Color { +struct Color +{ double r, g, b, a; Color() : r(0), g(0), b(0), a(0) {} - Color(double r, double g, double b, double a) { + Color(double r, double g, double b, double a) + { this->r = r; this->g = g; this->b = b; this->a = a; } - Color(const std::array &c) { + Color(const std::array & c) + { this->r = c[0]; this->g = c[1]; this->b = c[2]; this->a = c[3]; } - bool operator==(const Color &c) const { - return r == c.r && g == c.g && b == c.b && a == c.a; - } + bool operator==(const Color & c) const { return r == c.r && g == c.g && b == c.b && a == c.a; } - bool operator!=(const Color &c) const { return !operator==(c); } + bool operator!=(const Color & c) const { return !operator==(c); } }; -} +} // namespace flatland_server #endif diff --git a/flatland_server/include/flatland_server/world.h b/flatland_server/include/flatland_server/world.h index 28a9398f..ee2e02b3 100644 --- a/flatland_server/include/flatland_server/world.h +++ b/flatland_server/include/flatland_server/world.h @@ -54,36 +54,38 @@ #include #include #include -#include + #include -#include +#include #include #include -namespace flatland_server { +namespace flatland_server +{ /** * This class defines a world in the simulation. A world contains layers * that can represent environments at multiple levels, and models which are * can be robots or obstacles. */ -class World : public b2ContactListener { - public: - boost::filesystem::path world_yaml_dir_; /// node_; - b2World *physics_world_; ///< Box2D physics world - b2Vec2 gravity_; ///< Box2D world gravity, always (0, 0) + b2World * physics_world_; ///< Box2D physics world + b2Vec2 gravity_; ///< Box2D world gravity, always (0, 0) std::map, Layer *> - layers_name_map_; ///< map of all layers and thier name - std::vector layers_; ///< list of layers - std::vector models_; ///< list of models - CollisionFilterRegistry cfr_; ///< collision registry for layers and models - PluginManager plugin_manager_; ///< for loading and updating plugins - bool service_paused_; ///< indicates if simulation is paused by a service - /// call or not - InteractiveMarkerManager int_marker_manager_; ///< for dynamically moving models from Rviz - int physics_position_iterations_; ///< Box2D solver param - int physics_velocity_iterations_; ///< Box2D solver param + layers_name_map_; ///< map of all layers and thier name + std::vector layers_; ///< list of layers + std::vector models_; ///< list of models + CollisionFilterRegistry cfr_; ///< collision registry for layers and models + PluginManager plugin_manager_; ///< for loading and updating plugins + bool service_paused_; ///< indicates if simulation is paused by a service + /// call or not + InteractiveMarkerManager int_marker_manager_; ///< for dynamically moving models from Rviz + int physics_position_iterations_; ///< Box2D solver param + int physics_velocity_iterations_; ///< Box2D solver param /** * @brief Constructor for the world class. All data required for @@ -101,53 +103,52 @@ class World : public b2ContactListener { * @brief trigger world update include all physics and plugins * @param[in] timekeeper The time keeping object */ - void Update(Timekeeper &timekeeper); + void Update(Timekeeper & timekeeper); /** * @brief Box2D inherited begin contact * @param[in] contact Box2D contact information */ - void BeginContact(b2Contact *contact) override; + void BeginContact(b2Contact * contact) override; /** * @brief Box2D inherited end contact * @param[in] contact Box2D contact information */ - void EndContact(b2Contact *contact) override; + void EndContact(b2Contact * contact) override; /** * @brief Box2D inherited presolve * @param[in] contact Box2D contact information * @param[in] oldManifold The manifold from the previous timestep */ - void PreSolve(b2Contact *contact, const b2Manifold *oldManifold); + void PreSolve(b2Contact * contact, const b2Manifold * oldManifold); /** * @brief Box2D inherited pre solve * @param[in] contact Box2D contact information * @param[in] impulse The calculated impulse from the collision resolute */ - void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse); + void PostSolve(b2Contact * contact, const b2ContactImpulse * impulse); /* * @brief Load world plugins * @param[in] world_plugin_reader, readin the info about the plugin * @param[in] world, the world where the plugin will be applied to * @param[in] world config, the yaml reader of world.yaml - */ - void LoadWorldPlugins(YamlReader &world_plugin_reader, World *world, - YamlReader &world_config); + */ + void LoadWorldPlugins(YamlReader & world_plugin_reader, World * world, YamlReader & world_config); /** * @brief load layers into the world. Throws YAMLException. * @param[in] layers_reader Yaml reader for node that has list of layers */ - void LoadLayers(YamlReader &layers_reader); + void LoadLayers(YamlReader & layers_reader); /** * @brief load models into the world. Throws YAMLException. * @param[in] layers_reader Yaml reader for node that has a list of models */ - void LoadModels(YamlReader &models_reader); + void LoadModels(YamlReader & models_reader); /** * @brief load models into the world. Throws YAMLException. @@ -156,21 +157,22 @@ class World : public b2ContactListener { * @param[in] name Name of the model * @param[in] pose Initial pose of the model in x, y, yaw */ - void LoadModel(const std::string &model_yaml_path, const std::string &ns, - const std::string &name, const Pose &pose); + void LoadModel( + const std::string & model_yaml_path, const std::string & ns, const std::string & name, + const Pose & pose); /** * @brief remove model with a given name * @param[in] name The name of the model to remove */ - void DeleteModel(const std::string &name); + void DeleteModel(const std::string & name); /** * @brief move model with a given name * @param[in] name The name of the model to move * @param[in] pose The desired new pose of the model */ - void MoveModel(const std::string &name, const Pose &pose); + void MoveModel(const std::string & name, const Pose & pose); /** * @brief set the paused state of the simulation to true @@ -200,7 +202,7 @@ class World : public b2ContactListener { * @param[in] yaml_path Path to the world yaml file * @return pointer to a new world */ - static World *MakeWorld(std::shared_ptr node, const std::string &yaml_path); + static World * MakeWorld(std::shared_ptr node, const std::string & yaml_path); /** * @brief Publish debug visualizations for everything @@ -209,5 +211,5 @@ class World : public b2ContactListener { */ void DebugVisualize(bool update_layers = true); }; -} // namespace flatland_server +} // namespace flatland_server #endif // FLATLAND_SERVER_WORLD_H diff --git a/flatland_server/include/flatland_server/world_plugin.h b/flatland_server/include/flatland_server/world_plugin.h index a13b5497..2d00164b 100644 --- a/flatland_server/include/flatland_server/world_plugin.h +++ b/flatland_server/include/flatland_server/world_plugin.h @@ -51,45 +51,48 @@ #include #include #include -#include #include + +#include #include -namespace flatland_server { +namespace flatland_server +{ // forward declaration class World; -class WorldPlugin : public FlatlandPlugin { - protected: - World *world_; +class WorldPlugin : public FlatlandPlugin +{ +protected: + World * world_; rclcpp::Node::SharedPtr node_; - public: +public: /* - * @brief WorldPlugin default constructor - */ + * @brief WorldPlugin default constructor + */ WorldPlugin() = default; /* - * @brief initialize the plugin - * @param[in] node, the rclcpp node pointer - * @param[in] world, the World the plugin is attached to - * @param[in] name, name of the plugin - * @param[in] type, type of the plugin - * @param[in] plugin_reader, the YAML node contain the plugin's config - * @param[in] world_config, the yaml reader of world.yaml - */ - void Initialize(rclcpp::Node::SharedPtr node, World *world, std::string name, std::string type, - YAML::Node &plugin_reader, YamlReader &world_config); + * @brief initialize the plugin + * @param[in] node, the rclcpp node pointer + * @param[in] world, the World the plugin is attached to + * @param[in] name, name of the plugin + * @param[in] type, type of the plugin + * @param[in] plugin_reader, the YAML node contain the plugin's config + * @param[in] world_config, the yaml reader of world.yaml + */ + void Initialize( + rclcpp::Node::SharedPtr node, World * world, std::string name, std::string type, + YAML::Node & plugin_reader, YamlReader & world_config); /* - * @brief trigger after plugin is initialized - * - * @param[in] plugin_reader, the YAML node contain the plugin's config - * @param[in] world_config, the yaml reader of world.yaml - */ - virtual void OnInitialize(const YAML::Node &plugin_reader, YamlReader &world_config) = 0; - + * @brief trigger after plugin is initialized + * + * @param[in] plugin_reader, the YAML node contain the plugin's config + * @param[in] world_config, the yaml reader of world.yaml + */ + virtual void OnInitialize(const YAML::Node & plugin_reader, YamlReader & world_config) = 0; }; -} +} // namespace flatland_server #endif // FLATLAND_SERVER_WORLD_PLUGIN_H diff --git a/flatland_server/include/flatland_server/yaml_preprocessor.h b/flatland_server/include/flatland_server/yaml_preprocessor.h index dc47cda4..2da866a9 100644 --- a/flatland_server/include/flatland_server/yaml_preprocessor.h +++ b/flatland_server/include/flatland_server/yaml_preprocessor.h @@ -48,23 +48,25 @@ #define FLATLAND_SERVER_YAML_PREPROCESSOR_H #include -#include -#include - #include #include #include +#include + +#include #include #include #include -namespace flatland_server { +namespace flatland_server +{ /** */ -class YamlPreprocessor { - public: - std::shared_ptr ros_node_; +class YamlPreprocessor +{ +public: + std::shared_ptr ros_node_; /* * @brief YamlPreprocessor constructor for yaml with lua expressions @@ -73,48 +75,48 @@ class YamlPreprocessor { */ YamlPreprocessor(std::shared_ptr ros_node) : ros_node_(ros_node) {} -/** - * @brief Preprocess with a given node - - * @param[in/out] node A Yaml node to parse - * @return The parsed YAML::Node - */ -void Parse(YAML::Node &node); + /** + * @brief Preprocess with a given node -/** - * @brief Constructor with a given path to a yaml file, throws exception on - * failure - * @param[in] path Path to the yaml file - * @return The parsed YAML::Node - */ -YAML::Node LoadParse(const std::string &path); + * @param[in/out] node A Yaml node to parse + * @return The parsed YAML::Node + */ + void Parse(YAML::Node & node); -/** - * @brief Find and run any $eval nodes - * @param[in/out] node A Yaml node to recursively parse - */ -void ProcessNodes(YAML::Node &node); + /** + * @brief Constructor with a given path to a yaml file, throws exception on + * failure + * @param[in] path Path to the yaml file + * @return The parsed YAML::Node + */ + YAML::Node LoadParse(const std::string & path); -/** - * @brief Find and run any $eval expressions - * @param[in/out] ros_node the current ROS node for rosparam loading - * @param[in/out] node A Yaml string node to parse - */ -void ProcessScalarNode(YAML::Node &node); + /** + * @brief Find and run any $eval nodes + * @param[in/out] node A Yaml node to recursively parse + */ + void ProcessNodes(YAML::Node & node); -/** - * @brief Get an environment variable with an optional default value - * @param[in/out] lua_State The lua state/stack to read/write to/from - */ -static int LuaGetEnv(lua_State *L); + /** + * @brief Find and run any $eval expressions + * @param[in/out] ros_node the current ROS node for rosparam loading + * @param[in/out] node A Yaml string node to parse + */ + void ProcessScalarNode(YAML::Node & node); -/** - * @brief Get a rosparam with an optional default value - * @param[in/out] ros_node the current ROS node for rosparam loading - * @param[in/out] lua_State The lua state/stack to read/write to/from - */ -static int LuaGetParam(lua_State *L); + /** + * @brief Get an environment variable with an optional default value + * @param[in/out] lua_State The lua state/stack to read/write to/from + */ + static int LuaGetEnv(lua_State * L); + + /** + * @brief Get a rosparam with an optional default value + * @param[in/out] ros_node the current ROS node for rosparam loading + * @param[in/out] lua_State The lua state/stack to read/write to/from + */ + static int LuaGetParam(lua_State * L); }; -} +} // namespace flatland_server #endif // FLATLAND_SERVER_YAML_PREPROCESSOR_H diff --git a/flatland_server/include/flatland_server/yaml_reader.h b/flatland_server/include/flatland_server/yaml_reader.h index 38070c9c..34e3cf23 100644 --- a/flatland_server/include/flatland_server/yaml_reader.h +++ b/flatland_server/include/flatland_server/yaml_reader.h @@ -50,12 +50,13 @@ #include #include #include +#include #include + #include #include #include #include -#include // If we have a version of boost with type_index #if BOOST_VERSION / 100 % 1000 >= 56 @@ -71,17 +72,19 @@ #include #include -namespace flatland_server { +namespace flatland_server +{ /** */ -class YamlReader { - public: +class YamlReader +{ +public: enum NodeTypeCheck { MAP, LIST, NO_CHECK }; - YAML::Node node_; ///< The YAML Node this processes + YAML::Node node_; ///< The YAML Node this processes std::shared_ptr ros_node_; - YamlPreprocessor yaml_preprocessor_; + YamlPreprocessor yaml_preprocessor_; std::set accessed_keys_; /// Records of the keys processed ///< location of the entry, used to show where the error come from std::string filename_; @@ -102,14 +105,14 @@ class YamlReader { * @brief Constructor with a given node * @param[in] node A Yaml node to get data from */ - YamlReader(std::shared_ptr ros_node, const YAML::Node &node); + YamlReader(std::shared_ptr ros_node, const YAML::Node & node); /** * @brief Constructor with a given path to a yaml file, throws exception on * failure * @param[in] path Path to the yaml file */ - YamlReader(std::shared_ptr ros_node, const std::string &path); + YamlReader(std::shared_ptr ros_node, const std::string & path); /** * @brief Use this method to set the entry location and entry name for error @@ -126,7 +129,7 @@ class YamlReader { * @param[in] file_path path to the file, use "_NONE_" for empty string, * file_path with empty string will keep current path */ - void SetFile(const std::string &file_path); + void SetFile(const std::string & file_path); /** * @brief This method checks all keys in the yaml node are used, otherwise it @@ -159,8 +162,7 @@ class YamlReader { * using its parents entry location and entry name. It also accepts "_NONE_" * @return YamlReader of the sub node */ - YamlReader Subnode(int index, NodeTypeCheck type_check, - std::string sub_node_location = ""); + YamlReader Subnode(int index, NodeTypeCheck type_check, std::string sub_node_location = ""); /** * @brief Get one of the subnode using a key, throws exception on failure, * file path is inherited from the parent @@ -171,8 +173,8 @@ class YamlReader { * using its parents entry location and entry name. It also accepts "_NONE_" * @return YamlReader of the sub node */ - YamlReader Subnode(const std::string &key, NodeTypeCheck type_check, - std::string sub_node_location = ""); + YamlReader Subnode( + const std::string & key, NodeTypeCheck type_check, std::string sub_node_location = ""); /** * @brief Optionally get one of the subnode using a key, throws exception on * failure, file path is inherited from the parent @@ -184,8 +186,8 @@ class YamlReader { * @return YamlReader of the sub node, or a YamlReader of a empty node if a * node with the given key does not exist */ - YamlReader SubnodeOpt(const std::string &key, NodeTypeCheck type_check, - std::string sub_node_location = ""); + YamlReader SubnodeOpt( + const std::string & key, NodeTypeCheck type_check, std::string sub_node_location = ""); /** * @brief Convert the current node in yaml reader to a given template type. It * uses yaml-cpp's as() function, you could specify conversion for custom @@ -219,7 +221,7 @@ class YamlReader { * @return Value of the converted subnode */ template - T Get(const std::string &key); + T Get(const std::string & key); /** * @brief Optionally get subnode with a given key and converted to the given @@ -230,7 +232,7 @@ class YamlReader { * does not exist */ template - T Get(const std::string &key, const T &default_val); + T Get(const std::string & key, const T & default_val); /** * @brief Get subnode with a given key and converted to list of the given @@ -241,7 +243,7 @@ class YamlReader { * @return Value of the converted subnode */ template - std::vector GetList(const std::string &key, int min_size, int max_size); + std::vector GetList(const std::string & key, int min_size, int max_size); /** * @brief Optionally get subnode with a given key and converted to list of the @@ -253,9 +255,8 @@ class YamlReader { * @return Value of the converted subnode */ template - std::vector GetList(const std::string &key, - const std::vector default_val, int min_size, - int max_size); + std::vector GetList( + const std::string & key, const std::vector default_val, int min_size, int max_size); /** * @brief Get subnode with a given key and converted to array of the given @@ -264,7 +265,7 @@ class YamlReader { * @return Value of the converted subnode */ template - std::array GetArray(const std::string &key); + std::array GetArray(const std::string & key); /** * @brief Optionally get subnode with a given key and converted to array of @@ -274,53 +275,52 @@ class YamlReader { * @return Value of the converted subnode */ template - std::array GetArray(const std::string &key, - const std::array default_val); + std::array GetArray(const std::string & key, const std::array default_val); /** * @return A Vec2 accessed by a given key */ - Vec2 GetVec2(const std::string &key); + Vec2 GetVec2(const std::string & key); /** * @return A Vec2 accessed by a given key, or default value if key does not * exist */ - Vec2 GetVec2(const std::string &key, const Vec2 &default_val); + Vec2 GetVec2(const std::string & key, const Vec2 & default_val); /** * @return A Color value accessed by a given key, or default value if key does * not exist */ - Color GetColor(const std::string &key, const Color &default_val); + Color GetColor(const std::string & key, const Color & default_val); /** * @return A Pose value accessed by a given key */ - Pose GetPose(const std::string &key); + Pose GetPose(const std::string & key); /** * @return A Pose value accessed by a given key, or default value if key does * not exist */ - Pose GetPose(const std::string &key, const Pose &default_val); + Pose GetPose(const std::string & key, const Pose & default_val); }; /** * @return A string with quotes around the input */ -inline std::string Q(const std::string &str) { return "\"" + str + "\""; } +inline std::string Q(const std::string & str) { return "\"" + str + "\""; } template -T YamlReader::As() { +T YamlReader::As() +{ T ret; try { ret = node_.as(); - } catch (const YAML::RepresentationException &e) { - throw YAMLException("Error converting entry" + fmt_name_ + " to " + - TYPESTRING(T) + fmt_in_); - } catch (const YAML::Exception &e) { + } catch (const YAML::RepresentationException & e) { + throw YAMLException("Error converting entry" + fmt_name_ + " to " + TYPESTRING(T) + fmt_in_); + } catch (const YAML::Exception & e) { throw YAMLException("Error reading entry" + fmt_name_ + fmt_in_); } @@ -328,23 +328,23 @@ T YamlReader::As() { } template -std::vector YamlReader::AsList(int min_size, int max_size) { +std::vector YamlReader::AsList(int min_size, int max_size) +{ std::vector list; - if (min_size > 0 && max_size > 0 && min_size == max_size && - NodeSize() != max_size) { - throw YAMLException("Entry" + fmt_name_ + " must have size of exactly " + - std::to_string(min_size) + fmt_in_); + if (min_size > 0 && max_size > 0 && min_size == max_size && NodeSize() != max_size) { + throw YAMLException( + "Entry" + fmt_name_ + " must have size of exactly " + std::to_string(min_size) + fmt_in_); } if (min_size > 0 && NodeSize() < min_size) { - throw YAMLException("Entry" + fmt_name_ + " must have size >= " + - std::to_string(min_size) + fmt_in_); + throw YAMLException( + "Entry" + fmt_name_ + " must have size >= " + std::to_string(min_size) + fmt_in_); } if (max_size > 0 && NodeSize() > max_size) { - throw YAMLException("Entry" + fmt_name_ + " must have size <= " + - std::to_string(max_size) + fmt_in_); + throw YAMLException( + "Entry" + fmt_name_ + " must have size <= " + std::to_string(max_size) + fmt_in_); } for (int i = 0; i < NodeSize(); i++) { @@ -355,7 +355,8 @@ std::vector YamlReader::AsList(int min_size, int max_size) { } template -std::array YamlReader::AsArray() { +std::array YamlReader::AsArray() +{ std::vector list_ret = AsList(N, N); std::array array_ret; @@ -366,12 +367,14 @@ std::array YamlReader::AsArray() { } template -T YamlReader::Get(const std::string &key) { +T YamlReader::Get(const std::string & key) +{ return Subnode(key, NO_CHECK).As(); } template -T YamlReader::Get(const std::string &key, const T &default_val) { +T YamlReader::Get(const std::string & key, const T & default_val) +{ if (!node_[key]) { accessed_keys_.insert(key); return default_val; @@ -380,15 +383,15 @@ T YamlReader::Get(const std::string &key, const T &default_val) { } template -std::vector YamlReader::GetList(const std::string &key, int min_size, - int max_size) { +std::vector YamlReader::GetList(const std::string & key, int min_size, int max_size) +{ return Subnode(key, LIST).AsList(min_size, max_size); } template -std::vector YamlReader::GetList(const std::string &key, - const std::vector default_val, - int min_size, int max_size) { +std::vector YamlReader::GetList( + const std::string & key, const std::vector default_val, int min_size, int max_size) +{ if (!node_[key]) { accessed_keys_.insert(key); return default_val; @@ -398,13 +401,14 @@ std::vector YamlReader::GetList(const std::string &key, } template -std::array YamlReader::GetArray(const std::string &key) { +std::array YamlReader::GetArray(const std::string & key) +{ return Subnode(key, LIST).AsArray(); } template -std::array YamlReader::GetArray(const std::string &key, - const std::array default_val) { +std::array YamlReader::GetArray(const std::string & key, const std::array default_val) +{ if (!node_[key]) { accessed_keys_.insert(key); return default_val; @@ -412,14 +416,17 @@ std::array YamlReader::GetArray(const std::string &key, return GetArray(key); } -} +} // namespace flatland_server // encode and decode functions for yaml-cpp to convert values for commonly used // types in flatland server -namespace YAML { +namespace YAML +{ template <> -struct convert { - static bool decode(const Node &node, b2Vec2 &rhs) { +struct convert +{ + static bool decode(const Node & node, b2Vec2 & rhs) + { if (!node.IsSequence() || node.size() != 2) { return false; } @@ -431,8 +438,10 @@ struct convert { }; template <> -struct convert { - static bool decode(const Node &node, flatland_server::Vec2 &rhs) { +struct convert +{ + static bool decode(const Node & node, flatland_server::Vec2 & rhs) + { if (!node.IsSequence() || node.size() != 2) { return false; } @@ -444,8 +453,10 @@ struct convert { }; template <> -struct convert { - static bool decode(const Node &node, flatland_server::Color &rhs) { +struct convert +{ + static bool decode(const Node & node, flatland_server::Color & rhs) + { if (!node.IsSequence() || node.size() != 4) { return false; } @@ -459,8 +470,10 @@ struct convert { }; template <> -struct convert { - static bool decode(const Node &node, flatland_server::Pose &rhs) { +struct convert +{ + static bool decode(const Node & node, flatland_server::Pose & rhs) + { if (!node.IsSequence() || node.size() != 3) { return false; } @@ -471,6 +484,6 @@ struct convert { return true; } }; -} +} // namespace YAML #endif diff --git a/flatland_server/src/body.cpp b/flatland_server/src/body.cpp index cf164bf5..dec08bf1 100644 --- a/flatland_server/src/body.cpp +++ b/flatland_server/src/body.cpp @@ -45,15 +45,18 @@ */ #include + #include -namespace flatland_server { +namespace flatland_server +{ -Body::Body(b2World *physics_world, Entity *entity, const std::string &name, - const Color &color, const Pose &pose, b2BodyType body_type, - const YAML::Node &properties, double linear_damping, - double angular_damping) - : entity_(entity), name_(name), color_(color), properties_(properties) { +Body::Body( + b2World * physics_world, Entity * entity, const std::string & name, const Color & color, + const Pose & pose, b2BodyType body_type, const YAML::Node & properties, double linear_damping, + double angular_damping) +: entity_(entity), name_(name), color_(color), properties_(properties) +{ b2BodyDef body_def; body_def.type = body_type; body_def.position.Set(pose.x, pose.y); @@ -65,42 +68,45 @@ Body::Body(b2World *physics_world, Entity *entity, const std::string &name, physics_body_->SetUserData(this); } -Body::~Body() { +Body::~Body() +{ if (physics_body_) { physics_body_->GetWorld()->DestroyBody(physics_body_); } } -int Body::GetFixturesCount() const { +int Body::GetFixturesCount() const +{ int count = 0; - for (b2Fixture *f = physics_body_->GetFixtureList(); f; f = f->GetNext()) { + for (b2Fixture * f = physics_body_->GetFixtureList(); f; f = f->GetNext()) { count++; } return count; } -Entity *Body::GetEntity() { return entity_; } +Entity * Body::GetEntity() { return entity_; } -const std::string &Body::GetName() const { return name_; } +const std::string & Body::GetName() const { return name_; } -b2Body *Body::GetPhysicsBody() { return physics_body_; } +b2Body * Body::GetPhysicsBody() { return physics_body_; } -const Color &Body::GetColor() const { return color_; } +const Color & Body::GetColor() const { return color_; } -void Body::SetColor(const Color &color) { color_ = color; } +void Body::SetColor(const Color & color) { color_ = color; } -void Body::DebugOutput() const { +void Body::DebugOutput() const +{ // todo - RCLCPP_DEBUG(rclcpp::get_logger("Body"), - "Body %p: entity(%p, %s) name(%s) color(%f,%f,%f,%f) " - "physics_body(%p) num_fixtures(%d) type(%d) pose(%f, %f, %f) " - "angular_damping(%f) linear_damping(%f)", - this, entity_, entity_->name_.c_str(), name_.c_str(), color_.r, color_.g, - color_.b, color_.a, physics_body_, GetFixturesCount(), - physics_body_->GetType(), physics_body_->GetPosition().x, - physics_body_->GetPosition().y, physics_body_->GetAngle(), - physics_body_->GetAngularDamping(), physics_body_->GetLinearDamping()); + RCLCPP_DEBUG( + rclcpp::get_logger("Body"), + "Body %p: entity(%p, %s) name(%s) color(%f,%f,%f,%f) " + "physics_body(%p) num_fixtures(%d) type(%d) pose(%f, %f, %f) " + "angular_damping(%f) linear_damping(%f)", + this, entity_, entity_->name_.c_str(), name_.c_str(), color_.r, color_.g, color_.b, color_.a, + physics_body_, GetFixturesCount(), physics_body_->GetType(), physics_body_->GetPosition().x, + physics_body_->GetPosition().y, physics_body_->GetAngle(), physics_body_->GetAngularDamping(), + physics_body_->GetLinearDamping()); } -} //namespace flatland_server +} // namespace flatland_server diff --git a/flatland_server/src/collision_filter_registry.cpp b/flatland_server/src/collision_filter_registry.cpp index 26a1aa8e..9c32175a 100644 --- a/flatland_server/src/collision_filter_registry.cpp +++ b/flatland_server/src/collision_filter_registry.cpp @@ -46,31 +46,34 @@ #include -namespace flatland_server { +namespace flatland_server +{ const int CollisionFilterRegistry::LAYER_NOT_EXIST; const int CollisionFilterRegistry::LAYER_ALREADY_EXIST; const int CollisionFilterRegistry::LAYERS_FULL; const int CollisionFilterRegistry::MAX_LAYERS; -CollisionFilterRegistry::CollisionFilterRegistry() - : no_collide_group_cnt_(0), collide_group_cnt_(0) {} +CollisionFilterRegistry::CollisionFilterRegistry() : no_collide_group_cnt_(0), collide_group_cnt_(0) +{ +} -int CollisionFilterRegistry::RegisterCollide() { +int CollisionFilterRegistry::RegisterCollide() +{ collide_group_cnt_++; return collide_group_cnt_; } -int CollisionFilterRegistry::RegisterNoCollide() { +int CollisionFilterRegistry::RegisterNoCollide() +{ no_collide_group_cnt_--; return no_collide_group_cnt_; } -bool CollisionFilterRegistry::IsLayersFull() const { - return layer_id_table_.size() >= MAX_LAYERS; -} +bool CollisionFilterRegistry::IsLayersFull() const { return layer_id_table_.size() >= MAX_LAYERS; } -int CollisionFilterRegistry::RegisterLayer(std::string layer_name) { +int CollisionFilterRegistry::RegisterLayer(std::string layer_name) +{ if (IsLayersFull()) { return LAYERS_FULL; } @@ -99,14 +102,16 @@ int CollisionFilterRegistry::RegisterLayer(std::string layer_name) { return i; } -int CollisionFilterRegistry::LookUpLayerId(std::string layer_name) const { +int CollisionFilterRegistry::LookUpLayerId(std::string layer_name) const +{ if (layer_id_table_.count(layer_name) == 0) { return LAYER_NOT_EXIST; } return layer_id_table_.at(layer_name); } -std::vector CollisionFilterRegistry::GetAllLayers() const { +std::vector CollisionFilterRegistry::GetAllLayers() const +{ std::vector layer_names; std::map::const_iterator it; @@ -117,13 +122,11 @@ std::vector CollisionFilterRegistry::GetAllLayers() const { return layer_names; } -int CollisionFilterRegistry::LayersCount() const { - return layer_id_table_.size(); -} +int CollisionFilterRegistry::LayersCount() const { return layer_id_table_.size(); } uint16_t CollisionFilterRegistry::GetCategoryBits( - const std::vector &layers, - std::vector *invalid_layers) const { + const std::vector & layers, std::vector * invalid_layers) const +{ if (layers.size() == 1 && layers[0] == "all") { return ~((uint16_t)0x0); } @@ -133,7 +136,7 @@ uint16_t CollisionFilterRegistry::GetCategoryBits( } uint16_t category_bits = 0; - for (const auto &layer : layers) { + for (const auto & layer : layers) { int layer_id = LookUpLayerId(layer); if (layer_id < 0 && invalid_layers) { @@ -146,4 +149,4 @@ uint16_t CollisionFilterRegistry::GetCategoryBits( return category_bits; } -} //namespace flatland_server +} // namespace flatland_server diff --git a/flatland_server/src/debug_visualization.cpp b/flatland_server/src/debug_visualization.cpp index 90b9cb64..a5c44323 100644 --- a/flatland_server/src/debug_visualization.cpp +++ b/flatland_server/src/debug_visualization.cpp @@ -56,16 +56,16 @@ #include #include -namespace flatland_server { +namespace flatland_server +{ -DebugVisualization::DebugVisualization(rclcpp::Node::SharedPtr node) - : node_(node) { - topic_list_publisher_ = - node_->create_publisher("topics", 1); +DebugVisualization::DebugVisualization(rclcpp::Node::SharedPtr node) : node_(node) +{ + topic_list_publisher_ = node_->create_publisher("topics", 1); } -std::shared_ptr DebugVisualization::Get( - rclcpp::Node::SharedPtr node) { +std::shared_ptr DebugVisualization::Get(rclcpp::Node::SharedPtr node) +{ static std::shared_ptr instance; // todo: fix this? How should singletons work with shared pointers? if (!instance) { @@ -75,13 +75,16 @@ std::shared_ptr DebugVisualization::Get( } void DebugVisualization::JointToMarkers( - visualization_msgs::msg::MarkerArray& markers, b2Joint* joint, float r, - float g, float b, float a) { - if (joint->GetType() == e_distanceJoint || - joint->GetType() == e_pulleyJoint || joint->GetType() == e_mouseJoint) { - RCLCPP_ERROR(rclcpp::get_logger("DebugVis"), - "Unimplemented visualization joints. See b2World.cpp for " - "implementation"); + visualization_msgs::msg::MarkerArray & markers, b2Joint * joint, float r, float g, float b, + float a) +{ + if ( + joint->GetType() == e_distanceJoint || joint->GetType() == e_pulleyJoint || + joint->GetType() == e_mouseJoint) { + RCLCPP_ERROR( + rclcpp::get_logger("DebugVis"), + "Unimplemented visualization joints. See b2World.cpp for " + "implementation"); return; } @@ -128,9 +131,9 @@ void DebugVisualization::JointToMarkers( } void DebugVisualization::BodyToMarkers( - visualization_msgs::msg::MarkerArray& markers, b2Body* body, float r, - float g, float b, float a) { - b2Fixture* fixture = body->GetFixtureList(); + visualization_msgs::msg::MarkerArray & markers, b2Body * body, float r, float g, float b, float a) +{ + b2Fixture * fixture = body->GetFixtureList(); while (fixture != NULL) { // traverse fixture linked list visualization_msgs::msg::Marker marker; @@ -142,7 +145,7 @@ void DebugVisualization::BodyToMarkers( marker.color.a = a; marker.pose.position.x = body->GetPosition().x; marker.pose.position.y = body->GetPosition().y; - tf2::Quaternion q; // use tf2 to convert 2d yaw -> 3d quaternion + tf2::Quaternion q; // use tf2 to convert 2d yaw -> 3d quaternion q.setRPY(0, 0, body->GetAngle()); // from euler angles: roll, pitch, yaw marker.pose.orientation = tf2::toMsg(q); bool add_marker = true; @@ -150,7 +153,7 @@ void DebugVisualization::BodyToMarkers( // Get the shape from the fixture switch (fixture->GetType()) { case b2Shape::e_circle: { - b2CircleShape* circle = (b2CircleShape*)fixture->GetShape(); + b2CircleShape * circle = (b2CircleShape *)fixture->GetShape(); marker.type = marker.SPHERE_LIST; float diameter = circle->m_radius * 2.0; @@ -166,7 +169,7 @@ void DebugVisualization::BodyToMarkers( } break; case b2Shape::e_polygon: { // Convert b2Polygon -> LINE_STRIP - b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape(); + b2PolygonShape * poly = (b2PolygonShape *)fixture->GetShape(); marker.type = marker.LINE_STRIP; marker.scale.x = 0.03; // 3cm wide lines @@ -182,11 +185,10 @@ void DebugVisualization::BodyToMarkers( case b2Shape::e_edge: { // Convert b2Edge -> LINE_LIST geometry_msgs::msg::Point p; // b2Edge uses vertex1 and 2 for its edges - b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); + b2EdgeShape * edge = (b2EdgeShape *)fixture->GetShape(); // If the last marker is a line list, extend it - if (markers.markers.size() > 0 && - markers.markers.back().type == marker.LINE_LIST) { + if (markers.markers.size() > 0 && markers.markers.back().type == marker.LINE_LIST) { add_marker = false; p.x = edge->m_vertex1.x; p.y = edge->m_vertex1.y; @@ -212,9 +214,9 @@ void DebugVisualization::BodyToMarkers( default: // Unsupported shape rclcpp::Clock steady_clock = rclcpp::Clock(RCL_STEADY_TIME); - RCLCPP_WARN_THROTTLE(rclcpp::get_logger("Debug Visualization"), - steady_clock, 1000, "Unsupported Box2D shape %d", - static_cast(fixture->GetType())); + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("Debug Visualization"), steady_clock, 1000, + "Unsupported Box2D shape %d", static_cast(fixture->GetType())); fixture = fixture->GetNext(); continue; // Do not add broken marker break; @@ -227,12 +229,13 @@ void DebugVisualization::BodyToMarkers( } } -void DebugVisualization::Publish(const Timekeeper& timekeeper) { +void DebugVisualization::Publish(const Timekeeper & timekeeper) +{ // Iterate over the topics_ map as pair(name, topic) std::vector to_delete; - for (auto& topic : topics_) { + for (auto & topic : topics_) { if (!topic.second.needs_publishing) { continue; } @@ -252,19 +255,19 @@ void DebugVisualization::Publish(const Timekeeper& timekeeper) { } if (to_delete.size() > 0) { - for (const auto& topic : to_delete) { - RCLCPP_WARN(rclcpp::get_logger("DebugVis"), "Deleting topic %s", - topic.c_str()); + for (const auto & topic : to_delete) { + RCLCPP_WARN(rclcpp::get_logger("DebugVis"), "Deleting topic %s", topic.c_str()); topics_.erase(topic); } PublishTopicList(); } } -void DebugVisualization::VisualizeLayer(std::string name, Body* body) { +void DebugVisualization::VisualizeLayer(std::string name, Body * body) +{ AddTopicIfNotExist(name); - b2Fixture* fixture = body->physics_body_->GetFixtureList(); + b2Fixture * fixture = body->physics_body_->GetFixtureList(); visualization_msgs::msg::Marker marker; if (fixture == NULL) return; // Nothing to visualize, empty linked list @@ -284,21 +287,19 @@ void DebugVisualization::VisualizeLayer(std::string name, Body* body) { tf2::Quaternion q; // use tf2 to convert 2d yaw -> 3d quaternion q.setRPY(0, 0, - body->physics_body_ - ->GetAngle()); // from euler angles: roll, pitch, yaw + body->physics_body_->GetAngle()); // from euler angles: roll, pitch, yaw marker.pose.orientation = tf2::toMsg(q); marker.type = marker.TRIANGLE_LIST; YamlReader reader(node_, body->properties_); - YamlReader debug_reader = - reader.SubnodeOpt("debug", YamlReader::NodeTypeCheck::MAP); + YamlReader debug_reader = reader.SubnodeOpt("debug", YamlReader::NodeTypeCheck::MAP); float min_z = debug_reader.Get("min_z", 0.0); float max_z = debug_reader.Get("max_z", 1.0); // Get the shape from the fixture if (fixture->GetType() == b2Shape::e_edge) { geometry_msgs::msg::Point p; // b2Edge uses vertex1 and 2 for its edges - b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape(); + b2EdgeShape * edge = (b2EdgeShape *)fixture->GetShape(); p.x = edge->m_vertex1.x; p.y = edge->m_vertex1.y; @@ -334,56 +335,59 @@ void DebugVisualization::VisualizeLayer(std::string name, Body* body) { topics_[name].needs_publishing = true; } -void DebugVisualization::Visualize(std::string name, b2Body* body, float r, - float g, float b, float a) { +void DebugVisualization::Visualize( + std::string name, b2Body * body, float r, float g, float b, float a) +{ AddTopicIfNotExist(name); BodyToMarkers(topics_[name].markers, body, r, g, b, a); topics_[name].needs_publishing = true; } -void DebugVisualization::Visualize(std::string name, b2Joint* joint, float r, - float g, float b, float a) { +void DebugVisualization::Visualize( + std::string name, b2Joint * joint, float r, float g, float b, float a) +{ AddTopicIfNotExist(name); JointToMarkers(topics_[name].markers, joint, r, g, b, a); topics_[name].needs_publishing = true; } -void DebugVisualization::Reset(std::string name) { +void DebugVisualization::Reset(std::string name) +{ if (topics_.count(name) > 0) { // If the topic exists, clear it topics_[name].markers.markers.clear(); topics_[name].needs_publishing = true; } } -void DebugVisualization::AddTopicIfNotExist(const std::string& name) { +void DebugVisualization::AddTopicIfNotExist(const std::string & name) +{ static const rmw_qos_profile_t qos_profile = { - RMW_QOS_POLICY_HISTORY_KEEP_LAST, - 1, - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; // If the topic doesn't exist yet, create it if (topics_.count(name) == 0) { topics_[name] = { - node_->create_publisher( - name, rclcpp::QoS(rclcpp::KeepLast(1), qos_profile)), - true, visualization_msgs::msg::MarkerArray()}; + node_->create_publisher( + name, rclcpp::QoS(rclcpp::KeepLast(1), qos_profile)), + true, visualization_msgs::msg::MarkerArray()}; - RCLCPP_INFO_ONCE(rclcpp::get_logger("Debug Visualization"), - "Visualizing %s", name.c_str()); + RCLCPP_INFO_ONCE(rclcpp::get_logger("Debug Visualization"), "Visualizing %s", name.c_str()); PublishTopicList(); } } -void DebugVisualization::PublishTopicList() { +void DebugVisualization::PublishTopicList() +{ flatland_msgs::msg::DebugTopicList topic_list; - for (auto const& topic_pair : topics_) - topic_list.topics.push_back(topic_pair.first); + for (auto const & topic_pair : topics_) topic_list.topics.push_back(topic_pair.first); topic_list_publisher_->publish(topic_list); } } // namespace flatland_server diff --git a/flatland_server/src/dummy_model_plugin.cpp b/flatland_server/src/dummy_model_plugin.cpp index 4e945bb0..f862c34f 100644 --- a/flatland_server/src/dummy_model_plugin.cpp +++ b/flatland_server/src/dummy_model_plugin.cpp @@ -47,13 +47,16 @@ #include #include #include + #include using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ -void DummyModelPlugin::OnInitialize(const YAML::Node &config) { +void DummyModelPlugin::OnInitialize(const YAML::Node & config) +{ dummy_param_float_ = config["dummy_param_float"].as(); dummy_param_string_ = config["dummy_param_string"].as(); dummy_param_int_ = config["dummy_param_int"].as(); @@ -61,22 +64,22 @@ void DummyModelPlugin::OnInitialize(const YAML::Node &config) { // Dummy plugin has the these values enforced for testing if (std::fabs(dummy_param_float_ - 0.123456) > 1e-7) { throw YAMLException( - "dummy_param_float must be 0.1253456, instead it was \"" + - std::to_string(dummy_param_float_) + "\""); + "dummy_param_float must be 0.1253456, instead it was \"" + + std::to_string(dummy_param_float_) + "\""); } if (dummy_param_int_ != 123456) { - throw YAMLException("dummy_param_int must be 1253456, instead it was \"" + - std::to_string(dummy_param_int_) + "\""); + throw YAMLException( + "dummy_param_int must be 1253456, instead it was \"" + std::to_string(dummy_param_int_) + + "\""); } if (dummy_param_string_ != "dummy_test_123456") { throw YAMLException( - "dummy_param_float must be dummy_test_123456, instead it was \"" + - dummy_param_string_ + "\""); + "dummy_param_float must be dummy_test_123456, instead it was \"" + dummy_param_string_ + + "\""); } } -} +} // namespace flatland_plugins -PLUGINLIB_EXPORT_CLASS(flatland_plugins::DummyModelPlugin, - flatland_server::ModelPlugin) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(flatland_plugins::DummyModelPlugin, flatland_server::ModelPlugin) \ No newline at end of file diff --git a/flatland_server/src/dummy_world_plugin.cpp b/flatland_server/src/dummy_world_plugin.cpp index 06067458..5a729fa2 100644 --- a/flatland_server/src/dummy_world_plugin.cpp +++ b/flatland_server/src/dummy_world_plugin.cpp @@ -48,30 +48,32 @@ #include #include #include -#include #include + +#include using namespace flatland_server; -namespace flatland_plugins { +namespace flatland_plugins +{ -void DummyWorldPlugin::OnInitialize(const YAML::Node &plugin_reader, YamlReader &world_config) { +void DummyWorldPlugin::OnInitialize(const YAML::Node & plugin_reader, YamlReader & world_config) +{ if (world_ != NULL) { throw PluginException("World is not NULL!"); } if (name_ != "DummyWorldPluginName") { throw PluginException( - "Dummy world plugin name is in correct, instead of " - "\"DummyWorldPluginName\", the name is " + - name_); + "Dummy world plugin name is in correct, instead of " + "\"DummyWorldPluginName\", the name is " + + name_); } if (type_ != "DummyWorldPluginType") { throw PluginException( - "Dummy world plugin type is in correct, instead of " - "\"DummyWorldPluginType\", the type is " + - type_); + "Dummy world plugin type is in correct, instead of " + "\"DummyWorldPluginType\", the type is " + + type_); } } -} +} // namespace flatland_plugins -PLUGINLIB_EXPORT_CLASS(flatland_plugins::DummyWorldPlugin, - flatland_server::WorldPlugin) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(flatland_plugins::DummyWorldPlugin, flatland_server::WorldPlugin) \ No newline at end of file diff --git a/flatland_server/src/entity.cpp b/flatland_server/src/entity.cpp index 5071a6e1..9a247f92 100644 --- a/flatland_server/src/entity.cpp +++ b/flatland_server/src/entity.cpp @@ -45,15 +45,20 @@ */ #include + #include -namespace flatland_server { +namespace flatland_server +{ -Entity::Entity(std::shared_ptr node, b2World *physics_world, const std::string &name) - : node_(node), physics_world_(physics_world), name_(name) {} +Entity::Entity( + std::shared_ptr node, b2World * physics_world, const std::string & name) +: node_(node), physics_world_(physics_world), name_(name) +{ +} -const std::string &Entity::GetName() const { return name_; } +const std::string & Entity::GetName() const { return name_; } -b2World *Entity::GetPhysicsWorld() { return physics_world_; } +b2World * Entity::GetPhysicsWorld() { return physics_world_; } -} //namespace flatland_server +} // namespace flatland_server diff --git a/flatland_server/src/flatland_server_node.cpp b/flatland_server/src/flatland_server_node.cpp index adee301a..1ff2cc41 100644 --- a/flatland_server/src/flatland_server_node.cpp +++ b/flatland_server/src/flatland_server_node.cpp @@ -44,11 +44,12 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "rclcpp/rclcpp.hpp" #include + #include #include "flatland_server/simulation_manager.h" +#include "rclcpp/rclcpp.hpp" /** Global variables */ // @@ -72,57 +73,57 @@ class FlatlandServerNode : public rclcpp::Node { - public: - FlatlandServerNode() - : Node("flatland_server") - { - declare_parameter("world_path"); - declare_parameter("update_rate"); - declare_parameter("step_size"); - declare_parameter("show_viz"); - declare_parameter("viz_pub_rate"); +public: + FlatlandServerNode() : Node("flatland_server") + { + declare_parameter("world_path"); + declare_parameter("update_rate"); + declare_parameter("step_size"); + declare_parameter("show_viz"); + declare_parameter("viz_pub_rate"); - // Load parameters - if (!get_parameter("world_path", world_path_)) { - RCLCPP_INFO(get_logger(), "No world_path parameter given!"); - rclcpp::shutdown(); - return; - } - get_parameter_or("update_rate", update_rate_, 200.0f); - get_parameter_or("step_size", step_size_, 1.0f/200.0f); - get_parameter_or("show_viz", show_viz_, false); - get_parameter_or("viz_pub_rate", viz_pub_rate_, 30.0f); + // Load parameters + if (!get_parameter("world_path", world_path_)) { + RCLCPP_INFO(get_logger(), "No world_path parameter given!"); + rclcpp::shutdown(); + return; } + get_parameter_or("update_rate", update_rate_, 200.0f); + get_parameter_or("step_size", step_size_, 1.0f / 200.0f); + get_parameter_or("show_viz", show_viz_, false); + get_parameter_or("viz_pub_rate", viz_pub_rate_, 30.0f); + } - void Run() { + void Run() + { // Create simulation manager object - simulation_manager_ = std::make_shared( - shared_from_this(), world_path_, update_rate_, step_size_, show_viz_, viz_pub_rate_); - + simulation_manager_ = std::make_shared( + shared_from_this(), world_path_, update_rate_, step_size_, show_viz_, viz_pub_rate_); - RCLCPP_INFO(this->get_logger(), "Initialized"); - simulation_manager_->Main(); + RCLCPP_INFO(this->get_logger(), "Initialized"); + simulation_manager_->Main(); - RCLCPP_INFO(this->get_logger(), "Returned from simulation manager main2"); + RCLCPP_INFO(this->get_logger(), "Returned from simulation manager main2"); } - // TODO: Allow updates to step size, update rate etc. with new ros2 dynamic params + // TODO: Allow updates to step size, update rate etc. with new ros2 dynamic + // params - private: - std::string world_path_; // The file path to the world.yaml file - float update_rate_; // The physics update rate (Hz) - float step_size_; - bool show_viz_; - float viz_pub_rate_; - std::shared_ptr simulation_manager_; +private: + std::string world_path_; // The file path to the world.yaml file + float update_rate_; // The physics update rate (Hz) + float step_size_; + bool show_viz_; + float viz_pub_rate_; + std::shared_ptr simulation_manager_; }; /** * @name main * @brief Entrypoint for Flatland Server ros node */ -int main(int argc, char **argv) { - +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); auto flatland_server = std::make_shared(); flatland_server->Run(); diff --git a/flatland_server/src/geometry.cpp b/flatland_server/src/geometry.cpp index 7d877c25..7bfa8401 100644 --- a/flatland_server/src/geometry.cpp +++ b/flatland_server/src/geometry.cpp @@ -45,10 +45,13 @@ */ #include "flatland_server/geometry.h" + #include + #include -namespace flatland_server { +namespace flatland_server +{ /** * @brief Return a RotateTranslate given translation and rotation @@ -59,7 +62,8 @@ namespace flatland_server { * * @return THe RotateTranslate object */ -RotateTranslate Geometry::CreateTransform(double dx, double dy, double a) { +RotateTranslate Geometry::CreateTransform(double dx, double dy, double a) +{ RotateTranslate out = {dx, dy, cosf(a), sinf(a)}; return out; } @@ -72,17 +76,19 @@ RotateTranslate Geometry::CreateTransform(double dx, double dy, double a) { * * @return */ -b2Vec2 Geometry::Transform(const b2Vec2& in, const RotateTranslate& rt) { +b2Vec2 Geometry::Transform(const b2Vec2 & in, const RotateTranslate & rt) +{ b2Vec2 out; out.x = in.x * rt.cos - in.y * rt.sin + rt.dx; out.y = in.x * rt.sin + in.y * rt.cos + rt.dy; return out; } -b2Vec2 Geometry::InverseTransform(const b2Vec2& in, const RotateTranslate& rt) { +b2Vec2 Geometry::InverseTransform(const b2Vec2 & in, const RotateTranslate & rt) +{ b2Vec2 out; out.x = (in.x - rt.dx) * rt.cos + (in.y - rt.dy) * rt.sin; out.y = -(in.x - rt.dx) * rt.sin + (in.y - rt.dy) * rt.cos; return out; } -}; +}; // namespace flatland_server diff --git a/flatland_server/src/interactive_marker_manager.cpp b/flatland_server/src/interactive_marker_manager.cpp index 139e0180..d1daf87e 100644 --- a/flatland_server/src/interactive_marker_manager.cpp +++ b/flatland_server/src/interactive_marker_manager.cpp @@ -1,10 +1,13 @@ #include + #include -namespace flatland_server { +namespace flatland_server +{ InteractiveMarkerManager::InteractiveMarkerManager( - std::vector *model_list_ptr, PluginManager *plugin_manager_ptr) { + std::vector * model_list_ptr, PluginManager * plugin_manager_ptr) +{ models_ = model_list_ptr; plugin_manager_ = plugin_manager_ptr; manipulating_model_ = false; @@ -16,17 +19,16 @@ InteractiveMarkerManager::InteractiveMarkerManager( // Add "Delete Model" context menu option to menu handler and bind callback using namespace std::placeholders; menu_handler_.setCheckState( - menu_handler_.insert( - "Delete Model", - std::bind(&InteractiveMarkerManager::deleteModelMenuCallback, this, - _1)), - interactive_markers::MenuHandler::NO_CHECKBOX); + menu_handler_.insert( + "Delete Model", std::bind(&InteractiveMarkerManager::deleteModelMenuCallback, this, _1)), + interactive_markers::MenuHandler::NO_CHECKBOX); interactive_marker_server_->applyChanges(); } void InteractiveMarkerManager::createInteractiveMarker( - const std::string &model_name, const Pose &pose, - const visualization_msgs::msg::MarkerArray &body_markers) { + const std::string & model_name, const Pose & pose, + const visualization_msgs::msg::MarkerArray & body_markers) +{ // Set up interactive marker control objects to allow both translation and // rotation movement visualization_msgs::msg::InteractiveMarkerControl plane_control; @@ -34,22 +36,19 @@ void InteractiveMarkerManager::createInteractiveMarker( plane_control.orientation.w = 0.707; plane_control.orientation.y = 0.707; plane_control.name = "move_xy"; - plane_control.interaction_mode = - visualization_msgs::msg::InteractiveMarkerControl::MOVE_PLANE; + plane_control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_PLANE; visualization_msgs::msg::InteractiveMarkerControl rotate_control; rotate_control.always_visible = true; rotate_control.orientation.w = 0.707; rotate_control.orientation.y = 0.707; rotate_control.name = "rotate_z"; - rotate_control.interaction_mode = - visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; + rotate_control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::ROTATE_AXIS; // Add a non-interactive text marker with the model name visualization_msgs::msg::InteractiveMarkerControl no_control; no_control.always_visible = true; no_control.name = "no_control"; - no_control.interaction_mode = - visualization_msgs::msg::InteractiveMarkerControl::NONE; + no_control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::NONE; visualization_msgs::msg::Marker text_marker; text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; text_marker.color.r = 1.0; @@ -78,28 +77,27 @@ void InteractiveMarkerManager::createInteractiveMarker( // Also add body markers to the no_control object to visualize model pose // while moving its interactive marker for (size_t i = 0; i < body_markers.markers.size(); i++) { - visualization_msgs::msg::Marker transformed_body_marker = - body_markers.markers[i]; + visualization_msgs::msg::Marker transformed_body_marker = body_markers.markers[i]; // Transform original body frame marker from global to local frame RotateTranslate rt = Geometry::CreateTransform(pose.x, pose.y, pose.theta); transformed_body_marker.header.frame_id = ""; transformed_body_marker.header.stamp = rclcpp::Time(0); transformed_body_marker.pose.position.x = - (body_markers.markers[i].pose.position.x - rt.dx) * rt.cos + - (body_markers.markers[i].pose.position.y - rt.dy) * rt.sin; + (body_markers.markers[i].pose.position.x - rt.dx) * rt.cos + + (body_markers.markers[i].pose.position.y - rt.dy) * rt.sin; transformed_body_marker.pose.position.y = - -(body_markers.markers[i].pose.position.x - rt.dx) * rt.sin + - (body_markers.markers[i].pose.position.y - rt.dy) * rt.cos; + -(body_markers.markers[i].pose.position.x - rt.dx) * rt.sin + + (body_markers.markers[i].pose.position.y - rt.dy) * rt.cos; transformed_body_marker.pose.orientation.w = 1.0; transformed_body_marker.pose.orientation.x = 0.0; transformed_body_marker.pose.orientation.y = 0.0; transformed_body_marker.pose.orientation.z = 0.0; // Make line strips thicker than the original - if (transformed_body_marker.type == - visualization_msgs::msg::Marker::LINE_STRIP || - transformed_body_marker.type == visualization_msgs::msg::Marker::LINE_LIST) { + if ( + transformed_body_marker.type == visualization_msgs::msg::Marker::LINE_STRIP || + transformed_body_marker.type == visualization_msgs::msg::Marker::LINE_LIST) { transformed_body_marker.scale.x = 0.1; } @@ -124,19 +122,14 @@ void InteractiveMarkerManager::createInteractiveMarker( // Bind feedback callbacks for the new interactive marker using namespace std::placeholders; interactive_marker_server_->setCallback( - model_name, - std::bind(&InteractiveMarkerManager::processMouseUpFeedback, this, _1), - visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP); + model_name, std::bind(&InteractiveMarkerManager::processMouseUpFeedback, this, _1), + visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP); interactive_marker_server_->setCallback( - model_name, - std::bind(&InteractiveMarkerManager::processMouseDownFeedback, this, - _1), - visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_DOWN); + model_name, std::bind(&InteractiveMarkerManager::processMouseDownFeedback, this, _1), + visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_DOWN); interactive_marker_server_->setCallback( - model_name, - std::bind(&InteractiveMarkerManager::processPoseUpdateFeedback, this, - _1), - visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE); + model_name, std::bind(&InteractiveMarkerManager::processPoseUpdateFeedback, this, _1), + visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE); // Add context menu to the new interactive marker menu_handler_.apply(*interactive_marker_server_, model_name); @@ -146,7 +139,8 @@ void InteractiveMarkerManager::createInteractiveMarker( } void InteractiveMarkerManager::deleteModelMenuCallback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &feedback) { + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) +{ // Delete the model just as when the DeleteModel service is called for (unsigned int i = 0; i < (*models_).size(); i++) { if ((*models_)[i]->GetName() == feedback->marker_name) { @@ -166,8 +160,8 @@ void InteractiveMarkerManager::deleteModelMenuCallback( interactive_marker_server_->applyChanges(); } -void InteractiveMarkerManager::deleteInteractiveMarker( - const std::string &model_name) { +void InteractiveMarkerManager::deleteInteractiveMarker(const std::string & model_name) +{ // Remove target interactive marker by name and // update the server interactive_marker_server_->erase(model_name); @@ -175,7 +169,8 @@ void InteractiveMarkerManager::deleteInteractiveMarker( } void InteractiveMarkerManager::processMouseUpFeedback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &feedback) { + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) +{ // Update model that was manipulated the same way // as when the MoveModel service is called for (unsigned int i = 0; i < models_->size(); i++) { @@ -184,10 +179,8 @@ void InteractiveMarkerManager::processMouseUpFeedback( new_pose.x = feedback->pose.position.x; new_pose.y = feedback->pose.position.y; new_pose.theta = atan2( - 2.0 * feedback->pose.orientation.w * feedback->pose.orientation.z, - 1.0 - - 2.0 * feedback->pose.orientation.z * - feedback->pose.orientation.z); + 2.0 * feedback->pose.orientation.w * feedback->pose.orientation.z, + 1.0 - 2.0 * feedback->pose.orientation.z * feedback->pose.orientation.z); (*models_)[i]->SetPose(new_pose); break; } @@ -197,16 +190,19 @@ void InteractiveMarkerManager::processMouseUpFeedback( } void InteractiveMarkerManager::processMouseDownFeedback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &feedback) { + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) +{ manipulating_model_ = true; } void InteractiveMarkerManager::processPoseUpdateFeedback( - const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &feedback) { + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback) +{ pose_update_stamp_ = rclcpp::Clock(RCL_SYSTEM_TIME).now(); } -void InteractiveMarkerManager::update() { +void InteractiveMarkerManager::update() +{ // Loop through each model, extract the pose of the root body, // and use it to update the interactive marker pose. Only // necessary to compute if user is not currently dragging @@ -214,10 +210,8 @@ void InteractiveMarkerManager::update() { if (!manipulating_model_) { for (size_t i = 0; i < (*models_).size(); i++) { geometry_msgs::msg::Pose new_pose; - new_pose.position.x = - (*models_)[i]->bodies_[0]->physics_body_->GetPosition().x; - new_pose.position.y = - (*models_)[i]->bodies_[0]->physics_body_->GetPosition().y; + new_pose.position.x = (*models_)[i]->bodies_[0]->physics_body_->GetPosition().x; + new_pose.position.y = (*models_)[i]->bodies_[0]->physics_body_->GetPosition().y; double theta = (*models_)[i]->bodies_[0]->physics_body_->GetAngle(); new_pose.orientation.w = cos(0.5 * theta); new_pose.orientation.z = sin(0.5 * theta); @@ -233,18 +227,16 @@ void InteractiveMarkerManager::update() { // flag to unpause the simulation. double dt = 0; try { - dt = RCL_NS_TO_S((rclcpp::Clock(RCL_SYSTEM_TIME).now()- pose_update_stamp_).nanoseconds()); - } catch (std::runtime_error &ex) { - RCLCPP_ERROR(rclcpp::get_logger("Interactive Maker Manager"), - "Flatland Interactive Marker Manager runtime error: [%s]", - ex.what()); + dt = RCL_NS_TO_S((rclcpp::Clock(RCL_SYSTEM_TIME).now() - pose_update_stamp_).nanoseconds()); + } catch (std::runtime_error & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("Interactive Maker Manager"), + "Flatland Interactive Marker Manager runtime error: [%s]", ex.what()); } if (manipulating_model_ && dt > 0.1 && dt < 1.0) { manipulating_model_ = false; } } -InteractiveMarkerManager::~InteractiveMarkerManager() { - interactive_marker_server_.reset(); -} -} +InteractiveMarkerManager::~InteractiveMarkerManager() { interactive_marker_server_.reset(); } +} // namespace flatland_server diff --git a/flatland_server/src/joint.cpp b/flatland_server/src/joint.cpp index ce2649e5..5a7996a3 100644 --- a/flatland_server/src/joint.cpp +++ b/flatland_server/src/joint.cpp @@ -46,34 +46,38 @@ #include #include + #include -namespace flatland_server { +namespace flatland_server +{ -Joint::Joint(b2World *physics_world, Model *model, const std::string &name, - const Color &color, const b2JointDef &joint_def) - : model_(model), name_(name), physics_world_(physics_world), color_(color) { +Joint::Joint( + b2World * physics_world, Model * model, const std::string & name, const Color & color, + const b2JointDef & joint_def) +: model_(model), name_(name), physics_world_(physics_world), color_(color) +{ physics_joint_ = physics_world->CreateJoint(&joint_def); physics_joint_->SetUserData(this); } Joint::~Joint() { physics_world_->DestroyJoint(physics_joint_); } -Model *Joint::GetModel() { return model_; } +Model * Joint::GetModel() { return model_; } -const std::string &Joint::GetName() const { return name_; } +const std::string & Joint::GetName() const { return name_; } -const Color &Joint::GetColor() const { return color_; } +const Color & Joint::GetColor() const { return color_; } -void Joint::SetColor(const Color &color) { color_ = color; } +void Joint::SetColor(const Color & color) { color_ = color; } -b2Joint *Joint::GetPhysicsJoint() { return physics_joint_; } +b2Joint * Joint::GetPhysicsJoint() { return physics_joint_; } -b2World *Joint::GetphysicsWorld() { return physics_world_; } +b2World * Joint::GetphysicsWorld() { return physics_world_; } -Joint *Joint::MakeJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader) { - Joint *j; +Joint * Joint::MakeJoint(b2World * physics_world, Model * model, YamlReader & joint_reader) +{ + Joint * j; std::string name = joint_reader.Get("name"); joint_reader.SetErrorInfo("model " + Q(model->name_), "joint " + Q(name)); @@ -84,13 +88,13 @@ Joint *Joint::MakeJoint(b2World *physics_world, Model *model, YamlReader bodies_reader = joint_reader.Subnode("bodies", YamlReader::LIST); if (bodies_reader.NodeSize() != 2) { - throw YAMLException("Invalid \"bodies\" in " + - bodies_reader.entry_location_ + - ", must be a sequence of exactly two items"); + throw YAMLException( + "Invalid \"bodies\" in " + bodies_reader.entry_location_ + + ", must be a sequence of exactly two items"); } Vec2 anchors[2]; - ModelBody *bodies[2]; + ModelBody * bodies[2]; for (unsigned int i = 0; i < 2; i++) { YamlReader body_reader = bodies_reader.Subnode(i, YamlReader::MAP); std::string body_name = body_reader.Get("name"); @@ -98,44 +102,45 @@ Joint *Joint::MakeJoint(b2World *physics_world, Model *model, bodies[i] = model->GetBody(body_name); if (bodies[i] == nullptr) { - throw YAMLException("Cannot find body with name " + Q(body_name) + - " in " + body_reader.entry_location_ + " " + - body_reader.entry_name_); + throw YAMLException( + "Cannot find body with name " + Q(body_name) + " in " + body_reader.entry_location_ + " " + + body_reader.entry_name_); } } b2Vec2 anchor_A = anchors[0].Box2D(); b2Vec2 anchor_B = anchors[1].Box2D(); - b2Body *body_A = bodies[0]->physics_body_; - b2Body *body_B = bodies[1]->physics_body_; + b2Body * body_A = bodies[0]->physics_body_; + b2Body * body_B = bodies[1]->physics_body_; if (type == "revolute") { - j = MakeRevoluteJoint(physics_world, model, joint_reader, name, color, - body_A, anchor_A, body_B, anchor_B, - collide_connected); + j = MakeRevoluteJoint( + physics_world, model, joint_reader, name, color, body_A, anchor_A, body_B, anchor_B, + collide_connected); } else if (type == "weld") { - j = MakeWeldJoint(physics_world, model, joint_reader, name, color, body_A, - anchor_A, body_B, anchor_B, collide_connected); + j = MakeWeldJoint( + physics_world, model, joint_reader, name, color, body_A, anchor_A, body_B, anchor_B, + collide_connected); } else { throw YAMLException( - "Invalid joint \"type\" in " + joint_reader.entry_location_ + " " + - joint_reader.entry_name_ + ", supported joints are: revolute, weld"); + "Invalid joint \"type\" in " + joint_reader.entry_location_ + " " + joint_reader.entry_name_ + + ", supported joints are: revolute, weld"); } try { joint_reader.EnsureAccessedAllKeys(); - } catch (const YAMLException &e) { + } catch (const YAMLException & e) { delete j; throw e; } return j; } -Joint *Joint::MakeRevoluteJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader, - const std::string &name, const Color &color, - b2Body *body_A, b2Vec2 anchor_A, b2Body *body_B, - b2Vec2 anchor_B, bool collide_connected) { +Joint * Joint::MakeRevoluteJoint( + b2World * physics_world, Model * model, YamlReader & joint_reader, const std::string & name, + const Color & color, b2Body * body_A, b2Vec2 anchor_A, b2Body * body_B, b2Vec2 anchor_B, + bool collide_connected) +{ double upper_limit, lower_limit; bool has_limits = false; @@ -164,11 +169,11 @@ Joint *Joint::MakeRevoluteJoint(b2World *physics_world, Model *model, return new Joint(physics_world, model, name, color, joint_def); } -Joint *Joint::MakeWeldJoint(b2World *physics_world, Model *model, - YamlReader &joint_reader, const std::string &name, - const Color &color, b2Body *body_A, b2Vec2 anchor_A, - b2Body *body_B, b2Vec2 anchor_B, - bool collide_connected) { +Joint * Joint::MakeWeldJoint( + b2World * physics_world, Model * model, YamlReader & joint_reader, const std::string & name, + const Color & color, b2Body * body_A, b2Vec2 anchor_A, b2Body * body_B, b2Vec2 anchor_B, + bool collide_connected) +{ double angle = joint_reader.Get("angle", 0.0); double frequency = joint_reader.Get("frequency", 0.0); double damping = joint_reader.Get("damping", 0.0); @@ -186,20 +191,20 @@ Joint *Joint::MakeWeldJoint(b2World *physics_world, Model *model, return new Joint(physics_world, model, name, color, joint_def); } -void Joint::DebugOutput() const { - b2Joint *j = physics_joint_; - Body *body_A = static_cast(j->GetBodyA()->GetUserData()); - Body *body_B = static_cast(j->GetBodyB()->GetUserData()); - - RCLCPP_DEBUG(rclcpp::get_logger("Joint"), - "Joint %p: model(%p, %s) name(%s) color(%f,%f,%f,%f) " - "physics_joint(%p) body_A(%p, %s) anchor_A_world(%f, %f) " - "body_B(%p, %s) anchor_B_world(%f, %f)", - this, model_, model_->name_.c_str(), name_.c_str(), color_.r, - color_.g, color_.b, color_.a, physics_joint_, body_A, - body_A->name_.c_str(), j->GetAnchorA().x, j->GetAnchorA().y, - body_B, body_B->name_.c_str(), j->GetAnchorB().x, - j->GetAnchorB().y); +void Joint::DebugOutput() const +{ + b2Joint * j = physics_joint_; + Body * body_A = static_cast(j->GetBodyA()->GetUserData()); + Body * body_B = static_cast(j->GetBodyB()->GetUserData()); + + RCLCPP_DEBUG( + rclcpp::get_logger("Joint"), + "Joint %p: model(%p, %s) name(%s) color(%f,%f,%f,%f) " + "physics_joint(%p) body_A(%p, %s) anchor_A_world(%f, %f) " + "body_B(%p, %s) anchor_B_world(%f, %f)", + this, model_, model_->name_.c_str(), name_.c_str(), color_.r, color_.g, color_.b, color_.a, + physics_joint_, body_A, body_A->name_.c_str(), j->GetAnchorA().x, j->GetAnchorA().y, body_B, + body_B->name_.c_str(), j->GetAnchorB().x, j->GetAnchorB().y); } -} //namespace flatland_server +} // namespace flatland_server diff --git a/flatland_server/src/layer.cpp b/flatland_server/src/layer.cpp index 3abac480..f31b82bc 100644 --- a/flatland_server/src/layer.cpp +++ b/flatland_server/src/layer.cpp @@ -50,46 +50,42 @@ #include #include #include -#include #include + #include #include #include #include #include #include +#include #include -namespace flatland_server { +namespace flatland_server +{ -Layer::Layer(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const Pose &origin, const cv::Mat &bitmap, double occupied_thresh, - double resolution, const YAML::Node &properties) - : Entity(node, physics_world, names[0]), - names_(names), - cfr_(cfr), - viz_name_("layers/l_" + names[0]) { - body_ = new Body(physics_world_, this, name_, color, origin, b2_staticBody, - properties); +Layer::Layer( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::vector & names, const Color & color, const Pose & origin, + const cv::Mat & bitmap, double occupied_thresh, double resolution, const YAML::Node & properties) +: Entity(node, physics_world, names[0]), names_(names), cfr_(cfr), viz_name_("layers/l_" + names[0]) +{ + body_ = new Body(physics_world_, this, name_, color, origin, b2_staticBody, properties); LoadFromBitmap(bitmap, occupied_thresh, resolution); } -Layer::Layer(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const Pose &origin, const std::vector &line_segments, - double scale, const YAML::Node &properties) - : Entity(node, physics_world, names[0]), - names_(names), - cfr_(cfr), - viz_name_("layers/l_" + names[0]) { - body_ = new Body(physics_world_, this, name_, color, origin, b2_staticBody, - properties); +Layer::Layer( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::vector & names, const Color & color, const Pose & origin, + const std::vector & line_segments, double scale, const YAML::Node & properties) +: Entity(node, physics_world, names[0]), names_(names), cfr_(cfr), viz_name_("layers/l_" + names[0]) +{ + body_ = new Body(physics_world_, this, name_, color, origin, b2_staticBody, properties); uint16_t category_bits = cfr_->GetCategoryBits(names_); - for (const auto &line_segment : line_segments) { + for (const auto & line_segment : line_segments) { b2EdgeShape edge; edge.Set(line_segment.start.Box2D(), line_segment.end.Box2D()); edge.m_vertex1 *= scale; @@ -104,25 +100,25 @@ Layer::Layer(std::shared_ptr node, b2World *physics_world, Collisi } } -Layer::Layer(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::vector &names, const Color &color, - const YAML::Node &properties) - : Entity(node, physics_world, names[0]), - names_(names), - cfr_(cfr), - viz_name_("layers/l_" + names[0]) {} +Layer::Layer( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::vector & names, const Color & color, const YAML::Node & properties) +: Entity(node, physics_world, names[0]), names_(names), cfr_(cfr), viz_name_("layers/l_" + names[0]) +{ +} Layer::~Layer() { delete body_; } -const std::vector &Layer::GetNames() const { return names_; } +const std::vector & Layer::GetNames() const { return names_; } -const CollisionFilterRegistry *Layer::GetCfr() const { return cfr_; } -Body *Layer::GetBody() { return body_; } +const CollisionFilterRegistry * Layer::GetCfr() const { return cfr_; } +Body * Layer::GetBody() { return body_; } -Layer *Layer::MakeLayer(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &map_path, - const std::vector &names, - const Color &color, const YAML::Node &properties) { +Layer * Layer::MakeLayer( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::string & map_path, const std::vector & names, const Color & color, + const YAML::Node & properties) +{ if (map_path.length() > 0) { // If there is a map in this layer YamlReader reader(node, map_path); reader.SetErrorInfo("layer " + Q(names[0])); @@ -137,16 +133,16 @@ Layer *Layer::MakeLayer(std::shared_ptr node, b2World *physics_wor data_path = boost::filesystem::path(map_path).parent_path() / data_path; } - RCLCPP_INFO(rclcpp::get_logger("Layer"), - "layer \"%s\" loading line segments from path=\"%s\"", - names[0].c_str(), data_path.string().c_str()); + RCLCPP_INFO( + rclcpp::get_logger("Layer"), "layer \"%s\" loading line segments from path=\"%s\"", + names[0].c_str(), data_path.string().c_str()); std::vector line_segments; ReadLineSegmentsFile(data_path.string(), line_segments); - return new Layer(node, physics_world, cfr, names, color, origin, line_segments, - scale, properties); + return new Layer( + node, physics_world, cfr, names, color, origin, line_segments, scale, properties); } else { double resolution = reader.Get("resolution"); @@ -155,32 +151,34 @@ Layer *Layer::MakeLayer(std::shared_ptr node, b2World *physics_wor boost::filesystem::path image_path(reader.Get("image")); if (image_path.string().front() != '/') { - image_path = - boost::filesystem::path(map_path).parent_path() / image_path; + image_path = boost::filesystem::path(map_path).parent_path() / image_path; } - RCLCPP_INFO(rclcpp::get_logger("Layer"), "layer \"%s\" loading image from path=\"%s\"", - names[0].c_str(), image_path.string().c_str()); + RCLCPP_INFO( + rclcpp::get_logger("Layer"), "layer \"%s\" loading image from path=\"%s\"", + names[0].c_str(), image_path.string().c_str()); cv::Mat map = cv::imread(image_path.string(), cv::IMREAD_GRAYSCALE); if (map.empty()) { - throw YAMLException("Failed to load " + Q(image_path.string()) + - " in layer " + Q(names[0])); + throw YAMLException( + "Failed to load " + Q(image_path.string()) + " in layer " + Q(names[0])); } cv::Mat bitmap; map.convertTo(bitmap, CV_32FC1, 1.0 / 255.0); - return new Layer(node, physics_world, cfr, names, color, origin, bitmap, - occupied_thresh, resolution, properties); + return new Layer( + node, physics_world, cfr, names, color, origin, bitmap, occupied_thresh, resolution, + properties); } } else { // If the layer has no static obstacles return new Layer(node, physics_world, cfr, names, color, properties); } } -void Layer::ReadLineSegmentsFile(const std::string &file_path, - std::vector &line_segments) { +void Layer::ReadLineSegmentsFile( + const std::string & file_path, std::vector & line_segments) +{ std::ifstream in_file(file_path); std::string line; int line_count = 0; @@ -201,9 +199,8 @@ void Layer::ReadLineSegmentsFile(const std::string &file_path, if (ss.fail()) { throw Exception( - "Flatland File: Failed to read line segment from line " + - std::to_string(line_count) + ", in file " + - Q(boost::filesystem::path(file_path).filename().string())); + "Flatland File: Failed to read line segment from line " + std::to_string(line_count) + + ", in file " + Q(boost::filesystem::path(file_path).filename().string())); } } @@ -211,8 +208,8 @@ void Layer::ReadLineSegmentsFile(const std::string &file_path, } } -void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, - double resolution) { +void Layer::LoadFromBitmap(const cv::Mat & bitmap, double occupied_thresh, double resolution) +{ uint16_t category_bits = cfr_->GetCategoryBits(names_); auto add_edge = [&](double x1, double y1, double x2, double y2) { @@ -220,8 +217,7 @@ void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, double rows = bitmap.rows; double res = resolution; - edge.Set(b2Vec2(res * x1, res * (rows - y1)), - b2Vec2(res * x2, res * (rows - y2))); + edge.Set(b2Vec2(res * x1, res * (rows - y1)), b2Vec2(res * x2, res * (rows - y2))); b2FixtureDef fixture_def; fixture_def.shape = &edge; @@ -238,8 +234,7 @@ void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, // pad the top and bottom of the map each with an empty row (255=white). This // helps to look at the transition from one row of pixel to another - cv::copyMakeBorder(obstacle_map, padded_map, 1, 1, 0, 0, cv::BORDER_CONSTANT, - 255); + cv::copyMakeBorder(obstacle_map, padded_map, 1, 1, 0, 0, cv::BORDER_CONSTANT, 255); // loop through all the rows, looking at 2 at once for (int i = 0; i < padded_map.rows - 1; i++) { @@ -274,8 +269,7 @@ void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, } // pad the left and right of the map each with an empty column (255). - cv::copyMakeBorder(obstacle_map, padded_map, 0, 0, 1, 1, cv::BORDER_CONSTANT, - 255); + cv::copyMakeBorder(obstacle_map, padded_map, 0, 0, 1, 1, cv::BORDER_CONSTANT, 255); // loop through all the columns, looking at 2 at once for (int i = 0; i < padded_map.cols - 1; i++) { @@ -306,7 +300,8 @@ void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh, } } -void Layer::DebugVisualize() const { +void Layer::DebugVisualize() const +{ // Don't try to visualized uninitalized layers if (viz_name_.length() == 0) { return; @@ -316,26 +311,27 @@ void Layer::DebugVisualize() const { DebugVisualization::Get(node_)->Reset(viz_name_ + "_3d"); if (body_ != nullptr) { - DebugVisualization::Get(node_)->Visualize(viz_name_, body_->physics_body_, - body_->color_.r, body_->color_.g, - body_->color_.b, body_->color_.a); + DebugVisualization::Get(node_)->Visualize( + viz_name_, body_->physics_body_, body_->color_.r, body_->color_.g, body_->color_.b, + body_->color_.a); DebugVisualization::Get(node_)->VisualizeLayer(viz_name_ + "_3d", body_); } } -void Layer::DebugOutput() const { +void Layer::DebugOutput() const +{ std::string names = "{" + boost::algorithm::join(names_, ",") + "}"; uint16_t category_bits = cfr_->GetCategoryBits(names_); - RCLCPP_DEBUG(rclcpp::get_logger("Layer"), - "Layer %p: physics_world(%p) name(%s) names(%s) " - "category_bits(0x%X)", - this, physics_world_, name_.c_str(), names.c_str(), - category_bits); + RCLCPP_DEBUG( + rclcpp::get_logger("Layer"), + "Layer %p: physics_world(%p) name(%s) names(%s) " + "category_bits(0x%X)", + this, physics_world_, name_.c_str(), names.c_str(), category_bits); if (body_ != nullptr) { body_->DebugOutput(); } } -} //namespace flatland_server +} // namespace flatland_server diff --git a/flatland_server/src/model.cpp b/flatland_server/src/model.cpp index 84384422..99b6528f 100644 --- a/flatland_server/src/model.cpp +++ b/flatland_server/src/model.cpp @@ -49,17 +49,22 @@ #include #include -namespace flatland_server { - -Model::Model(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &ns, const std::string &name) - : Entity(node, physics_world, name), - namespace_(ns), - cfr_(cfr), - plugins_reader_(node), - viz_name_("models/m_" + name_) {} +namespace flatland_server +{ + +Model::Model( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::string & ns, const std::string & name) +: Entity(node, physics_world, name), + namespace_(ns), + cfr_(cfr), + plugins_reader_(node), + viz_name_("models/m_" + name_) +{ +} -Model::~Model() { +Model::~Model() +{ for (unsigned int i = 0; i < joints_.size(); i++) { delete joints_[i]; } @@ -75,13 +80,14 @@ Model::~Model() { DebugVisualization::Get(node_)->Reset(viz_name_); } -Model *Model::MakeModel(std::shared_ptr node, b2World *physics_world, CollisionFilterRegistry *cfr, - const std::string &model_yaml_path, - const std::string &ns, const std::string &name) { +Model * Model::MakeModel( + std::shared_ptr node, b2World * physics_world, CollisionFilterRegistry * cfr, + const std::string & model_yaml_path, const std::string & ns, const std::string & name) +{ YamlReader reader(node, model_yaml_path); reader.SetErrorInfo("model " + Q(name)); - Model *m = new Model(node, physics_world, cfr, ns, name); + Model * m = new Model(node, physics_world, cfr, ns, name); m->plugins_reader_ = reader.SubnodeOpt("plugins", YamlReader::LIST); @@ -92,7 +98,7 @@ Model *Model::MakeModel(std::shared_ptr node, b2World *physics_wor m->LoadBodies(bodies_reader); m->LoadJoints(joints_reader); - } catch (const YAMLException &e) { + } catch (const YAMLException & e) { delete m; throw e; } @@ -100,61 +106,64 @@ Model *Model::MakeModel(std::shared_ptr node, b2World *physics_wor return m; } -void Model::LoadBodies(YamlReader &bodies_reader) { +void Model::LoadBodies(YamlReader & bodies_reader) +{ if (bodies_reader.NodeSize() <= 0) { - throw YAMLException("Invalid \"bodies\" in " + Q(name_) + - " model, must a be list of bodies of at least size 1"); + throw YAMLException( + "Invalid \"bodies\" in " + Q(name_) + " model, must a be list of bodies of at least size 1"); } else { for (int i = 0; i < bodies_reader.NodeSize(); i++) { YamlReader body_reader = bodies_reader.Subnode(i, YamlReader::MAP); if (!body_reader.Get("enabled", "true")) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("Model"), "Body " - << Q(name_) << "." - << body_reader.Get("name", "unnamed") - << " disabled"); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("Model"), "Body " << Q(name_) << "." + << body_reader.Get("name", "unnamed") + << " disabled"); continue; } - ModelBody *b = - ModelBody::MakeBody(physics_world_, cfr_, this, body_reader); + ModelBody * b = ModelBody::MakeBody(physics_world_, cfr_, this, body_reader); bodies_.push_back(b); // ensure body is not a duplicate - if (std::count_if(bodies_.begin(), bodies_.end(), - [&](Body *i) { return i->name_ == b->name_; }) > 1) { - throw YAMLException("Invalid \"bodies\" in " + Q(name_) + - " model, body with name " + Q(b->name_) + - " already exists"); + if (std::count_if(bodies_.begin(), bodies_.end(), [&](Body * i) { + return i->name_ == b->name_; + }) > 1) { + throw YAMLException( + "Invalid \"bodies\" in " + Q(name_) + " model, body with name " + Q(b->name_) + + " already exists"); } } } } -void Model::LoadJoints(YamlReader &joints_reader) { +void Model::LoadJoints(YamlReader & joints_reader) +{ if (!joints_reader.IsNodeNull()) { for (int i = 0; i < joints_reader.NodeSize(); i++) { YamlReader joint_reader = joints_reader.Subnode(i, YamlReader::MAP); if (!joint_reader.Get("enabled", "true")) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("YAML Preprocessor"), "" - << Q(name_) << "." - << joint_reader.Get("name", "unnamed") - << " disabled"); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("YAML Preprocessor"), + "" << Q(name_) << "." << joint_reader.Get("name", "unnamed") << " disabled"); continue; } - Joint *j = Joint::MakeJoint(physics_world_, this, joint_reader); + Joint * j = Joint::MakeJoint(physics_world_, this, joint_reader); joints_.push_back(j); // ensure joint is not a duplicate - if (std::count_if(joints_.begin(), joints_.end(), - [&](Joint *i) { return i->name_ == j->name_; }) > 1) { - throw YAMLException("Invalid \"joints\" in " + Q(name_) + - " model, joint with name " + Q(j->name_) + - " already exists"); + if (std::count_if(joints_.begin(), joints_.end(), [&](Joint * i) { + return i->name_ == j->name_; + }) > 1) { + throw YAMLException( + "Invalid \"joints\" in " + Q(name_) + " model, joint with name " + Q(j->name_) + + " already exists"); } } } } -ModelBody *Model::GetBody(const std::string &name) { +ModelBody * Model::GetBody(const std::string & name) +{ for (unsigned int i = 0; i < bodies_.size(); i++) { if (bodies_[i]->name_ == name) { return bodies_[i]; @@ -163,7 +172,8 @@ ModelBody *Model::GetBody(const std::string &name) { return nullptr; } -Joint *Model::GetJoint(const std::string &name) { +Joint * Model::GetJoint(const std::string & name) +{ for (unsigned int i = 0; i < joints_.size(); i++) { if (joints_[i]->name_ == name) { return joints_[i]; @@ -172,18 +182,19 @@ Joint *Model::GetJoint(const std::string &name) { return nullptr; } -const std::vector &Model::GetBodies() { return bodies_; } +const std::vector & Model::GetBodies() { return bodies_; } -const std::vector &Model::GetJoints() { return joints_; } +const std::vector & Model::GetJoints() { return joints_; } -const std::string &Model::GetNameSpace() const { return namespace_; } +const std::string & Model::GetNameSpace() const { return namespace_; } -std::string Model::NameSpaceTF(const std::string &frame_id) const { +std::string Model::NameSpaceTF(const std::string & frame_id) const +{ // case: "global" namespace: don't prefix, strip leading slash if (frame_id.substr(0, 1) == "/") { return std::string(frame_id, 1, std::string::npos); // Strip the leading '/' - } else { // case: "local" namespace: prepend namespace + } else { // case: "local" namespace: prepend namespace if (namespace_.length() > 0) { return namespace_ + "_" + frame_id; } else { @@ -192,12 +203,13 @@ std::string Model::NameSpaceTF(const std::string &frame_id) const { } } -std::string Model::NameSpaceTopic(const std::string &topic) const { +std::string Model::NameSpaceTopic(const std::string & topic) const +{ // We don't actually want the topic to be "global" in case flatland is itself // namespaced, so strip leading slash if (topic.substr(0, 1) == "/") { return std::string(topic, 1, std::string::npos); // Strip the leading '/' - } else { // case: "local" namespace: prepend namespace/ + } else { // case: "local" namespace: prepend namespace/ if (namespace_.length() > 0) { return namespace_ + "/" + topic; } else { @@ -206,30 +218,30 @@ std::string Model::NameSpaceTopic(const std::string &topic) const { } } -const std::string &Model::GetName() const { return name_; } +const std::string & Model::GetName() const { return name_; } -const CollisionFilterRegistry *Model::GetCfr() const { return cfr_; } +const CollisionFilterRegistry * Model::GetCfr() const { return cfr_; } -void Model::SetPose(const Pose &pose) { +void Model::SetPose(const Pose & pose) +{ // Grab first (root?) body transform - RotateTranslate root_body_transform = - Geometry::CreateTransform(bodies_[0]->physics_body_->GetPosition().x, - bodies_[0]->physics_body_->GetPosition().y, - bodies_[0]->physics_body_->GetAngle()); + RotateTranslate root_body_transform = Geometry::CreateTransform( + bodies_[0]->physics_body_->GetPosition().x, bodies_[0]->physics_body_->GetPosition().y, + bodies_[0]->physics_body_->GetAngle()); // Inverse transform all bodies by this to reset their poses for (unsigned int i = 0; i < bodies_.size(); i++) { bodies_[i]->physics_body_->SetTransform( - Geometry::InverseTransform(bodies_[i]->physics_body_->GetPosition(), - root_body_transform), - 0.0); + Geometry::InverseTransform(bodies_[i]->physics_body_->GetPosition(), root_body_transform), + 0.0); } // Apply new desired pose in world coordinates TransformAll(pose); } -void Model::TransformAll(const Pose &pose_delta) { +void Model::TransformAll(const Pose & pose_delta) +{ // -- -- -- -- // | cos(a) -sin(a) x | | cos(b) -sin(b) u | // | sin(a) cos(a) y | x | sin(b) cos(b) v | @@ -241,66 +253,67 @@ void Model::TransformAll(const Pose &pose_delta) { // | 0 0 1 | // -- -- - RotateTranslate tf = - Geometry::CreateTransform(pose_delta.x, pose_delta.y, pose_delta.theta); + RotateTranslate tf = Geometry::CreateTransform(pose_delta.x, pose_delta.y, pose_delta.theta); for (unsigned int i = 0; i < bodies_.size(); i++) { bodies_[i]->physics_body_->SetTransform( - Geometry::Transform(bodies_[i]->physics_body_->GetPosition(), tf), - bodies_[i]->physics_body_->GetAngle() + pose_delta.theta); + Geometry::Transform(bodies_[i]->physics_body_->GetPosition(), tf), + bodies_[i]->physics_body_->GetAngle() + pose_delta.theta); } } -void Model::DebugVisualize() const { +void Model::DebugVisualize() const +{ DebugVisualization::Get(node_)->Reset(viz_name_); - for (const auto &body : bodies_) { - DebugVisualization::Get(node_)->Visualize(viz_name_, body->physics_body_, - body->color_.r, body->color_.g, - body->color_.b, body->color_.a); + for (const auto & body : bodies_) { + DebugVisualization::Get(node_)->Visualize( + viz_name_, body->physics_body_, body->color_.r, body->color_.g, body->color_.b, + body->color_.a); } - for (const auto &joint : joints_) { - DebugVisualization::Get(node_)->Visualize(viz_name_, joint->physics_joint_, - joint->color_.r, joint->color_.g, - joint->color_.b, joint->color_.a); + for (const auto & joint : joints_) { + DebugVisualization::Get(node_)->Visualize( + viz_name_, joint->physics_joint_, joint->color_.r, joint->color_.g, joint->color_.b, + joint->color_.a); } } -void Model::DebugOutput() const { - RCLCPP_DEBUG(rclcpp::get_logger("Model"), - "Model %p: physics_world(%p) name(%s) namespace(%s) " - "num_bodies(%lu) num_joints(%lu)", - this, physics_world_, name_.c_str(), namespace_.c_str(), - bodies_.size(), joints_.size()); +void Model::DebugOutput() const +{ + RCLCPP_DEBUG( + rclcpp::get_logger("Model"), + "Model %p: physics_world(%p) name(%s) namespace(%s) " + "num_bodies(%lu) num_joints(%lu)", + this, physics_world_, name_.c_str(), namespace_.c_str(), bodies_.size(), joints_.size()); - for (const auto &body : bodies_) { + for (const auto & body : bodies_) { body->DebugOutput(); } - for (const auto &joint : joints_) { + for (const auto & joint : joints_) { joint->DebugOutput(); } } -void Model::DumpBox2D() const { - for (const auto &body : bodies_) { - b2Log("BODY %p name=%s box2d_body=%p model=%p model_name=%s\n", body, - body->name_.c_str(), body->physics_body_, this, name_.c_str()); +void Model::DumpBox2D() const +{ + for (const auto & body : bodies_) { + b2Log( + "BODY %p name=%s box2d_body=%p model=%p model_name=%s\n", body, body->name_.c_str(), + body->physics_body_, this, name_.c_str()); body->physics_body_->Dump(); } - for (const auto &joint : joints_) { - Body *body_A = - static_cast(joint->physics_joint_->GetBodyA()->GetUserData()); - Body *body_B = - static_cast(joint->physics_joint_->GetBodyB()->GetUserData()); + for (const auto & joint : joints_) { + Body * body_A = static_cast(joint->physics_joint_->GetBodyA()->GetUserData()); + Body * body_B = static_cast(joint->physics_joint_->GetBodyB()->GetUserData()); b2Log( - "JOINT %p name=%s box2d_joint=%p model=%p model_name=%s " - "body_A(%p %s) body_B(%p %s)\n", - joint, joint->name_.c_str(), joint->physics_joint_, this, name_.c_str(), - body_A, body_A->name_.c_str(), body_B, body_B->name_.c_str()); + "JOINT %p name=%s box2d_joint=%p model=%p model_name=%s " + "body_A(%p %s) body_B(%p %s)\n", + joint, joint->name_.c_str(), joint->physics_joint_, this, name_.c_str(), body_A, + body_A->name_.c_str(), body_B, body_B->name_.c_str()); joint->physics_joint_->Dump(); } } -} //namespace flatland_server +} // namespace flatland_server diff --git a/flatland_server/src/model_body.cpp b/flatland_server/src/model_body.cpp index 1845dbab..0748f29d 100644 --- a/flatland_server/src/model_body.cpp +++ b/flatland_server/src/model_body.cpp @@ -46,24 +46,28 @@ #include #include -#include -namespace flatland_server { +#include -ModelBody::ModelBody(b2World *physics_world, CollisionFilterRegistry *cfr, - Model *model, const std::string &name, const Color &color, - const Pose &pose, b2BodyType body_type, - const YAML::Node &properties, double linear_damping, - double angular_damping) - : Body(physics_world, model, name, color, pose, body_type, properties, - linear_damping, angular_damping), - cfr_(cfr) {} +namespace flatland_server +{ + +ModelBody::ModelBody( + b2World * physics_world, CollisionFilterRegistry * cfr, Model * model, const std::string & name, + const Color & color, const Pose & pose, b2BodyType body_type, const YAML::Node & properties, + double linear_damping, double angular_damping) +: Body( + physics_world, model, name, color, pose, body_type, properties, linear_damping, + angular_damping), + cfr_(cfr) +{ +} -const CollisionFilterRegistry *ModelBody::GetCfr() const { return cfr_; } +const CollisionFilterRegistry * ModelBody::GetCfr() const { return cfr_; } -ModelBody *ModelBody::MakeBody(b2World *physics_world, - CollisionFilterRegistry *cfr, Model *model, - YamlReader &body_reader) { +ModelBody * ModelBody::MakeBody( + b2World * physics_world, CollisionFilterRegistry * cfr, Model * model, YamlReader & body_reader) +{ std::string name = body_reader.Get("name"); body_reader.SetErrorInfo("model " + Q(model->name_), "body " + Q(name)); @@ -81,23 +85,22 @@ ModelBody *ModelBody::MakeBody(b2World *physics_world, } else if (type_str == "dynamic") { type = b2_dynamicBody; } else { - throw YAMLException("Invalid \"type\" in " + body_reader.entry_location_ + - " " + body_reader.entry_name_ + - ", must be one of: static, kinematic, dynamic"); + throw YAMLException( + "Invalid \"type\" in " + body_reader.entry_location_ + " " + body_reader.entry_name_ + + ", must be one of: static, kinematic, dynamic"); } // TODO: Read the model's properties - ModelBody *m = - new ModelBody(physics_world, cfr, model, name, color, pose, type, - YAML::Node(), linear_damping, angular_damping); + ModelBody * m = new ModelBody( + physics_world, cfr, model, name, color, pose, type, YAML::Node(), linear_damping, + angular_damping); try { - YamlReader footprints_node = - body_reader.Subnode("footprints", YamlReader::LIST); + YamlReader footprints_node = body_reader.Subnode("footprints", YamlReader::LIST); body_reader.EnsureAccessedAllKeys(); m->LoadFootprints(footprints_node); - } catch (const YAMLException &e) { + } catch (const YAMLException & e) { delete m; throw e; } @@ -105,7 +108,8 @@ ModelBody *ModelBody::MakeBody(b2World *physics_world, return m; } -void ModelBody::LoadFootprints(YamlReader &footprints_reader) { +void ModelBody::LoadFootprints(YamlReader & footprints_reader) +{ for (int i = 0; i < footprints_reader.NodeSize(); i++) { YamlReader reader = footprints_reader.Subnode(i, YamlReader::MAP); @@ -115,17 +119,17 @@ void ModelBody::LoadFootprints(YamlReader &footprints_reader) { } else if (type == "polygon") { LoadPolygonFootprint(reader); } else { - throw YAMLException("Invalid footprint \"type\" in " + - reader.entry_location_ + " " + reader.entry_name_ + - ", support footprints are: circle, polygon"); + throw YAMLException( + "Invalid footprint \"type\" in " + reader.entry_location_ + " " + reader.entry_name_ + + ", support footprints are: circle, polygon"); } reader.EnsureAccessedAllKeys(); } } -void ModelBody::ConfigFootprintDef(YamlReader &footprint_reader, - b2FixtureDef &fixture_def) { +void ModelBody::ConfigFootprintDef(YamlReader & footprint_reader, b2FixtureDef & fixture_def) +{ // configure physics properties fixture_def.density = footprint_reader.Get("density"); fixture_def.friction = footprint_reader.Get("friction", 0.0); @@ -136,17 +140,15 @@ void ModelBody::ConfigFootprintDef(YamlReader &footprint_reader, fixture_def.filter.groupIndex = 0; std::vector layers = - footprint_reader.GetList("layers", {"all"}, -1, -1); + footprint_reader.GetList("layers", {"all"}, -1, -1); std::vector invalid_layers; - fixture_def.filter.categoryBits = - cfr_->GetCategoryBits(layers, &invalid_layers); + fixture_def.filter.categoryBits = cfr_->GetCategoryBits(layers, &invalid_layers); if (!invalid_layers.empty()) { - throw YAMLException("Invalid footprint \"layers\" in " + - footprint_reader.entry_location_ + " " + - footprint_reader.entry_name_ + ", {" + - boost::algorithm::join(invalid_layers, ",") + - "} layer(s) does not exist"); + throw YAMLException( + "Invalid footprint \"layers\" in " + footprint_reader.entry_location_ + " " + + footprint_reader.entry_name_ + ", {" + boost::algorithm::join(invalid_layers, ",") + + "} layer(s) does not exist"); } bool collision = footprint_reader.Get("collision", true); @@ -159,7 +161,8 @@ void ModelBody::ConfigFootprintDef(YamlReader &footprint_reader, } } -void ModelBody::LoadCircleFootprint(YamlReader &footprint_reader) { +void ModelBody::LoadCircleFootprint(YamlReader & footprint_reader) +{ Vec2 center = footprint_reader.GetVec2("center", Vec2(0, 0)); double radius = footprint_reader.Get("radius"); @@ -174,9 +177,9 @@ void ModelBody::LoadCircleFootprint(YamlReader &footprint_reader) { physics_body_->CreateFixture(&fixture_def); } -void ModelBody::LoadPolygonFootprint(YamlReader &footprint_reader) { - std::vector points = - footprint_reader.GetList("points", 3, b2_maxPolygonVertices); +void ModelBody::LoadPolygonFootprint(YamlReader & footprint_reader) +{ + std::vector points = footprint_reader.GetList("points", 3, b2_maxPolygonVertices); b2FixtureDef fixture_def; ConfigFootprintDef(footprint_reader, fixture_def); @@ -187,4 +190,4 @@ void ModelBody::LoadPolygonFootprint(YamlReader &footprint_reader) { fixture_def.shape = &shape; physics_body_->CreateFixture(&fixture_def); } -}; +}; // namespace flatland_server diff --git a/flatland_server/src/model_plugin.cpp b/flatland_server/src/model_plugin.cpp index 7a6cba19..1f8cb5a4 100644 --- a/flatland_server/src/model_plugin.cpp +++ b/flatland_server/src/model_plugin.cpp @@ -46,12 +46,15 @@ #include -namespace flatland_server { +namespace flatland_server +{ -Model *ModelPlugin::GetModel() { return model_; } +Model * ModelPlugin::GetModel() { return model_; } -void ModelPlugin::Initialize(rclcpp::Node::SharedPtr node, const std::string &type, const std::string &name, - Model *model, const YAML::Node &config) { +void ModelPlugin::Initialize( + rclcpp::Node::SharedPtr node, const std::string & type, const std::string & name, Model * model, + const YAML::Node & config) +{ type_ = type; name_ = name; model_ = model; @@ -60,15 +63,15 @@ void ModelPlugin::Initialize(rclcpp::Node::SharedPtr node, const std::string &ty OnInitialize(config); } -bool ModelPlugin::FilterContact(b2Contact *contact, Entity *&entity, - b2Fixture *&this_fixture, - b2Fixture *&other_fixture) { - b2Fixture *f_A = contact->GetFixtureA(); - b2Fixture *f_B = contact->GetFixtureB(); - Body *b_A = static_cast(f_A->GetBody()->GetUserData()); - Body *b_B = static_cast(f_B->GetBody()->GetUserData()); - Entity *e_A = b_A->GetEntity(); - Entity *e_B = b_B->GetEntity(); +bool ModelPlugin::FilterContact( + b2Contact * contact, Entity *& entity, b2Fixture *& this_fixture, b2Fixture *& other_fixture) +{ + b2Fixture * f_A = contact->GetFixtureA(); + b2Fixture * f_B = contact->GetFixtureB(); + Body * b_A = static_cast(f_A->GetBody()->GetUserData()); + Body * b_B = static_cast(f_B->GetBody()->GetUserData()); + Entity * e_A = b_A->GetEntity(); + Entity * e_B = b_B->GetEntity(); if (e_A == model_) { entity = e_B; @@ -84,10 +87,11 @@ bool ModelPlugin::FilterContact(b2Contact *contact, Entity *&entity, return true; } -bool ModelPlugin::FilterContact(b2Contact *contact) { +bool ModelPlugin::FilterContact(b2Contact * contact) +{ b2Fixture *f1, *f2; - Entity *e; + Entity * e; return FilterContact(contact, e, f1, f2); } -} //namespace flatland_server +} // namespace flatland_server diff --git a/flatland_server/src/plugin_manager.cpp b/flatland_server/src/plugin_manager.cpp index e4cb2e42..f5171862 100644 --- a/flatland_server/src/plugin_manager.cpp +++ b/flatland_server/src/plugin_manager.cpp @@ -52,30 +52,31 @@ #include #include -namespace flatland_server { +namespace flatland_server +{ -PluginManager::PluginManager(rclcpp::Node::SharedPtr node) : node_(node) { - model_plugin_loader_ = - new pluginlib::ClassLoader( - "flatland_server", "flatland_server::ModelPlugin"); +PluginManager::PluginManager(rclcpp::Node::SharedPtr node) : node_(node) +{ + model_plugin_loader_ = new pluginlib::ClassLoader( + "flatland_server", "flatland_server::ModelPlugin"); RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), "Declared Classes:"); - for(auto a : model_plugin_loader_->getDeclaredClasses()) { + for (auto a : model_plugin_loader_->getDeclaredClasses()) { RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a.c_str()); } RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), "Registered Libraries:"); - for(auto a : model_plugin_loader_->getRegisteredLibraries()) { + for (auto a : model_plugin_loader_->getRegisteredLibraries()) { RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a.c_str()); } RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), "XML Paths:"); - for(auto a : model_plugin_loader_->getPluginXmlPaths()) { + for (auto a : model_plugin_loader_->getPluginXmlPaths()) { RCLCPP_INFO(rclcpp::get_logger("Plugin Manager"), a.c_str()); } - world_plugin_loader_ = - new pluginlib::ClassLoader( - "flatland_server", "flatland_server::WorldPlugin"); + world_plugin_loader_ = new pluginlib::ClassLoader( + "flatland_server", "flatland_server::WorldPlugin"); } -PluginManager::~PluginManager() { +PluginManager::~PluginManager() +{ for (unsigned int i = 0; i < model_plugins_.size(); i++) { model_plugins_[i].reset(); // deletes a shared pointer } @@ -87,60 +88,65 @@ PluginManager::~PluginManager() { delete world_plugin_loader_; } -void PluginManager::BeforePhysicsStep(const Timekeeper &timekeeper_) { - for (const auto &model_plugin : model_plugins_) { +void PluginManager::BeforePhysicsStep(const Timekeeper & timekeeper_) +{ + for (const auto & model_plugin : model_plugins_) { model_plugin->BeforePhysicsStep(timekeeper_); } - for (const auto &world_plugin : world_plugins_) { + for (const auto & world_plugin : world_plugins_) { world_plugin->BeforePhysicsStep(timekeeper_); } } -void PluginManager::AfterPhysicsStep(const Timekeeper &timekeeper_) { - for (const auto &model_plugin : model_plugins_) { +void PluginManager::AfterPhysicsStep(const Timekeeper & timekeeper_) +{ + for (const auto & model_plugin : model_plugins_) { model_plugin->AfterPhysicsStep(timekeeper_); } - for (const auto &world_plugin : world_plugins_) { + for (const auto & world_plugin : world_plugins_) { world_plugin->AfterPhysicsStep(timekeeper_); } } -void PluginManager::DeleteModelPlugin(Model *model) { +void PluginManager::DeleteModelPlugin(Model * model) +{ model_plugins_.erase( - std::remove_if(model_plugins_.begin(), model_plugins_.end(), - [&](std::shared_ptr p) { - return p->GetModel() == model; - }), - model_plugins_.end()); + std::remove_if( + model_plugins_.begin(), model_plugins_.end(), + [&](std::shared_ptr p) { return p->GetModel() == model; }), + model_plugins_.end()); } -void PluginManager::LoadModelPlugin(Model *model, YamlReader &plugin_reader) { +void PluginManager::LoadModelPlugin(Model * model, YamlReader & plugin_reader) +{ std::string name = plugin_reader.Get("name"); std::string type = plugin_reader.Get("type"); // ensure no plugin with the same model and name - if (std::count_if(model_plugins_.begin(), model_plugins_.end(), - [&](std::shared_ptr i) { - return i->GetName() == name && i->GetModel() == model; - }) >= 1) { - throw YAMLException("Invalid \"plugins\" in " + Q(model->name_) + - " model, plugin with name " + Q(name) + - " already exists"); + if ( + std::count_if( + model_plugins_.begin(), model_plugins_.end(), [&](std::shared_ptr i) { + return i->GetName() == name && i->GetModel() == model; + }) >= 1) { + throw YAMLException( + "Invalid \"plugins\" in " + Q(model->name_) + " model, plugin with name " + Q(name) + + " already exists"); } try { if (!plugin_reader.Get("enabled", "true")) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("Plugin Manager"), "Plugin " - << Q(model->name_) << "." - << plugin_reader.Get("name", "unnamed") - << " disabled"); + RCLCPP_WARN_STREAM( + rclcpp::get_logger("Plugin Manager"), + "Plugin " << Q(model->name_) << "." << plugin_reader.Get("name", "unnamed") + << " disabled"); return; } } catch (...) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("Plugin Manager"), "Body " << Q(model->name_) << "." - << plugin_reader.Get("name", "unnamed") - << " enabled because flag failed to parse: " - << plugin_reader.Get("enabled")); + RCLCPP_WARN_STREAM( + rclcpp::get_logger("Plugin Manager"), + "Body " << Q(model->name_) << "." << plugin_reader.Get("name", "unnamed") + << " enabled because flag failed to parse: " + << plugin_reader.Get("enabled")); } // remove the name, type and enabled of the YAML Node, the plugin does not @@ -148,33 +154,31 @@ void PluginManager::LoadModelPlugin(Model *model, YamlReader &plugin_reader) { // about these parameters, remove method is broken in yaml cpp 5.2, so we // create a new node and add everything YAML::Node yaml_node; - for (const auto &k : plugin_reader.Node()) { - if (k.first.as() != "name" && - k.first.as() != "type" && - k.first.as() != "enabled") { + for (const auto & k : plugin_reader.Node()) { + if ( + k.first.as() != "name" && k.first.as() != "type" && + k.first.as() != "enabled") { yaml_node[k.first] = k.second; } } std::shared_ptr model_plugin; - std::string msg = "Model Plugin " + Q(name) + " type " + Q(type) + " model " + - Q(model->name_); + std::string msg = "Model Plugin " + Q(name) + " type " + Q(type) + " model " + Q(model->name_); try { if (type.find("::") != std::string::npos) { model_plugin = model_plugin_loader_->createSharedInstance(type); } else { - model_plugin = - model_plugin_loader_->createSharedInstance("flatland_plugins::" + type); + model_plugin = model_plugin_loader_->createSharedInstance("flatland_plugins::" + type); } - } catch (pluginlib::PluginlibException &e) { + } catch (pluginlib::PluginlibException & e) { throw PluginException(msg + ": " + std::string(e.what())); } try { model_plugin->Initialize(node_, type, name, model, yaml_node); - } catch (const std::exception &e) { + } catch (const std::exception & e) { throw PluginException(msg + ": " + std::string(e.what())); } model_plugins_.push_back(model_plugin); @@ -182,16 +186,17 @@ void PluginManager::LoadModelPlugin(Model *model, YamlReader &plugin_reader) { RCLCPP_INFO(rclcpp::get_logger("PluginManager"), "%s loaded", msg.c_str()); } -void PluginManager::LoadWorldPlugin(World *world, YamlReader &plugin_reader, - YamlReader &world_config) { +void PluginManager::LoadWorldPlugin( + World * world, YamlReader & plugin_reader, YamlReader & world_config) +{ std::string name = plugin_reader.Get("name"); std::string type = plugin_reader.Get("type"); RCLCPP_INFO(rclcpp::get_logger("PluginManager"), "finished load name and type"); // first check for duplicate plugins - for (auto &it : world_plugins_) { + for (auto & it : world_plugins_) { if (it->GetName() == name && it->GetType() == type) { - throw YAMLException("Invalid \"world plugins\" with name " + Q(name) + - ", type " + Q(type) + " already exists"); + throw YAMLException( + "Invalid \"world plugins\" with name " + Q(name) + ", type " + Q(type) + " already exists"); } } @@ -199,9 +204,8 @@ void PluginManager::LoadWorldPlugin(World *world, YamlReader &plugin_reader, std::string msg = "World Plugin " + Q(name) + " type " + Q(type); YAML::Node yaml_node; - for (const auto &k : plugin_reader.Node()) { - if (k.first.as() != "name" && - k.first.as() != "type") { + for (const auto & k : plugin_reader.Node()) { + if (k.first.as() != "name" && k.first.as() != "type") { yaml_node[k.first] = k.second; } } @@ -211,10 +215,9 @@ void PluginManager::LoadWorldPlugin(World *world, YamlReader &plugin_reader, if (type.find("::") != std::string::npos) { world_plugin = world_plugin_loader_->createSharedInstance(type); } else { - world_plugin = - world_plugin_loader_->createSharedInstance("flatland_plugins::" + type); + world_plugin = world_plugin_loader_->createSharedInstance("flatland_plugins::" + type); } - } catch (pluginlib::PluginlibException &e) { + } catch (pluginlib::PluginlibException & e) { throw PluginException(msg + ": " + std::string(e.what())); } @@ -222,7 +225,7 @@ void PluginManager::LoadWorldPlugin(World *world, YamlReader &plugin_reader, try { world_plugin->Initialize(node_, world, name, type, yaml_node, world_config); - } catch (const std::exception &e) { + } catch (const std::exception & e) { RCLCPP_INFO(rclcpp::get_logger("PluginManager"), "exception happened!"); throw PluginException(msg + ": " + std::string(e.what())); } @@ -231,30 +234,32 @@ void PluginManager::LoadWorldPlugin(World *world, YamlReader &plugin_reader, RCLCPP_INFO(rclcpp::get_logger("PluginManager"), "%s loaded ", msg.c_str()); } -void PluginManager::BeginContact(b2Contact *contact) { - for (auto &model_plugin : model_plugins_) { +void PluginManager::BeginContact(b2Contact * contact) +{ + for (auto & model_plugin : model_plugins_) { model_plugin->BeginContact(contact); } } -void PluginManager::EndContact(b2Contact *contact) { - for (auto &model_plugin : model_plugins_) { +void PluginManager::EndContact(b2Contact * contact) +{ + for (auto & model_plugin : model_plugins_) { model_plugin->EndContact(contact); } } -void PluginManager::PreSolve(b2Contact *contact, - const b2Manifold *oldManifold) { - for (auto &model_plugin : model_plugins_) { +void PluginManager::PreSolve(b2Contact * contact, const b2Manifold * oldManifold) +{ + for (auto & model_plugin : model_plugins_) { model_plugin->PreSolve(contact, oldManifold); } } -void PluginManager::PostSolve(b2Contact *contact, - const b2ContactImpulse *impulse) { - for (auto &model_plugin : model_plugins_) { +void PluginManager::PostSolve(b2Contact * contact, const b2ContactImpulse * impulse) +{ + for (auto & model_plugin : model_plugins_) { model_plugin->PostSolve(contact, impulse); } } -} //namespace flatland_server +} // namespace flatland_server diff --git a/flatland_server/src/service_manager.cpp b/flatland_server/src/service_manager.cpp index 7d5f3bb2..6757acc2 100644 --- a/flatland_server/src/service_manager.cpp +++ b/flatland_server/src/service_manager.cpp @@ -49,66 +49,60 @@ #include -namespace flatland_server { +namespace flatland_server +{ -ServiceManager::ServiceManager(SimulationManager *sim_man, World *world) - : world_(world), node_(world->node_), sim_man_(sim_man) { +ServiceManager::ServiceManager(SimulationManager * sim_man, World * world) +: world_(world), node_(world->node_), sim_man_(sim_man) +{ using namespace std::placeholders; // for _1, _2, ... etc change_rate_service_ = node_->create_service( - "change_rate", std::bind(&ServiceManager::ChangeRate, this, _1, _2, _3)); + "change_rate", std::bind(&ServiceManager::ChangeRate, this, _1, _2, _3)); spawn_model_service_ = node_->create_service( - "spawn_model", std::bind(&ServiceManager::SpawnModel, this, _1, _2, _3)); - delete_model_service_ = - node_->create_service( - "delete_model", - std::bind(&ServiceManager::DeleteModel, this, _1, _2, _3)); + "spawn_model", std::bind(&ServiceManager::SpawnModel, this, _1, _2, _3)); + delete_model_service_ = node_->create_service( + "delete_model", std::bind(&ServiceManager::DeleteModel, this, _1, _2, _3)); move_model_service_ = node_->create_service( - "move_model", std::bind(&ServiceManager::MoveModel, this, _1, _2, _3)); + "move_model", std::bind(&ServiceManager::MoveModel, this, _1, _2, _3)); pause_service_ = node_->create_service( - "pause", std::bind(&ServiceManager::Pause, this, _1, _2, _3)); + "pause", std::bind(&ServiceManager::Pause, this, _1, _2, _3)); resume_service_ = node_->create_service( - "resume", std::bind(&ServiceManager::Resume, this, _1, _2, _3)); + "resume", std::bind(&ServiceManager::Resume, this, _1, _2, _3)); toggle_pause_service_ = node_->create_service( - "toggle_pause", - std::bind(&ServiceManager::TogglePause, this, _1, _2, _3)); + "toggle_pause", std::bind(&ServiceManager::TogglePause, this, _1, _2, _3)); if (spawn_model_service_) { - RCLCPP_INFO(rclcpp::get_logger("Service Manager"), - "Model spawning service ready to go"); + RCLCPP_INFO(rclcpp::get_logger("Service Manager"), "Model spawning service ready to go"); } else { - RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), - "Error starting model spawning service"); + RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), "Error starting model spawning service"); } if (delete_model_service_) { - RCLCPP_INFO(rclcpp::get_logger("Service Manager"), - "Model deleting service ready to go"); + RCLCPP_INFO(rclcpp::get_logger("Service Manager"), "Model deleting service ready to go"); } else { - RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), - "Error starting model deleting service"); + RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), "Error starting model deleting service"); } if (move_model_service_) { - RCLCPP_INFO(rclcpp::get_logger("Service Manager"), - "Model moving service ready to go"); + RCLCPP_INFO(rclcpp::get_logger("Service Manager"), "Model moving service ready to go"); } else { - RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), - "Error starting model moving service"); + RCLCPP_ERROR(rclcpp::get_logger("Service Manager"), "Error starting model moving service"); } } bool ServiceManager::ChangeRate( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { - RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), - "Change rate requested with rate(\"%f\")", request->rate); + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("ServiceManager"), "Change rate requested with rate(\"%f\")", request->rate); try { sim_man_->setUpdateRate(request->rate); response->success = true; response->message = ""; - } catch (const std::exception &e) { + } catch (const std::exception & e) { response->success = false; response->message = std::string(e.what()); } @@ -117,15 +111,16 @@ bool ServiceManager::ChangeRate( } bool ServiceManager::SpawnModel( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { - RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), - "Model spawn requested with path(\"%s\"), namespace(\"%s\"), " - "name(\'%s\"), pose(%f,%f,%f)", - request->yaml_path.c_str(), request->ns.c_str(), - request->name.c_str(), request->pose.x, request->pose.y, - request->pose.theta); + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("ServiceManager"), + "Model spawn requested with path(\"%s\"), namespace(\"%s\"), " + "name(\'%s\"), pose(%f,%f,%f)", + request->yaml_path.c_str(), request->ns.c_str(), request->name.c_str(), request->pose.x, + request->pose.y, request->pose.theta); Pose pose(request->pose.x, request->pose.y, request->pose.theta); @@ -133,29 +128,30 @@ bool ServiceManager::SpawnModel( world_->LoadModel(request->yaml_path, request->ns, request->name, pose); response->success = true; response->message = ""; - } catch (const std::exception &e) { + } catch (const std::exception & e) { response->success = false; response->message = std::string(e.what()); - RCLCPP_ERROR(rclcpp::get_logger("ServiceManager"), - "Failed to load model! Exception: %s", e.what()); + RCLCPP_ERROR( + rclcpp::get_logger("ServiceManager"), "Failed to load model! Exception: %s", e.what()); } return true; } bool ServiceManager::DeleteModel( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { - RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), - "Model delete requested with name(\"%s\")", - request->name.c_str()); + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("ServiceManager"), "Model delete requested with name(\"%s\")", + request->name.c_str()); try { world_->DeleteModel(request->name); response->success = true; response->message = ""; - } catch (const std::exception &e) { + } catch (const std::exception & e) { response->success = false; response->message = std::string(e.what()); } @@ -164,11 +160,13 @@ bool ServiceManager::DeleteModel( } bool ServiceManager::MoveModel( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { - RCLCPP_DEBUG(rclcpp::get_logger("ServiceManager"), - "Model move requested with name(\"%s\")", request->name.c_str()); + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("ServiceManager"), "Model move requested with name(\"%s\")", + request->name.c_str()); Pose pose(request->pose.x, request->pose.y, request->pose.theta); @@ -176,7 +174,7 @@ bool ServiceManager::MoveModel( world_->MoveModel(request->name, pose); response->success = true; response->message = ""; - } catch (const std::exception &e) { + } catch (const std::exception & e) { response->success = false; response->message = std::string(e.what()); } @@ -185,25 +183,28 @@ bool ServiceManager::MoveModel( } bool ServiceManager::Pause( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ world_->Pause(); return true; } bool ServiceManager::Resume( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ world_->Resume(); return true; } bool ServiceManager::TogglePause( - const std::shared_ptr request_header, - const std::shared_ptr request, - std::shared_ptr response) { + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ world_->TogglePaused(); return true; } diff --git a/flatland_server/src/simulation_manager.cpp b/flatland_server/src/simulation_manager.cpp index 72075247..189c02d3 100644 --- a/flatland_server/src/simulation_manager.cpp +++ b/flatland_server/src/simulation_manager.cpp @@ -57,43 +57,47 @@ #include #include -namespace flatland_server { - -SimulationManager::SimulationManager(std::shared_ptr node, - std::string world_yaml_file, - double update_rate, double step_size, - bool show_viz, double viz_pub_rate) - : node_(node), - world_(nullptr), - update_rate_(update_rate), - step_size_(step_size), - show_viz_(show_viz), - viz_pub_rate_(viz_pub_rate), - world_yaml_file_(world_yaml_file), - rate_(new rclcpp::WallRate(update_rate)) { - RCLCPP_INFO(rclcpp::get_logger("SimMan"), - "Simulation params: world_yaml_file(%s) update_rate(%f), " - "step_size(%f) show_viz(%s), viz_pub_rate(%f)", - world_yaml_file_.c_str(), update_rate_, step_size_, - show_viz_ ? "true" : "false", viz_pub_rate_); +namespace flatland_server +{ + +SimulationManager::SimulationManager( + std::shared_ptr node, std::string world_yaml_file, double update_rate, + double step_size, bool show_viz, double viz_pub_rate) +: node_(node), + world_(nullptr), + update_rate_(update_rate), + step_size_(step_size), + show_viz_(show_viz), + viz_pub_rate_(viz_pub_rate), + world_yaml_file_(world_yaml_file), + rate_(new rclcpp::WallRate(update_rate)) +{ + RCLCPP_INFO( + rclcpp::get_logger("SimMan"), + "Simulation params: world_yaml_file(%s) update_rate(%f), " + "step_size(%f) show_viz(%s), viz_pub_rate(%f)", + world_yaml_file_.c_str(), update_rate_, step_size_, show_viz_ ? "true" : "false", + viz_pub_rate_); } SimulationManager::~SimulationManager() { delete rate_; } -void SimulationManager::setUpdateRate(double update_rate) { +void SimulationManager::setUpdateRate(double update_rate) +{ update_rate_ = update_rate; delete rate_; rate_ = new rclcpp::WallRate(update_rate_); } -void SimulationManager::Main() { +void SimulationManager::Main() +{ RCLCPP_INFO(rclcpp::get_logger("SimMan"), "Initializing..."); run_simulator_ = true; try { world_ = World::MakeWorld(node_, world_yaml_file_); RCLCPP_INFO(rclcpp::get_logger("SimMan"), "World loaded"); - } catch (const std::exception& e) { + } catch (const std::exception & e) { RCLCPP_FATAL(rclcpp::get_logger("SimMan"), "%s", e.what()); return; } @@ -118,20 +122,17 @@ void SimulationManager::Main() { double start_time = wall_clock.now().seconds(); double f = 0.0; try { - f = std::fmod(wall_clock.now().seconds() + (update_rate_ / 2.0), - viz_update_period); - } catch (std::runtime_error& ex) { - RCLCPP_ERROR(rclcpp::get_logger("SimMan"), "Flatland runtime error: [%s]", - ex.what()); + f = std::fmod(wall_clock.now().seconds() + (update_rate_ / 2.0), viz_update_period); + } catch (std::runtime_error & ex) { + RCLCPP_ERROR(rclcpp::get_logger("SimMan"), "Flatland runtime error: [%s]", ex.what()); } bool update_viz = ((f >= 0.0) && (f < 1.0f / update_rate_)); world_->Update(timekeeper); // Step physics by ros cycle time if (show_viz_ && update_viz) { - world_->DebugVisualize(false); // no need to update layer - DebugVisualization::Get(node_)->Publish( - timekeeper); // publish debug visualization + world_->DebugVisualize(false); // no need to update layer + DebugVisualization::Get(node_)->Publish(timekeeper); // publish debug visualization } rclcpp::spin_some(node_); @@ -148,16 +149,17 @@ void SimulationManager::Main() { filtered_cycle_util = 0.99 * filtered_cycle_util + 0.01 * cycle_util; RCLCPP_INFO_THROTTLE( - rclcpp::get_logger("SimMan"), wall_clock, 1000, - "utilization: min %.1f%% max %.1f%% ave %.1f%% factor: %.1f", - min_cycle_util, max_cycle_util, filtered_cycle_util, factor); + rclcpp::get_logger("SimMan"), wall_clock, 1000, + "utilization: min %.1f%% max %.1f%% ave %.1f%% factor: %.1f", min_cycle_util, max_cycle_util, + filtered_cycle_util, factor); } RCLCPP_INFO(rclcpp::get_logger("SimMan"), "Simulation loop ended"); delete world_; } -void SimulationManager::Shutdown() { +void SimulationManager::Shutdown() +{ RCLCPP_INFO(rclcpp::get_logger("SimMan"), "Shutdown called"); run_simulator_ = false; } diff --git a/flatland_server/src/timekeeper.cpp b/flatland_server/src/timekeeper.cpp index 617afeb9..a51f717e 100644 --- a/flatland_server/src/timekeeper.cpp +++ b/flatland_server/src/timekeeper.cpp @@ -45,38 +45,42 @@ */ #include -#include + #include +#include -// Note: ros2 implementation based partially on https://github.com/ros2/rclcpp/blob/master/rclcpp/test/test_time_source.cpp +// Note: ros2 implementation based partially on +// https://github.com/ros2/rclcpp/blob/master/rclcpp/test/test_time_source.cpp -namespace flatland_server { +namespace flatland_server +{ Timekeeper::Timekeeper(rclcpp::Node::SharedPtr node) - : node_(node), time_(rclcpp::Time(0, 0)), max_step_size_(0), clock_topic_("/clock") { +: node_(node), time_(rclcpp::Time(0, 0)), max_step_size_(0), clock_topic_("/clock") +{ clock_pub_ = node_->create_publisher(clock_topic_, 1); } -void Timekeeper::StepTime() { +void Timekeeper::StepTime() +{ time_ = time_ + rclcpp::Duration(std::chrono::duration(max_step_size_)); UpdateRosClock(); } -void Timekeeper::UpdateRosClock() const { +void Timekeeper::UpdateRosClock() const +{ rosgraph_msgs::msg::Clock clock; clock.clock = time_; clock_pub_->publish(clock); } -void Timekeeper::SetMaxStepSize(double step_size) { - max_step_size_ = step_size; -} +void Timekeeper::SetMaxStepSize(double step_size) { max_step_size_ = step_size; } -const rclcpp::Time& Timekeeper::GetSimTime() const { return time_; } +const rclcpp::Time & Timekeeper::GetSimTime() const { return time_; } double Timekeeper::GetStepSize() const { return max_step_size_; } double Timekeeper::GetMaxStepSize() const { return max_step_size_; } -} //namespace flatland_server \ No newline at end of file +} // namespace flatland_server \ No newline at end of file diff --git a/flatland_server/src/world.cpp b/flatland_server/src/world.cpp index f4540287..043962a6 100644 --- a/flatland_server/src/world.cpp +++ b/flatland_server/src/world.cpp @@ -50,25 +50,29 @@ #include #include #include -#include #include + #include #include +#include #include -namespace flatland_server { +namespace flatland_server +{ World::World(std::shared_ptr node) - : node_(node), - gravity_(0, 0), - service_paused_(false), - plugin_manager_(node), - int_marker_manager_(&models_, &plugin_manager_) { +: node_(node), + gravity_(0, 0), + service_paused_(false), + plugin_manager_(node), + int_marker_manager_(&models_, &plugin_manager_) +{ physics_world_ = new b2World(gravity_); physics_world_->SetContactListener(this); } -World::~World() { +World::~World() +{ RCLCPP_INFO(node_->get_logger(), "World: Destroying world..."); // The order of things matters in the destructor. The contact listener is @@ -80,7 +84,7 @@ World::~World() { // fixtures in a layer and it is too slow for the destroyBody method to remove // them since the AABB tree gets restructured everytime a fixture is removed // The memory will later be freed by deleting the world - for (auto &layer : layers_) { + for (auto & layer : layers_) { if (layer->body_ != nullptr) { layer->body_->physics_body_ = nullptr; } @@ -100,41 +104,41 @@ World::~World() { RCLCPP_INFO(node_->get_logger(), "World: World destroyed"); } -void World::Update(Timekeeper &timekeeper) { +void World::Update(Timekeeper & timekeeper) +{ if (!IsPaused()) { plugin_manager_.BeforePhysicsStep(timekeeper); - physics_world_->Step(timekeeper.GetStepSize(), physics_velocity_iterations_, - physics_position_iterations_); + physics_world_->Step( + timekeeper.GetStepSize(), physics_velocity_iterations_, physics_position_iterations_); timekeeper.StepTime(); plugin_manager_.AfterPhysicsStep(timekeeper); } int_marker_manager_.update(); } -void World::BeginContact(b2Contact *contact) { - plugin_manager_.BeginContact(contact); -} +void World::BeginContact(b2Contact * contact) { plugin_manager_.BeginContact(contact); } -void World::EndContact(b2Contact *contact) { - plugin_manager_.EndContact(contact); -} +void World::EndContact(b2Contact * contact) { plugin_manager_.EndContact(contact); } -void World::PreSolve(b2Contact *contact, const b2Manifold *oldManifold) { +void World::PreSolve(b2Contact * contact, const b2Manifold * oldManifold) +{ plugin_manager_.PreSolve(contact, oldManifold); } -void World::PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) { +void World::PostSolve(b2Contact * contact, const b2ContactImpulse * impulse) +{ plugin_manager_.PostSolve(contact, impulse); } -World *World::MakeWorld(std::shared_ptr node, const std::string &yaml_path) { +World * World::MakeWorld(std::shared_ptr node, const std::string & yaml_path) +{ YamlReader world_reader = YamlReader(node, yaml_path); YamlReader prop_reader = world_reader.Subnode("properties", YamlReader::MAP); int v = prop_reader.Get("velocity_iterations", 10); int p = prop_reader.Get("position_iterations", 10); prop_reader.EnsureAccessedAllKeys(); - World *w = new World(node); + World * w = new World(node); w->world_yaml_dir_ = boost::filesystem::path(yaml_path).parent_path(); w->physics_velocity_iterations_ = v; @@ -142,23 +146,21 @@ World *World::MakeWorld(std::shared_ptr node, const std::string &y try { YamlReader layers_reader = world_reader.Subnode("layers", YamlReader::LIST); - YamlReader models_reader = - world_reader.SubnodeOpt("models", YamlReader::LIST); - YamlReader world_plugin_reader = - world_reader.SubnodeOpt("plugins", YamlReader::LIST); + YamlReader models_reader = world_reader.SubnodeOpt("models", YamlReader::LIST); + YamlReader world_plugin_reader = world_reader.SubnodeOpt("plugins", YamlReader::LIST); world_reader.EnsureAccessedAllKeys(); w->LoadLayers(layers_reader); w->LoadModels(models_reader); w->LoadWorldPlugins(world_plugin_reader, w, world_reader); - } catch (const YAMLException &e) { + } catch (const YAMLException & e) { RCLCPP_FATAL(node->get_logger(), "World: Error loading from YAML"); delete w; throw e; - } catch (const PluginException &e) { + } catch (const PluginException & e) { RCLCPP_FATAL(node->get_logger(), "World: Error loading plugins"); delete w; throw e; - } catch (const Exception &e) { + } catch (const Exception & e) { RCLCPP_FATAL(node->get_logger(), "World: Error loading world"); delete w; throw e; @@ -166,7 +168,8 @@ World *World::MakeWorld(std::shared_ptr node, const std::string &y return w; } -void World::LoadLayers(YamlReader &layers_reader) { +void World::LoadLayers(YamlReader & layers_reader) +{ // loop through each layer and parse the data for (int i = 0; i < layers_reader.NodeSize(); i++) { YamlReader reader = layers_reader.Subnode(i, YamlReader::MAP); @@ -182,19 +185,17 @@ void World::LoadLayers(YamlReader &layers_reader) { if (cfr_.LayersCount() + names.size() > cfr_.MAX_LAYERS) { throw YAMLException( - "Unable to add " + std::to_string(names.size()) + - " additional layer(s) {" + boost::algorithm::join(names, ", ") + - "}, current layers count is " + std::to_string(cfr_.LayersCount()) + - ", max allowed is " + std::to_string(cfr_.MAX_LAYERS)); + "Unable to add " + std::to_string(names.size()) + " additional layer(s) {" + + boost::algorithm::join(names, ", ") + "}, current layers count is " + + std::to_string(cfr_.LayersCount()) + ", max allowed is " + std::to_string(cfr_.MAX_LAYERS)); } boost::filesystem::path map_path(reader.Get("map", "")); Color color = reader.GetColor("color", Color(1, 1, 1, 1)); - auto properties = - reader.SubnodeOpt("properties", YamlReader::NodeTypeCheck::MAP).Node(); + auto properties = reader.SubnodeOpt("properties", YamlReader::NodeTypeCheck::MAP).Node(); reader.EnsureAccessedAllKeys(); - for (const auto &name : names) { + for (const auto & name : names) { if (cfr_.RegisterLayer(name) == cfr_.LAYER_ALREADY_EXIST) { throw YAMLException("Layer with name " + Q(name) + " already exists"); } @@ -204,13 +205,13 @@ void World::LoadLayers(YamlReader &layers_reader) { map_path = world_yaml_dir_ / map_path; } - RCLCPP_INFO(node_->get_logger(), "World: Loading layer \"%s\" from path=\"%s\"", - names[0].c_str(), map_path.string().c_str()); + RCLCPP_INFO( + node_->get_logger(), "World: Loading layer \"%s\" from path=\"%s\"", names[0].c_str(), + map_path.string().c_str()); - Layer *layer = Layer::MakeLayer(node_, physics_world_, &cfr_, map_path.string(), - names, color, properties); - layers_name_map_.insert( - std::pair, Layer *>(names, layer)); + Layer * layer = + Layer::MakeLayer(node_, physics_world_, &cfr_, map_path.string(), names, color, properties); + layers_name_map_.insert(std::pair, Layer *>(names, layer)); layers_.push_back(layer); RCLCPP_INFO(node_->get_logger(), "World: Layer \"%s\" loaded", layer->name_.c_str()); @@ -218,7 +219,8 @@ void World::LoadLayers(YamlReader &layers_reader) { } } -void World::LoadModels(YamlReader &models_reader) { +void World::LoadModels(YamlReader & models_reader) +{ if (!models_reader.IsNodeNull()) { for (int i = 0; i < models_reader.NodeSize(); i++) { YamlReader reader = models_reader.Subnode(i, YamlReader::MAP); @@ -233,8 +235,9 @@ void World::LoadModels(YamlReader &models_reader) { } } -void World::LoadWorldPlugins(YamlReader &world_plugin_reader, World *world, - YamlReader &world_config) { +void World::LoadWorldPlugins( + YamlReader & world_plugin_reader, World * world, YamlReader & world_config) +{ if (!world_plugin_reader.IsNodeNull()) { for (int i = 0; i < world_plugin_reader.NodeSize(); i++) { YamlReader reader = world_plugin_reader.Subnode(i, YamlReader::MAP); @@ -243,11 +246,14 @@ void World::LoadWorldPlugins(YamlReader &world_plugin_reader, World *world, } } } -void World::LoadModel(const std::string &model_yaml_path, const std::string &ns, - const std::string &name, const Pose &pose) { +void World::LoadModel( + const std::string & model_yaml_path, const std::string & ns, const std::string & name, + const Pose & pose) +{ // ensure no duplicate model names - if (std::count_if(models_.begin(), models_.end(), - [&](Model *m) { return m->name_ == name; }) >= 1) { + if (std::count_if(models_.begin(), models_.end(), [&](Model * m) { + return m->name_ == name; + }) >= 1) { throw YAMLException("Model with name " + Q(name) + " already exists"); } @@ -256,11 +262,10 @@ void World::LoadModel(const std::string &model_yaml_path, const std::string &ns, abs_path = world_yaml_dir_ / abs_path; } - RCLCPP_INFO(node_->get_logger(), "World: Loading model from path=\"%s\"", - abs_path.string().c_str()); + RCLCPP_INFO( + node_->get_logger(), "World: Loading model from path=\"%s\"", abs_path.string().c_str()); - Model *m = - Model::MakeModel(node_, physics_world_, &cfr_, abs_path.string(), ns, name); + Model * m = Model::MakeModel(node_, physics_world_, &cfr_, abs_path.string(), ns, name); m->TransformAll(pose); try { @@ -268,11 +273,11 @@ void World::LoadModel(const std::string &model_yaml_path, const std::string &ns, YamlReader plugin_reader = m->plugins_reader_.Subnode(i, YamlReader::MAP); plugin_manager_.LoadModelPlugin(m, plugin_reader); } - } catch (const YAMLException &e) { + } catch (const YAMLException & e) { plugin_manager_.DeleteModelPlugin(m); delete m; throw e; - } catch (const PluginException &e) { + } catch (const PluginException & e) { plugin_manager_.DeleteModelPlugin(m); delete m; throw e; @@ -283,7 +288,7 @@ void World::LoadModel(const std::string &model_yaml_path, const std::string &ns, visualization_msgs::msg::MarkerArray body_markers; for (size_t i = 0; i < m->bodies_.size(); i++) { DebugVisualization::Get(node_)->BodyToMarkers( - body_markers, m->bodies_[i]->physics_body_, 1.0, 0.0, 0.0, 1.0); + body_markers, m->bodies_[i]->physics_body_, 1.0, 0.0, 0.0, 1.0); } int_marker_manager_.createInteractiveMarker(name, pose, body_markers); @@ -291,7 +296,8 @@ void World::LoadModel(const std::string &model_yaml_path, const std::string &ns, m->DebugOutput(); } -void World::DeleteModel(const std::string &name) { +void World::DeleteModel(const std::string & name) +{ bool found = false; for (unsigned int i = 0; i < models_.size(); i++) { @@ -308,12 +314,13 @@ void World::DeleteModel(const std::string &name) { } if (!found) { - throw Exception("Flatland World: failed to delete model, model with name " + - Q(name) + " does not exist"); + throw Exception( + "Flatland World: failed to delete model, model with name " + Q(name) + " does not exist"); } } -void World::MoveModel(const std::string &name, const Pose &pose) { +void World::MoveModel(const std::string & name, const Pose & pose) +{ // Find desired model bool found = false; @@ -327,8 +334,8 @@ void World::MoveModel(const std::string &name, const Pose &pose) { } if (!found) { - throw Exception("Flatland World: failed to move model, model with name " + - Q(name) + " does not exist"); + throw Exception( + "Flatland World: failed to move model, model with name " + Q(name) + " does not exist"); } } @@ -338,19 +345,18 @@ void World::Resume() { service_paused_ = false; } void World::TogglePaused() { service_paused_ = !service_paused_; } -bool World::IsPaused() { - return service_paused_ /*|| int_marker_manager_.isManipulating()*/; -} +bool World::IsPaused() { return service_paused_ /*|| int_marker_manager_.isManipulating()*/; } -void World::DebugVisualize(bool update_layers) { +void World::DebugVisualize(bool update_layers) +{ if (update_layers) { - for (const auto &layer : layers_) { + for (const auto & layer : layers_) { layer->DebugVisualize(); } } - for (const auto &model : models_) { + for (const auto & model : models_) { model->DebugVisualize(); } } -} //namespace flatland_server +} // namespace flatland_server diff --git a/flatland_server/src/world_plugin.cpp b/flatland_server/src/world_plugin.cpp index 2d631f0b..a4e2fd1a 100644 --- a/flatland_server/src/world_plugin.cpp +++ b/flatland_server/src/world_plugin.cpp @@ -45,16 +45,18 @@ */ #include -#include - #include #include + +#include #include -namespace flatland_server { -void WorldPlugin::Initialize(rclcpp::Node::SharedPtr node, World *world, std::string name, std::string type, - YAML::Node &plugin_reader, - YamlReader &world_config) { +namespace flatland_server +{ +void WorldPlugin::Initialize( + rclcpp::Node::SharedPtr node, World * world, std::string name, std::string type, + YAML::Node & plugin_reader, YamlReader & world_config) +{ world_ = world; name_ = name; type_ = type; @@ -62,4 +64,4 @@ void WorldPlugin::Initialize(rclcpp::Node::SharedPtr node, World *world, std::st node_ = node; OnInitialize(plugin_reader, world_config); } -} \ No newline at end of file +} // namespace flatland_server \ No newline at end of file diff --git a/flatland_server/src/yaml_preprocessor.cpp b/flatland_server/src/yaml_preprocessor.cpp index aadcdbae..419ff61d 100644 --- a/flatland_server/src/yaml_preprocessor.cpp +++ b/flatland_server/src/yaml_preprocessor.cpp @@ -45,21 +45,21 @@ */ #include "flatland_server/yaml_preprocessor.h" -#include + #include #include - #include #include #include +#include -namespace flatland_server { +namespace flatland_server +{ -void YamlPreprocessor::Parse(YAML::Node &node) { - this->ProcessNodes(node); -} +void YamlPreprocessor::Parse(YAML::Node & node) { this->ProcessNodes(node); } -void YamlPreprocessor::ProcessNodes(YAML::Node &node) { +void YamlPreprocessor::ProcessNodes(YAML::Node & node) +{ switch (node.Type()) { case YAML::NodeType::Sequence: for (YAML::Node child : node) { @@ -77,13 +77,15 @@ void YamlPreprocessor::ProcessNodes(YAML::Node &node) { } break; default: - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Yaml Preprocessor"), - "Yaml Preprocessor found an unexpected type: " << node.Type()); + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("Yaml Preprocessor"), + "Yaml Preprocessor found an unexpected type: " << node.Type()); break; } } -void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) { +void YamlPreprocessor::ProcessScalarNode(YAML::Node & node) +{ std::string value = node.as().substr(5); // omit the $eval boost::algorithm::trim(value); // trim whitespace RCLCPP_INFO_STREAM(rclcpp::get_logger("YAML Preprocessor"), "Attempting to parse lua " << value); @@ -93,13 +95,13 @@ void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) { } // Create the Lua context - lua_State *L = luaL_newstate(); + lua_State * L = luaL_newstate(); luaL_openlibs(L); lua_pushcfunction(L, YamlPreprocessor::LuaGetEnv); lua_setglobal(L, "env"); lua_pushcfunction(L, YamlPreprocessor::LuaGetParam); lua_setglobal(L, "param"); - lua_pushlightuserdata(L, (void*)this); + lua_pushlightuserdata(L, (void *)this); lua_setglobal(L, "class_pointer"); try { /* Attempt to run the Lua string and parse its results */ @@ -111,15 +113,19 @@ void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) { int t = lua_type(L, 1); if (t == LUA_TNIL) { node = ""; - RCLCPP_INFO_STREAM(rclcpp::get_logger("YAML Preprocessor"), "Preprocessor parsed " << value << " as empty string"); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("YAML Preprocessor"), + "Preprocessor parsed " << value << " as empty string"); } else if (t == LUA_TBOOLEAN) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("YAML Preprocessor"), "Preprocessor parsed " - << value << " as bool " - << (lua_toboolean(L, 1) ? "true" : "false")); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("YAML Preprocessor"), "Preprocessor parsed " + << value << " as bool " + << (lua_toboolean(L, 1) ? "true" : "false")); node = lua_toboolean(L, 1) ? "true" : "false"; } else if (t == LUA_TSTRING || t == LUA_TNUMBER) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("YAML Preprocessor"), "Preprocessor parsed " << value << " as " - << lua_tostring(L, 1)); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("YAML Preprocessor"), + "Preprocessor parsed " << value << " as " << lua_tostring(L, 1)); node = lua_tostring(L, 1); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "No lua output for " << value); @@ -131,16 +137,17 @@ void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) { } } -YAML::Node YamlPreprocessor::LoadParse(const std::string &path) { +YAML::Node YamlPreprocessor::LoadParse(const std::string & path) +{ YAML::Node node; try { node = YAML::LoadFile(path); - } catch (const YAML::BadFile &e) { + } catch (const YAML::BadFile & e) { throw YAMLException("File does not exist, path=" + path); - } catch (const YAML::ParserException &e) { + } catch (const YAML::ParserException & e) { throw YAMLException("Malformatted file, path=" + path, e); - } catch (const YAML::Exception &e) { + } catch (const YAML::Exception & e) { throw YAMLException("Error loading file, path=" + path, e); } @@ -148,9 +155,10 @@ YAML::Node YamlPreprocessor::LoadParse(const std::string &path) { return node; } -int YamlPreprocessor::LuaGetEnv(lua_State *L) { - const char *name = lua_tostring(L, 1); - const char *env = std::getenv(name); +int YamlPreprocessor::LuaGetEnv(lua_State * L) +{ + const char * name = lua_tostring(L, 1); + const char * env = std::getenv(name); if (lua_gettop(L) == 2 && env == NULL) { // use default if (lua_isnumber(L, 2)) { @@ -162,7 +170,8 @@ int YamlPreprocessor::LuaGetEnv(lua_State *L) { } } else { // no default if (env == NULL) { // Push back a nil - RCLCPP_WARN_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "No environment variable for: " << name); + RCLCPP_WARN_STREAM( + rclcpp::get_logger("Yaml Preprocessor"), "No environment variable for: " << name); lua_pushnil(L); } else { RCLCPP_WARN_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "Found env for " << name); @@ -178,13 +187,15 @@ int YamlPreprocessor::LuaGetEnv(lua_State *L) { return 1; // 1 return value } -int YamlPreprocessor::LuaGetParam(lua_State *L) { - const char *name = lua_tostring(L, 1); +int YamlPreprocessor::LuaGetParam(lua_State * L) +{ + const char * name = lua_tostring(L, 1); rclcpp::Parameter param; lua_getglobal(L, "class_pointer"); // push class pointer to the stack // grab the class pointer and cast it, so we can use it - YamlPreprocessor *class_pointer = reinterpret_cast(lua_touserdata(L, lua_gettop(L))); + YamlPreprocessor * class_pointer = + reinterpret_cast(lua_touserdata(L, lua_gettop(L))); lua_pop(L, 1); // pop that class pointer from the stack if (lua_gettop(L) == 2 && !class_pointer->ros_node_->has_parameter(name)) { // use default @@ -195,31 +206,34 @@ int YamlPreprocessor::LuaGetParam(lua_State *L) { } else if (lua_isstring(L, 2)) { lua_pushstring(L, lua_tostring(L, 2)); } else { - RCLCPP_WARN_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "Couldn't load int/double/string value at param " - << name); + RCLCPP_WARN_STREAM( + rclcpp::get_logger("Yaml Preprocessor"), + "Couldn't load int/double/string value at param " << name); lua_pushnil(L); } - } else { // no default + } else { // no default if (!class_pointer->ros_node_->has_parameter(name)) { // Push back a nil - RCLCPP_WARN_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "No rosparam found for: " << name); + RCLCPP_WARN_STREAM( + rclcpp::get_logger("Yaml Preprocessor"), "No rosparam found for: " << name); lua_pushnil(L); } else { if (!class_pointer->ros_node_->get_parameter(name, param)) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "Couldn't find a param with name " - << name); + RCLCPP_WARN_STREAM( + rclcpp::get_logger("Yaml Preprocessor"), "Couldn't find a param with name " << name); lua_pushnil(L); } if (param.get_type() == rclcpp::PARAMETER_DOUBLE) { lua_pushnumber(L, param.as_double()); } else if (param.get_type() == rclcpp::PARAMETER_INTEGER) { - lua_pushinteger(L, param.as_int()); + lua_pushinteger(L, param.as_int()); } else if (param.get_type() == rclcpp::PARAMETER_STRING) { lua_pushstring(L, param.as_string().c_str()); } else if (param.get_type() == rclcpp::PARAMETER_BOOL) { lua_pushstring(L, param.as_bool() ? "true" : "false"); } else { - RCLCPP_WARN_STREAM(rclcpp::get_logger("Yaml Preprocessor"), "Couldn't load int/double/string value at param " - << name); + RCLCPP_WARN_STREAM( + rclcpp::get_logger("Yaml Preprocessor"), + "Couldn't load int/double/string value at param " << name); lua_pushnil(L); } } @@ -227,4 +241,4 @@ int YamlPreprocessor::LuaGetParam(lua_State *L) { return 1; // 1 return value } -} \ No newline at end of file +} // namespace flatland_server \ No newline at end of file diff --git a/flatland_server/src/yaml_reader.cpp b/flatland_server/src/yaml_reader.cpp index ac8cdfec..1cd7f7d6 100644 --- a/flatland_server/src/yaml_reader.cpp +++ b/flatland_server/src/yaml_reader.cpp @@ -44,32 +44,39 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #include + #include -namespace flatland_server { +namespace flatland_server +{ -YamlReader::YamlReader(std::shared_ptr ros_node) : ros_node_(ros_node), yaml_preprocessor_(ros_node) { +YamlReader::YamlReader(std::shared_ptr ros_node) +: ros_node_(ros_node), yaml_preprocessor_(ros_node) +{ SetErrorInfo("_NONE_", "_NONE_"); } -YamlReader::YamlReader(std::shared_ptr ros_node, const YAML::Node &node) : node_(node), ros_node_(ros_node), yaml_preprocessor_(ros_node) { +YamlReader::YamlReader(std::shared_ptr ros_node, const YAML::Node & node) +: node_(node), ros_node_(ros_node), yaml_preprocessor_(ros_node) +{ yaml_preprocessor_.Parse(node_); SetErrorInfo("_NONE_", "_NONE_"); } -YamlReader::YamlReader(std::shared_ptr ros_node, const std::string &path) : ros_node_(ros_node), yaml_preprocessor_(ros_node) { +YamlReader::YamlReader(std::shared_ptr ros_node, const std::string & path) +: ros_node_(ros_node), yaml_preprocessor_(ros_node) +{ try { node_ = YAML::LoadFile(path); yaml_preprocessor_.Parse(node_); - } catch (const YAML::BadFile &e) { + } catch (const YAML::BadFile & e) { throw YAMLException("File does not exist, path=" + Q(path)); - } catch (const YAML::ParserException &e) { + } catch (const YAML::ParserException & e) { throw YAMLException("Malformatted file, path=" + Q(path), e); - } catch (const YAML::Exception &e) { + } catch (const YAML::Exception & e) { throw YAMLException("Error loading file, path=" + Q(path), e); } @@ -83,41 +90,40 @@ bool YamlReader::IsNodeNull() { return node_.IsNull(); } int YamlReader::NodeSize() { return node_.size(); } -YamlReader YamlReader::Subnode(int index, NodeTypeCheck type_check, - std::string subnode_location) { +YamlReader YamlReader::Subnode(int index, NodeTypeCheck type_check, std::string subnode_location) +{ YamlReader reader(ros_node_, node_[index]); // default to use the error location message of its parent - std::string location = subnode_location == "" - ? (entry_location_ + " " + entry_name_) - : subnode_location; + std::string location = + subnode_location == "" ? (entry_location_ + " " + entry_name_) : subnode_location; reader.SetErrorInfo(location, "index=" + std::to_string(index)); reader.SetFile(file_path_); if (reader.IsNodeNull()) { - throw YAMLException("Entry index=" + std::to_string(index) + - " does not exist" + reader.fmt_in_); + throw YAMLException( + "Entry index=" + std::to_string(index) + " does not exist" + reader.fmt_in_); } else if (type_check == NodeTypeCheck::MAP && !node_[index].IsMap()) { - throw YAMLException("Entry index=" + std::to_string(index) + - " must be a map" + reader.fmt_in_); + throw YAMLException( + "Entry index=" + std::to_string(index) + " must be a map" + reader.fmt_in_); } else if (type_check == NodeTypeCheck::LIST && !node_[index].IsSequence()) { - throw YAMLException("Entry index=" + std::to_string(index) + - " must be a list" + reader.fmt_in_); + throw YAMLException( + "Entry index=" + std::to_string(index) + " must be a list" + reader.fmt_in_); } return reader; } -YamlReader YamlReader::Subnode(const std::string &key, NodeTypeCheck type_check, - std::string subnode_location) { +YamlReader YamlReader::Subnode( + const std::string & key, NodeTypeCheck type_check, std::string subnode_location) +{ accessed_keys_.insert(key); YamlReader reader(ros_node_, node_[key]); // default to use the error location message of its parent - std::string location = subnode_location == "" - ? (entry_location_ + " " + entry_name_) - : subnode_location; + std::string location = + subnode_location == "" ? (entry_location_ + " " + entry_name_) : subnode_location; reader.SetErrorInfo(location, Q(key)); reader.SetFile(file_path_); @@ -132,9 +138,9 @@ YamlReader YamlReader::Subnode(const std::string &key, NodeTypeCheck type_check, return reader; } -YamlReader YamlReader::SubnodeOpt(const std::string &key, - NodeTypeCheck type_check, - std::string subnode_location) { +YamlReader YamlReader::SubnodeOpt( + const std::string & key, NodeTypeCheck type_check, std::string subnode_location) +{ if (!node_[key]) { accessed_keys_.insert(key); return YamlReader(ros_node_, YAML::Node()); @@ -142,12 +148,14 @@ YamlReader YamlReader::SubnodeOpt(const std::string &key, return Subnode(key, type_check, subnode_location); } -Vec2 YamlReader::GetVec2(const std::string &key) { +Vec2 YamlReader::GetVec2(const std::string & key) +{ std::array v = GetArray(key); return Vec2(v[0], v[1]); } -Vec2 YamlReader::GetVec2(const std::string &key, const Vec2 &vec2) { +Vec2 YamlReader::GetVec2(const std::string & key, const Vec2 & vec2) +{ if (!node_[key]) { accessed_keys_.insert(key); return vec2; @@ -156,18 +164,21 @@ Vec2 YamlReader::GetVec2(const std::string &key, const Vec2 &vec2) { return GetVec2(key); } -Color YamlReader::GetColor(const std::string &key, const Color &default_val) { - std::array v = GetArray( - key, {default_val.r, default_val.g, default_val.b, default_val.a}); +Color YamlReader::GetColor(const std::string & key, const Color & default_val) +{ + std::array v = + GetArray(key, {default_val.r, default_val.g, default_val.b, default_val.a}); return Color(v[0], v[1], v[2], v[3]); } -Pose YamlReader::GetPose(const std::string &key) { +Pose YamlReader::GetPose(const std::string & key) +{ std::array v = GetArray(key); return Pose(v[0], v[1], v[2]); } -Pose YamlReader::GetPose(const std::string &key, const Pose &default_val) { +Pose YamlReader::GetPose(const std::string & key, const Pose & default_val) +{ if (!node_[key]) { accessed_keys_.insert(key); return default_val; @@ -176,8 +187,8 @@ Pose YamlReader::GetPose(const std::string &key, const Pose &default_val) { return GetPose(key); } -void YamlReader::SetErrorInfo(std::string entry_location, - std::string entry_name) { +void YamlReader::SetErrorInfo(std::string entry_location, std::string entry_name) +{ boost::algorithm::trim(entry_location); boost::algorithm::trim(entry_name); @@ -210,7 +221,8 @@ void YamlReader::SetErrorInfo(std::string entry_location, } } -void YamlReader::SetFile(const std::string &file_path) { +void YamlReader::SetFile(const std::string & file_path) +{ if (file_path == "_NONE_") { file_path_ = ""; } else { @@ -220,7 +232,8 @@ void YamlReader::SetFile(const std::string &file_path) { filename_ = boost::filesystem::path(file_path).filename().string(); } -void YamlReader::EnsureAccessedAllKeys() { +void YamlReader::EnsureAccessedAllKeys() +{ if (!node_.IsMap()) { throw YAMLException("Entry" + fmt_name_ + " should be a map" + fmt_in_); } @@ -228,26 +241,24 @@ void YamlReader::EnsureAccessedAllKeys() { std::vector unused_keys; std::vector keys; - for (const auto &k : node_) { + for (const auto & k : node_) { keys.push_back(k.first.as()); } std::sort(keys.begin(), keys.end()); - for (const auto &key : keys) { + for (const auto & key : keys) { if (accessed_keys_.count(key) == 0) { unused_keys.push_back("\"" + key + "\""); } } std::string keys_str = "{" + boost::algorithm::join(keys, ", ") + "}"; - std::string unused_keys_str = - "{" + boost::algorithm::join(unused_keys, ", ") + "}"; + std::string unused_keys_str = "{" + boost::algorithm::join(unused_keys, ", ") + "}"; if (unused_keys.size() > 0) { - throw YAMLException("Entry" + fmt_name_ + - " contains unrecognized entry(s) " + unused_keys_str + - fmt_in_); + throw YAMLException( + "Entry" + fmt_name_ + " contains unrecognized entry(s) " + unused_keys_str + fmt_in_); } } -} \ No newline at end of file +} // namespace flatland_server \ No newline at end of file diff --git a/flatland_server/test/collision_filter_registry_test.cpp b/flatland_server/test/collision_filter_registry_test.cpp index ded183f7..ab83b555 100644 --- a/flatland_server/test/collision_filter_registry_test.cpp +++ b/flatland_server/test/collision_filter_registry_test.cpp @@ -54,12 +54,14 @@ using namespace flatland_server; typedef CollisionFilterRegistry CFR; -class CollisionFilterRegistryTest : public ::testing::Test { - protected: +class CollisionFilterRegistryTest : public ::testing::Test +{ +protected: CollisionFilterRegistry cfr; }; -TEST_F(CollisionFilterRegistryTest, empty_test) { +TEST_F(CollisionFilterRegistryTest, empty_test) +{ EXPECT_FALSE(cfr.IsLayersFull()); EXPECT_EQ(cfr.LookUpLayerId("random_layer"), CFR::LAYER_NOT_EXIST); @@ -67,7 +69,8 @@ TEST_F(CollisionFilterRegistryTest, empty_test) { EXPECT_EQ(layer_names.size(), 0UL); } -TEST_F(CollisionFilterRegistryTest, register_collide_test) { +TEST_F(CollisionFilterRegistryTest, register_collide_test) +{ EXPECT_EQ(cfr.RegisterCollide(), 1); EXPECT_EQ(cfr.RegisterCollide(), 2); EXPECT_EQ(cfr.RegisterCollide(), 3); @@ -75,7 +78,8 @@ TEST_F(CollisionFilterRegistryTest, register_collide_test) { EXPECT_EQ(cfr.RegisterCollide(), 5); } -TEST_F(CollisionFilterRegistryTest, register_no_collide_test) { +TEST_F(CollisionFilterRegistryTest, register_no_collide_test) +{ EXPECT_EQ(cfr.RegisterNoCollide(), -1); EXPECT_EQ(cfr.RegisterNoCollide(), -2); EXPECT_EQ(cfr.RegisterNoCollide(), -3); @@ -83,7 +87,8 @@ TEST_F(CollisionFilterRegistryTest, register_no_collide_test) { EXPECT_EQ(cfr.RegisterNoCollide(), -5); } -TEST_F(CollisionFilterRegistryTest, register_layers_test) { +TEST_F(CollisionFilterRegistryTest, register_layers_test) +{ EXPECT_EQ(CFR::MAX_LAYERS, 16); EXPECT_EQ(cfr.RegisterLayer("layer1"), 0); @@ -101,16 +106,11 @@ TEST_F(CollisionFilterRegistryTest, register_layers_test) { std::vector layer_names = cfr.GetAllLayers(); EXPECT_EQ(layer_names.size(), 5UL); - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer1") != - layer_names.end()); - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer2") != - layer_names.end()); - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer3") != - layer_names.end()); - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer4") != - layer_names.end()); - EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer5") != - layer_names.end()); + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer1") != layer_names.end()); + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer2") != layer_names.end()); + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer3") != layer_names.end()); + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer4") != layer_names.end()); + EXPECT_TRUE(std::find(layer_names.begin(), layer_names.end(), "layer5") != layer_names.end()); EXPECT_EQ(cfr.RegisterLayer("layer5"), CFR::LAYER_ALREADY_EXIST); EXPECT_EQ(cfr.RegisterLayer("layer6"), 5); @@ -133,7 +133,8 @@ TEST_F(CollisionFilterRegistryTest, register_layers_test) { } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_server/test/debug_visualization_test.cpp b/flatland_server/test/debug_visualization_test.cpp index 07837b87..dbc1442a 100644 --- a/flatland_server/test/debug_visualization_test.cpp +++ b/flatland_server/test/debug_visualization_test.cpp @@ -58,7 +58,8 @@ using std::placeholders::_1; // Test the bodyToMarkers method on a polygon shape -TEST(DebugVizTest, testBodyToMarkersPolygon) { +TEST(DebugVizTest, testBodyToMarkersPolygon) +{ b2Vec2 gravity(0.0, 0.0); b2World world(gravity); @@ -66,7 +67,7 @@ TEST(DebugVizTest, testBodyToMarkersPolygon) { bodyDef.type = b2_dynamicBody; bodyDef.position.Set(3.0f, 4.0f); bodyDef.angle = M_PI_2; - b2Body* body = world.CreateBody(&bodyDef); + b2Body * body = world.CreateBody(&bodyDef); b2PolygonShape dynamicBox; dynamicBox.SetAsBox(1.0f, 2.0f); @@ -77,10 +78,9 @@ TEST(DebugVizTest, testBodyToMarkersPolygon) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared( - "test_debug_visualization_BodyToMarkersPolygon"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers( - markers, body, 1.0, 0.0, 0.5, 0.7); + std::shared_ptr node = + rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersPolygon"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.5, 0.7); // check that marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -123,7 +123,8 @@ TEST(DebugVizTest, testBodyToMarkersPolygon) { } // Test the bodyToMarkers method on a circle shape -TEST(DebugVizTest, testBodyToMarkersCircle) { +TEST(DebugVizTest, testBodyToMarkersCircle) +{ b2Vec2 gravity(0.0, 0.0); b2World world(gravity); @@ -131,7 +132,7 @@ TEST(DebugVizTest, testBodyToMarkersCircle) { bodyDef.type = b2_dynamicBody; bodyDef.position.Set(3.0f, 4.0f); bodyDef.angle = M_PI_2; - b2Body* body = world.CreateBody(&bodyDef); + b2Body * body = world.CreateBody(&bodyDef); b2FixtureDef fixtureDef; b2CircleShape circle; @@ -142,9 +143,8 @@ TEST(DebugVizTest, testBodyToMarkersCircle) { visualization_msgs::msg::MarkerArray markers; std::shared_ptr node = - rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersCircle"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers( - markers, body, 1.0, 0.0, 0.0, 1.0); + rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersCircle"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); // check that marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -156,12 +156,13 @@ TEST(DebugVizTest, testBodyToMarkersCircle) { } // Test the bodyToMarkers method on a edge shape -TEST(DebugVizTest, testBodyToMarkersEdge) { +TEST(DebugVizTest, testBodyToMarkersEdge) +{ b2Vec2 gravity(0.0, 0.0); b2World world(gravity); b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); + b2Body * body = world.CreateBody(&bodyDef); b2FixtureDef fixtureDef; b2EdgeShape edge; @@ -172,9 +173,8 @@ TEST(DebugVizTest, testBodyToMarkersEdge) { visualization_msgs::msg::MarkerArray markers; std::shared_ptr node = - rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersEdge"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers( - markers, body, 1.0, 0.0, 0.0, 1.0); + rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersEdge"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); // check that marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -188,12 +188,13 @@ TEST(DebugVizTest, testBodyToMarkersEdge) { } // Test the bodyToMarkers method on an unsupported shape -TEST(DebugVizTest, testBodyToMarkersUnsupported) { +TEST(DebugVizTest, testBodyToMarkersUnsupported) +{ b2Vec2 gravity(0.0, 0.0); b2World world(gravity); b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); + b2Body * body = world.CreateBody(&bodyDef); b2FixtureDef fixtureDef; b2Vec2 vs[4]; @@ -207,21 +208,21 @@ TEST(DebugVizTest, testBodyToMarkersUnsupported) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared( - "test_debug_visualization_BodyToMarkersUnsupported"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers( - markers, body, 1.0, 0.0, 0.0, 1.0); + std::shared_ptr node = + rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersUnsupported"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); // check that marker was not created ASSERT_EQ(markers.markers.size(), 0UL); } // test bodyToMarkers with a body with multiple fixtures -TEST(DebugVizTest, testBodyToMarkersMultifixture) { +TEST(DebugVizTest, testBodyToMarkersMultifixture) +{ b2Vec2 gravity(0.0, 0.0); b2World world(gravity); b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); + b2Body * body = world.CreateBody(&bodyDef); // body 2 before body 1 because fixture ordering is LIFO in box2d b2FixtureDef fixtureDef, fixtureDef2; @@ -238,10 +239,9 @@ TEST(DebugVizTest, testBodyToMarkersMultifixture) { body->CreateFixture(&fixtureDef); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared( - "test_debug_visualization_BodyToMarkersMultifixture"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers( - markers, body, 1.0, 0.0, 0.0, 1.0); + std::shared_ptr node = + rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersMultifixture"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); // check that one marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -261,13 +261,14 @@ TEST(DebugVizTest, testBodyToMarkersMultifixture) { } // test bodyToMarkers with multiple bodies -TEST(DebugVizTest, testBodyToMarkersMultibody) { +TEST(DebugVizTest, testBodyToMarkersMultibody) +{ b2Vec2 gravity(0.0, 0.0); b2World world(gravity); b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); - b2Body* body2 = world.CreateBody(&bodyDef); + b2Body * body = world.CreateBody(&bodyDef); + b2Body * body2 = world.CreateBody(&bodyDef); b2FixtureDef fixtureDef, fixtureDef2; b2EdgeShape edge, edge2; @@ -283,12 +284,10 @@ TEST(DebugVizTest, testBodyToMarkersMultibody) { body2->CreateFixture(&fixtureDef2); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared( - "test_debug_visualization_BodyToMarkersMultibody"); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers( - markers, body, 1.0, 0.0, 0.0, 1.0); - flatland_server::DebugVisualization::Get(node)->BodyToMarkers( - markers, body2, 1.0, 0.0, 0.0, 1.0); + std::shared_ptr node = + rclcpp::Node::make_shared("test_debug_visualization_BodyToMarkersMultibody"); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body, 1.0, 0.0, 0.0, 1.0); + flatland_server::DebugVisualization::Get(node)->BodyToMarkers(markers, body2, 1.0, 0.0, 0.0, 1.0); // check that marker was created ASSERT_EQ(markers.markers.size(), 1UL); @@ -308,13 +307,14 @@ TEST(DebugVizTest, testBodyToMarkersMultibody) { } // test bodyToMarkers with multiple joint -TEST(DebugVizTest, testJointToMarkersMultiJoint) { +TEST(DebugVizTest, testJointToMarkersMultiJoint) +{ b2Vec2 gravity(0.0, 0.0); b2World world(gravity); b2BodyDef bodyDef; - b2Body* b1 = world.CreateBody(&bodyDef); - b2Body* b2 = world.CreateBody(&bodyDef); + b2Body * b1 = world.CreateBody(&bodyDef); + b2Body * b2 = world.CreateBody(&bodyDef); b2WeldJointDef jd1, jd2; jd1.bodyA = b1; @@ -326,16 +326,14 @@ TEST(DebugVizTest, testJointToMarkersMultiJoint) { jd2.localAnchorA = b2Vec2(1, 2); jd2.localAnchorB = b2Vec2(3, 4); - b2Joint* j1 = world.CreateJoint(&jd1); - b2Joint* j2 = world.CreateJoint(&jd2); + b2Joint * j1 = world.CreateJoint(&jd1); + b2Joint * j2 = world.CreateJoint(&jd2); visualization_msgs::msg::MarkerArray markers; - std::shared_ptr node = rclcpp::Node::make_shared( - "test_debug_visualization_JointToMarkersMultiJoint"); - flatland_server::DebugVisualization::Get(node)->JointToMarkers( - markers, j1, 0.1, 0.2, 0.3, 0.4); - flatland_server::DebugVisualization::Get(node)->JointToMarkers( - markers, j2, 0.5, 0.6, 0.7, 0.8); + std::shared_ptr node = + rclcpp::Node::make_shared("test_debug_visualization_JointToMarkersMultiJoint"); + flatland_server::DebugVisualization::Get(node)->JointToMarkers(markers, j1, 0.1, 0.2, 0.3, 0.4); + flatland_server::DebugVisualization::Get(node)->JointToMarkers(markers, j2, 0.5, 0.6, 0.7, 0.8); // check that marker was created ASSERT_EQ(markers.markers.size(), 4UL); @@ -394,19 +392,23 @@ TEST(DebugVizTest, testJointToMarkersMultiJoint) { } // A helper class to accept MarkerArray message callbacks -struct MarkerArraySubscriptionHelper { +struct MarkerArraySubscriptionHelper +{ std::shared_ptr node_; visualization_msgs::msg::MarkerArray markers_; int count_; explicit MarkerArraySubscriptionHelper(std::shared_ptr node) - : node_(std::move(node)), count_(0) {} + : node_(std::move(node)), count_(0) + { + } /** * @brief callback that stores the last message and total message count * @param msg The input message pointer */ - void callback(const visualization_msgs::msg::MarkerArray& msg) { + void callback(const visualization_msgs::msg::MarkerArray & msg) + { // void callback(const visualization_msgs::msg::MarkerArrayConstPtr& msg) { ++count_; RCLCPP_INFO(rclcpp::get_logger("Debug Visualization Test"), "GOT ONE"); @@ -420,7 +422,8 @@ struct MarkerArraySubscriptionHelper { * * @return true if successful */ - bool waitForMessageCount(int count) const { + bool waitForMessageCount(int count) const + { rclcpp::Rate rate(10); // throttle check to 10Hz for (unsigned int i = 0; i < 20; i++) { rclcpp::spin_some(node_); @@ -432,9 +435,10 @@ struct MarkerArraySubscriptionHelper { }; // Test the bodyToMarkers method on an unsupported shape -TEST(DebugVizTest, testPublishMarkers) { +TEST(DebugVizTest, testPublishMarkers) +{ std::shared_ptr node = - rclcpp::Node::make_shared("test_debug_visualization_PublishMarkers"); + rclcpp::Node::make_shared("test_debug_visualization_PublishMarkers"); flatland_server::Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(0.01); @@ -442,8 +446,8 @@ TEST(DebugVizTest, testPublishMarkers) { b2World world(gravity); b2BodyDef bodyDef; - b2Body* body = world.CreateBody(&bodyDef); - b2Body* body2 = world.CreateBody(&bodyDef); + b2Body * body = world.CreateBody(&bodyDef); + b2Body * body2 = world.CreateBody(&bodyDef); b2FixtureDef fixtureDef; b2CircleShape circle; @@ -457,14 +461,13 @@ TEST(DebugVizTest, testPublishMarkers) { joint_def.bodyB = body2; joint_def.localAnchorA = b2Vec2(0, 0); joint_def.localAnchorB = b2Vec2(0, 0); - b2Joint* joint = world.CreateJoint(&joint_def); + b2Joint * joint = world.CreateJoint(&joint_def); // Set up helper class subscribing to rostopic MarkerArraySubscriptionHelper helper(node); std::string topicName = "example"; auto sub = node->create_subscription( - topicName, 0, - std::bind(&MarkerArraySubscriptionHelper::callback, &helper, _1)); + topicName, 0, std::bind(&MarkerArraySubscriptionHelper::callback, &helper, _1)); auto debugVis = flatland_server::DebugVisualization::Get(node); debugVis->Visualize(topicName, body, 1.0, 0.0, 0.0, 1.0); @@ -520,7 +523,8 @@ TEST(DebugVizTest, testPublishMarkers) { } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_server/test/dummy_model_plugin_test.cpp b/flatland_server/test/dummy_model_plugin_test.cpp index c4712723..3218543a 100644 --- a/flatland_server/test/dummy_model_plugin_test.cpp +++ b/flatland_server/test/dummy_model_plugin_test.cpp @@ -57,15 +57,15 @@ * Test the pluginlib is configured correctly so that the model can be * discovered */ -TEST(DummyModelPluginTest, pluginlib_load_test) { - std::shared_ptr node = - rclcpp::Node::make_shared("test_dummy_model"); +TEST(DummyModelPluginTest, pluginlib_load_test) +{ + std::shared_ptr node = rclcpp::Node::make_shared("test_dummy_model"); pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::ModelPlugin"); + "flatland_server", "flatland_server::ModelPlugin"); try { std::shared_ptr plugin = - loader.createSharedInstance("flatland_plugins::DummyModelPlugin"); + loader.createSharedInstance("flatland_plugins::DummyModelPlugin"); YAML::Node n = YAML::Node(); n["dummy_param_float"] = 0.123456; @@ -75,15 +75,15 @@ TEST(DummyModelPluginTest, pluginlib_load_test) { flatland_server::CollisionFilterRegistry cfr; flatland_server::Model model(node, nullptr, &cfr, "", ""); - plugin->Initialize(node, "DummyModelPlugin", "DummyModelPluginTest", &model, - n); - } catch (pluginlib::PluginlibException& e) { + plugin->Initialize(node, "DummyModelPlugin", "DummyModelPluginTest", &model, n); + } catch (pluginlib::PluginlibException & e) { FAIL() << "Failed to load Dummy Model Plugin. " << e.what(); } } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_server/test/dummy_world_plugin_test.cpp b/flatland_server/test/dummy_world_plugin_test.cpp index df72f1c4..f2a82b0e 100644 --- a/flatland_server/test/dummy_world_plugin_test.cpp +++ b/flatland_server/test/dummy_world_plugin_test.cpp @@ -55,27 +55,27 @@ using namespace flatland_server; -TEST(DummyWorldPluginTest, pluginlib_load_test) { - std::shared_ptr node = - rclcpp::Node::make_shared("test_dummy_world"); +TEST(DummyWorldPluginTest, pluginlib_load_test) +{ + std::shared_ptr node = rclcpp::Node::make_shared("test_dummy_world"); pluginlib::ClassLoader loader( - "flatland_server", "flatland_server::WorldPlugin"); + "flatland_server", "flatland_server::WorldPlugin"); try { std::shared_ptr plugin = - loader.createSharedInstance("flatland_plugins::DummyWorldPlugin"); + loader.createSharedInstance("flatland_plugins::DummyWorldPlugin"); YAML::Node n = YAML::Node(); YamlReader reader = YamlReader(node); - plugin->Initialize(node, nullptr, "DummyWorldPluginName", - "DummyWorldPluginType", n, reader); - } catch (pluginlib::PluginlibException& e) { + plugin->Initialize(node, nullptr, "DummyWorldPluginName", "DummyWorldPluginType", n, reader); + } catch (pluginlib::PluginlibException & e) { FAIL() << "Failed to load Dummy World Plugin. " << e.what(); } } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_server/test/geometry_test.cpp b/flatland_server/test/geometry_test.cpp index 1df2cfb0..220c1ede 100644 --- a/flatland_server/test/geometry_test.cpp +++ b/flatland_server/test/geometry_test.cpp @@ -51,9 +51,9 @@ #include // Test the CreateTransform method -TEST(TestSuite, testCreateTransform) { - flatland_server::RotateTranslate rt = - flatland_server::Geometry::CreateTransform(2.0, 1.0, 1.1); +TEST(TestSuite, testCreateTransform) +{ + flatland_server::RotateTranslate rt = flatland_server::Geometry::CreateTransform(2.0, 1.0, 1.1); EXPECT_NEAR(rt.dx, 2.0, 1e-5); EXPECT_NEAR(rt.dy, 1.0, 1e-5); @@ -62,9 +62,9 @@ TEST(TestSuite, testCreateTransform) { } // Test the Transform method, translation -TEST(TestSuite, testTransformTranslate) { - flatland_server::RotateTranslate rt = - flatland_server::Geometry::CreateTransform(2.0, 1.5, 0.0); +TEST(TestSuite, testTransformTranslate) +{ + flatland_server::RotateTranslate rt = flatland_server::Geometry::CreateTransform(2.0, 1.5, 0.0); b2Vec2 in(1.0, 2.0); b2Vec2 out = flatland_server::Geometry::Transform(in, rt); @@ -74,9 +74,10 @@ TEST(TestSuite, testTransformTranslate) { } // Test the Transform method, rotation -TEST(TestSuite, testTransformRotate) { +TEST(TestSuite, testTransformRotate) +{ flatland_server::RotateTranslate rt = - flatland_server::Geometry::CreateTransform(0.0, 0.0, M_PI_2); + flatland_server::Geometry::CreateTransform(0.0, 0.0, M_PI_2); b2Vec2 in(1.0, 2.0); b2Vec2 out = flatland_server::Geometry::Transform(in, rt); @@ -86,9 +87,9 @@ TEST(TestSuite, testTransformRotate) { } // Test the Transform method, translation + rotation -TEST(TestSuite, testTransformTranslateAndRotate) { - flatland_server::RotateTranslate rt = - flatland_server::Geometry::CreateTransform(1.0, 0.5, -M_PI); +TEST(TestSuite, testTransformTranslateAndRotate) +{ + flatland_server::RotateTranslate rt = flatland_server::Geometry::CreateTransform(1.0, 0.5, -M_PI); b2Vec2 in(-1.0, 1.5); b2Vec2 out = flatland_server::Geometry::Transform(in, rt); @@ -98,7 +99,8 @@ TEST(TestSuite, testTransformTranslateAndRotate) { } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_server/test/load_world_test.cpp b/flatland_server/test/load_world_test.cpp index 0ae1e936..9f9f343c 100644 --- a/flatland_server/test/load_world_test.cpp +++ b/flatland_server/test/load_world_test.cpp @@ -61,18 +61,21 @@ namespace fs = boost::filesystem; using namespace flatland_server; -class LoadWorldTest : public ::testing::Test { - protected: +class LoadWorldTest : public ::testing::Test +{ +protected: boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; - World *w; + World * w; - void SetUp() override { + void SetUp() override + { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); w = nullptr; } - void TearDown() override { + void TearDown() override + { if (w != nullptr) { delete w; } @@ -80,46 +83,45 @@ class LoadWorldTest : public ::testing::Test { // to test that the world instantiation will fail, and the exception // message matches the given regex string - void test_yaml_fail(std::string regex_str) { + void test_yaml_fail(std::string regex_str) + { // do a regex match against error messages std::cmatch match; std::regex regex(regex_str); try { - std::shared_ptr node = - rclcpp::Node::make_shared("test_yaml_fail_node"); + std::shared_ptr node = rclcpp::Node::make_shared("test_yaml_fail_node"); w = World::MakeWorld(node, world_yaml.string()); ADD_FAILURE() << "Expected an exception, but none were raised"; - } catch (const Exception &e) { + } catch (const Exception & e) { EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception &e) { + << "Exception Message '" + std::string(e.what()) + "'" + " did not match against regex '" + + regex_str + "'"; + } catch (const std::exception & e) { ADD_FAILURE() << "Expected flatland_server::Exception, another exception " "was caught instead: " << e.what(); } } - bool float_cmp(double n1, double n2) { + bool float_cmp(double n1, double n2) + { bool ret = std::fabs(n1 - n2) < 1e-5; return ret; } // return the id if found, -1 otherwise - int does_edge_exist(const b2EdgeShape &edge, - const std::vector> &edges) { + int does_edge_exist( + const b2EdgeShape & edge, const std::vector> & edges) + { for (unsigned int i = 0; i < edges.size(); i++) { auto e = edges[i]; - if ((float_cmp(edge.m_vertex1.x, e.first.x) && - float_cmp(edge.m_vertex1.y, e.first.y) && - float_cmp(edge.m_vertex2.x, e.second.x) && - float_cmp(edge.m_vertex2.y, e.second.y)) || - - (float_cmp(edge.m_vertex1.x, e.second.x) && - float_cmp(edge.m_vertex1.y, e.second.y) && - float_cmp(edge.m_vertex2.x, e.first.x) && - float_cmp(edge.m_vertex2.y, e.first.y))) { + if ( + (float_cmp(edge.m_vertex1.x, e.first.x) && float_cmp(edge.m_vertex1.y, e.first.y) && + float_cmp(edge.m_vertex2.x, e.second.x) && float_cmp(edge.m_vertex2.y, e.second.y)) || + + (float_cmp(edge.m_vertex1.x, e.second.x) && float_cmp(edge.m_vertex1.y, e.second.y) && + float_cmp(edge.m_vertex2.x, e.first.x) && float_cmp(edge.m_vertex2.y, e.first.y))) { return i; } } @@ -130,8 +132,8 @@ class LoadWorldTest : public ::testing::Test { // checks if one list of edges completely matches the content // of the other list bool do_edges_exactly_match( - const std::vector &edges1, - const std::vector> &edges2) { + const std::vector & edges1, const std::vector> & edges2) + { std::vector> edges_cpy = edges2; for (unsigned int i = 0; i < edges1.size(); i++) { auto e = edges1[i]; @@ -151,25 +153,26 @@ class LoadWorldTest : public ::testing::Test { } } - bool ColorEq(const Color &c1, const Color &c2) { + bool ColorEq(const Color & c1, const Color & c2) + { if (c1 != c2) { - printf("Color Actual:[%f,%f,%f,%f] != Expected:[%f,%f,%f,%f]\n", c1.r, - c1.g, c1.b, c1.a, c2.r, c2.g, c2.b, c2.a); + printf( + "Color Actual:[%f,%f,%f,%f] != Expected:[%f,%f,%f,%f]\n", c1.r, c1.g, c1.b, c1.a, c2.r, + c2.g, c2.b, c2.a); return false; } return true; } - bool BodyEq(Body *body, const std::string name, b2BodyType type, - const std::array &pose, - const std::array &color, double linear_damping, - double angular_damping) { + bool BodyEq( + Body * body, const std::string name, b2BodyType type, const std::array & pose, + const std::array & color, double linear_damping, double angular_damping) + { b2Vec2 t = body->physics_body_->GetPosition(); double a = body->physics_body_->GetAngle(); if (name != body->name_) { - printf("Name Actual:%s != Expected:%s\n", body->name_.c_str(), - name.c_str()); + printf("Name Actual:%s != Expected:%s\n", body->name_.c_str(), name.c_str()); return false; } @@ -182,15 +185,13 @@ class LoadWorldTest : public ::testing::Test { b2_dynamicBody }; */ - printf("Body type Actual:%d != Expected:%d\n", - body->physics_body_->GetType(), type); + printf("Body type Actual:%d != Expected:%d\n", body->physics_body_->GetType(), type); return false; } - if (!float_cmp(t.x, pose[0]) || !float_cmp(t.y, pose[1]) || - !float_cmp(a, pose[2])) { - printf("Pose Actual:[%f,%f,%f] != Expected:[%f,%f,%f]\n", t.x, t.y, a, - pose[0], pose[1], pose[2]); + if (!float_cmp(t.x, pose[0]) || !float_cmp(t.y, pose[1]) || !float_cmp(a, pose[2])) { + printf( + "Pose Actual:[%f,%f,%f] != Expected:[%f,%f,%f]\n", t.x, t.y, a, pose[0], pose[1], pose[2]); return false; } @@ -199,21 +200,24 @@ class LoadWorldTest : public ::testing::Test { } if (!float_cmp(linear_damping, body->physics_body_->GetLinearDamping())) { - printf("Linear Damping Actual:%f != Expected:%f\n", - body->physics_body_->GetLinearDamping(), linear_damping); + printf( + "Linear Damping Actual:%f != Expected:%f\n", body->physics_body_->GetLinearDamping(), + linear_damping); return false; } if (!float_cmp(angular_damping, body->physics_body_->GetAngularDamping())) { - printf("Angular Damping Actual %f != Expected:%f\n", - body->physics_body_->GetAngularDamping(), angular_damping); + printf( + "Angular Damping Actual %f != Expected:%f\n", body->physics_body_->GetAngularDamping(), + angular_damping); return false; } return true; } - bool CircleEq(b2Fixture *f, double x, double y, double r) { + bool CircleEq(b2Fixture * f, double x, double y, double r) + { if (f->GetShape()->GetType() != b2Shape::e_circle) { /* enum Type @@ -225,23 +229,23 @@ class LoadWorldTest : public ::testing::Test { e_typeCount = 4 }; */ - printf("Shape is not of type b2Shape::e_circle, Actual=%d\n", - f->GetShape()->GetType()); + printf("Shape is not of type b2Shape::e_circle, Actual=%d\n", f->GetShape()->GetType()); return false; } - b2CircleShape *s = dynamic_cast(f->GetShape()); + b2CircleShape * s = dynamic_cast(f->GetShape()); - if (!float_cmp(r, s->m_radius) || !float_cmp(x, s->m_p.x) || - !float_cmp(y, s->m_p.y)) { - printf("Actual:[x=%f,y=%f,r=%f] != Expected:[%f,%f,%f] \n", s->m_p.x, - s->m_p.y, s->m_radius, x, y, r); + if (!float_cmp(r, s->m_radius) || !float_cmp(x, s->m_p.x) || !float_cmp(y, s->m_p.y)) { + printf( + "Actual:[x=%f,y=%f,r=%f] != Expected:[%f,%f,%f] \n", s->m_p.x, s->m_p.y, s->m_radius, x, y, + r); return false; } return true; } - bool PolygonEq(b2Fixture *f, std::vector> points) { + bool PolygonEq(b2Fixture * f, std::vector> points) + { if (f->GetShape()->GetType() != b2Shape::e_polygon) { /* enum Type @@ -253,16 +257,14 @@ class LoadWorldTest : public ::testing::Test { e_typeCount = 4 }; */ - printf("Shape is not of type b2Shape::e_polygon, Actual=%d\n", - f->GetShape()->GetType()); + printf("Shape is not of type b2Shape::e_polygon, Actual=%d\n", f->GetShape()->GetType()); return false; } - b2PolygonShape *s = dynamic_cast(f->GetShape()); + b2PolygonShape * s = dynamic_cast(f->GetShape()); unsigned int cnt = s->m_count; if (cnt != points.size()) { - printf("Number of points Actual:%d != Expected:%lu\n", cnt, - points.size()); + printf("Number of points Actual:%d != Expected:%lu\n", cnt, points.size()); return false; } @@ -274,8 +276,7 @@ class LoadWorldTest : public ::testing::Test { bool found_match = false; unsigned int j; for (j = 0; j < pts.size(); j++) { - if (!float_cmp(p.x, points[i].first) || - !float_cmp(p.y, points[i].second)) { + if (!float_cmp(p.x, points[i].first) || !float_cmp(p.y, points[i].second)) { found_match = true; break; } @@ -307,11 +308,12 @@ class LoadWorldTest : public ::testing::Test { } } - std::vector GetBodyFixtures(Body *body) { + std::vector GetBodyFixtures(Body * body) + { std::vector fixtures; - b2Body *b = body->physics_body_; - for (b2Fixture *f = b->GetFixtureList(); f; f = f->GetNext()) { + b2Body * b = body->physics_body_; + for (b2Fixture * f = b->GetFixtureList(); f; f = f->GetNext()) { fixtures.push_back(f); } @@ -320,29 +322,29 @@ class LoadWorldTest : public ::testing::Test { return fixtures; } - bool FixtureEq(b2Fixture *f, bool is_sensor, int group_index, - uint16_t category_bits, uint16_t mask_bits, double density, - double friction, double restitution) { + bool FixtureEq( + b2Fixture * f, bool is_sensor, int group_index, uint16_t category_bits, uint16_t mask_bits, + double density, double friction, double restitution) + { if (f->IsSensor() != is_sensor) { printf("is_sensor Actual:%d != Expected:%d\n", f->IsSensor(), is_sensor); return false; } if (f->GetFilterData().groupIndex != group_index) { - printf("group_index Actual:%d != Expected:%d\n", - f->GetFilterData().groupIndex, group_index); + printf("group_index Actual:%d != Expected:%d\n", f->GetFilterData().groupIndex, group_index); return false; } if (f->GetFilterData().categoryBits != category_bits) { - printf("category_bits Actual:0x%X != Expected:0x%X\n", - f->GetFilterData().categoryBits, category_bits); + printf( + "category_bits Actual:0x%X != Expected:0x%X\n", f->GetFilterData().categoryBits, + category_bits); return false; } if (f->GetFilterData().maskBits != mask_bits) { - printf("mask_bits Actual:0x%X != Expected:0x%X\n", - f->GetFilterData().maskBits, mask_bits); + printf("mask_bits Actual:0x%X != Expected:0x%X\n", f->GetFilterData().maskBits, mask_bits); return false; } @@ -357,23 +359,22 @@ class LoadWorldTest : public ::testing::Test { } if (!float_cmp(f->GetRestitution(), restitution)) { - printf("restitution Actual:%f != Expected:%f\n", f->GetRestitution(), - restitution); + printf("restitution Actual:%f != Expected:%f\n", f->GetRestitution(), restitution); return false; } return true; } - bool JointEq(Joint *joint, const std::string name, - const std::array &color, Body *body_A, - const std::array &anchor_A, Body *body_B, - const std::array &anchor_B, bool collide_connected) { - b2Joint *j = joint->physics_joint_; + bool JointEq( + Joint * joint, const std::string name, const std::array & color, Body * body_A, + const std::array & anchor_A, Body * body_B, const std::array & anchor_B, + bool collide_connected) + { + b2Joint * j = joint->physics_joint_; if (name != joint->name_) { - printf("Name Actual:%s != Expected:%s\n", joint->name_.c_str(), - name.c_str()); + printf("Name Actual:%s != Expected:%s\n", joint->name_.c_str(), name.c_str()); return false; } @@ -382,15 +383,16 @@ class LoadWorldTest : public ::testing::Test { } if (j->GetBodyA() != body_A->physics_body_) { - printf("BodyA ptr Actual %p != Expected:%p\n", - (void *)joint->physics_joint_->GetBodyA(), - (void *)body_A->physics_body_); + printf( + "BodyA ptr Actual %p != Expected:%p\n", (void *)joint->physics_joint_->GetBodyA(), + (void *)body_A->physics_body_); return false; } if (j->GetBodyB() != body_B->physics_body_) { - printf("BodyB ptr Actual %p != Expected:%p\n", (void *)j->GetBodyB(), - (void *)body_B->physics_body_); + printf( + "BodyB ptr Actual %p != Expected:%p\n", (void *)j->GetBodyB(), + (void *)body_B->physics_body_); return false; } @@ -399,31 +401,33 @@ class LoadWorldTest : public ::testing::Test { b2Vec2 local_anchor_A = j->GetAnchorA() - j->GetBodyA()->GetPosition(); b2Vec2 local_anchor_B = j->GetAnchorB() - j->GetBodyB()->GetPosition(); - if (!float_cmp(local_anchor_A.x, anchor_A[0]) || - !float_cmp(local_anchor_A.y, anchor_A[1])) { - printf("Anchor A Actual:[%f,%f] != Expected:[%f,%f]\n", local_anchor_A.x, - local_anchor_A.y, anchor_A[0], anchor_A[1]); + if (!float_cmp(local_anchor_A.x, anchor_A[0]) || !float_cmp(local_anchor_A.y, anchor_A[1])) { + printf( + "Anchor A Actual:[%f,%f] != Expected:[%f,%f]\n", local_anchor_A.x, local_anchor_A.y, + anchor_A[0], anchor_A[1]); return false; } - if (!float_cmp(local_anchor_B.x, anchor_B[0]) || - !float_cmp(local_anchor_B.y, anchor_B[1])) { - printf("Anchor B Actual:[%f,%f] != Expected:[%f,%f]\n", local_anchor_B.x, - local_anchor_B.y, anchor_B[0], anchor_B[1]); + if (!float_cmp(local_anchor_B.x, anchor_B[0]) || !float_cmp(local_anchor_B.y, anchor_B[1])) { + printf( + "Anchor B Actual:[%f,%f] != Expected:[%f,%f]\n", local_anchor_B.x, local_anchor_B.y, + anchor_B[0], anchor_B[1]); return false; } if (collide_connected != j->GetCollideConnected()) { - printf("Collide connected Actual:%d != Expected:%d\n", - j->GetCollideConnected(), collide_connected); + printf( + "Collide connected Actual:%d != Expected:%d\n", j->GetCollideConnected(), + collide_connected); return false; } return true; } - bool WeldEq(Joint *joint, double angle, double freq, double damping) { - b2WeldJoint *j = dynamic_cast(joint->physics_joint_); + bool WeldEq(Joint * joint, double angle, double freq, double damping) + { + b2WeldJoint * j = dynamic_cast(joint->physics_joint_); if (j->GetType() != e_weldJoint) { /* @@ -443,8 +447,7 @@ class LoadWorldTest : public ::testing::Test { e_motorJoint }; */ - printf("Joint type Actual:%d != Expected:%d(weld joint)\n", j->GetType(), - e_weldJoint); + printf("Joint type Actual:%d != Expected:%d(weld joint)\n", j->GetType(), e_weldJoint); return false; } @@ -459,17 +462,16 @@ class LoadWorldTest : public ::testing::Test { } if (!float_cmp(damping, j->GetDampingRatio())) { - printf("Damping Actual:%f != Expected:%f\n", damping, - j->GetDampingRatio()); + printf("Damping Actual:%f != Expected:%f\n", damping, j->GetDampingRatio()); return false; } return true; } - bool RevoluteEq(Joint *joint, bool is_limit_enabled, - const std::array limits) { - b2RevoluteJoint *j = dynamic_cast(joint->physics_joint_); + bool RevoluteEq(Joint * joint, bool is_limit_enabled, const std::array limits) + { + b2RevoluteJoint * j = dynamic_cast(joint->physics_joint_); if (j->GetType() != e_revoluteJoint) { /* @@ -489,21 +491,22 @@ class LoadWorldTest : public ::testing::Test { e_motorJoint }; */ - printf("Joint type Actual:%d != Expected:%d(revolute joint)\n", - j->GetType(), e_revoluteJoint); + printf( + "Joint type Actual:%d != Expected:%d(revolute joint)\n", j->GetType(), e_revoluteJoint); return false; } if (is_limit_enabled != j->IsLimitEnabled()) { - printf("Limits enabled Actual:%d != Expected:%d\n", is_limit_enabled, - j->IsLimitEnabled()); + printf("Limits enabled Actual:%d != Expected:%d\n", is_limit_enabled, j->IsLimitEnabled()); return false; } - if (is_limit_enabled && (!float_cmp(limits[0], j->GetLowerLimit()) || - !float_cmp(limits[1], j->GetUpperLimit()))) { - printf("Limits Actual:[%f,%f] != Expected:[%f,%f]\n", j->GetLowerLimit(), - j->GetLowerLimit(), limits[0], limits[1]); + if ( + is_limit_enabled && + (!float_cmp(limits[0], j->GetLowerLimit()) || !float_cmp(limits[1], j->GetUpperLimit()))) { + printf( + "Limits Actual:[%f,%f] != Expected:[%f,%f]\n", j->GetLowerLimit(), j->GetLowerLimit(), + limits[0], limits[1]); return false; } @@ -516,11 +519,10 @@ class LoadWorldTest : public ::testing::Test { * yaml file and checks that all configurations, data, and calculations are * correct after instantiation */ -TEST_F(LoadWorldTest, simple_test_A) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - std::shared_ptr node = - rclcpp::Node::make_shared("simple_test_A_node"); +TEST_F(LoadWorldTest, simple_test_A) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); + std::shared_ptr node = rclcpp::Node::make_shared("simple_test_A_node"); w = World::MakeWorld(node, world_yaml.string()); EXPECT_EQ(w->physics_velocity_iterations_, 11); @@ -533,8 +535,8 @@ TEST_F(LoadWorldTest, simple_test_A) { ASSERT_EQ(w->layers_[0]->names_.size(), (unsigned int)1); EXPECT_STREQ(w->layers_[0]->names_[0].c_str(), "2d"); EXPECT_EQ(w->layers_[0]->Type(), Entity::EntityType::LAYER); - EXPECT_TRUE(BodyEq(w->layers_[0]->body_, "2d", b2_staticBody, - {0.05, -0.05, 1.57}, {0, 1, 0, 0.675}, 0, 0)); + EXPECT_TRUE( + BodyEq(w->layers_[0]->body_, "2d", b2_staticBody, {0.05, -0.05, 1.57}, {0, 1, 0, 0.675}, 0, 0)); EXPECT_EQ(w->cfr_.LookUpLayerId("2d"), 0); EXPECT_EQ(w->cfr_.GetCategoryBits(w->layers_[0]->names_), 0b1); @@ -545,8 +547,8 @@ TEST_F(LoadWorldTest, simple_test_A) { EXPECT_STREQ(w->layers_[1]->names_[1].c_str(), "4d"); EXPECT_STREQ(w->layers_[1]->names_[2].c_str(), "5d"); EXPECT_EQ(w->layers_[1]->Type(), Entity::EntityType::LAYER); - EXPECT_TRUE(BodyEq(w->layers_[1]->body_, "3d", b2_staticBody, {0.0, 0.0, 0.0}, - {1, 1, 1, 1}, 0, 0)); + EXPECT_TRUE( + BodyEq(w->layers_[1]->body_, "3d", b2_staticBody, {0.0, 0.0, 0.0}, {1, 1, 1, 1}, 0, 0)); EXPECT_EQ(w->cfr_.LookUpLayerId("3d"), 1); EXPECT_EQ(w->cfr_.LookUpLayerId("4d"), 2); EXPECT_EQ(w->cfr_.LookUpLayerId("5d"), 3); @@ -557,31 +559,30 @@ TEST_F(LoadWorldTest, simple_test_A) { ASSERT_EQ(w->layers_[2]->names_.size(), (unsigned int)1); EXPECT_STREQ(w->layers_[2]->names_[0].c_str(), "lines"); EXPECT_EQ(w->layers_[2]->Type(), Entity::EntityType::LAYER); - EXPECT_TRUE(BodyEq(w->layers_[2]->body_, "lines", b2_staticBody, - {-1.20, -5, 1.23}, {1, 1, 1, 1}, 0, 0)); + EXPECT_TRUE( + BodyEq(w->layers_[2]->body_, "lines", b2_staticBody, {-1.20, -5, 1.23}, {1, 1, 1, 1}, 0, 0)); EXPECT_EQ(w->cfr_.LookUpLayerId("lines"), 4); // check that bitmap is transformed correctly. This involves flipping the y // coordinates and apply the resolution. Note that the translation and // rotation is performed internally by Box2D std::vector> layer0_expected_edges = { - std::pair(b2Vec2(0.00, 0.25), b2Vec2(0.25, 0.25)), - std::pair(b2Vec2(0.05, 0.20), b2Vec2(0.20, 0.20)), - std::pair(b2Vec2(0.10, 0.15), b2Vec2(0.15, 0.15)), - std::pair(b2Vec2(0.10, 0.10), b2Vec2(0.15, 0.10)), - std::pair(b2Vec2(0.05, 0.05), b2Vec2(0.20, 0.05)), - std::pair(b2Vec2(0.00, 0.00), b2Vec2(0.25, 0.00)), - - std::pair(b2Vec2(0.00, 0.25), b2Vec2(0.00, 0.00)), - std::pair(b2Vec2(0.05, 0.20), b2Vec2(0.05, 0.05)), - std::pair(b2Vec2(0.10, 0.15), b2Vec2(0.10, 0.10)), - std::pair(b2Vec2(0.15, 0.15), b2Vec2(0.15, 0.10)), - std::pair(b2Vec2(0.20, 0.20), b2Vec2(0.20, 0.05)), - std::pair(b2Vec2(0.25, 0.25), b2Vec2(0.25, 0.00))}; + std::pair(b2Vec2(0.00, 0.25), b2Vec2(0.25, 0.25)), + std::pair(b2Vec2(0.05, 0.20), b2Vec2(0.20, 0.20)), + std::pair(b2Vec2(0.10, 0.15), b2Vec2(0.15, 0.15)), + std::pair(b2Vec2(0.10, 0.10), b2Vec2(0.15, 0.10)), + std::pair(b2Vec2(0.05, 0.05), b2Vec2(0.20, 0.05)), + std::pair(b2Vec2(0.00, 0.00), b2Vec2(0.25, 0.00)), + + std::pair(b2Vec2(0.00, 0.25), b2Vec2(0.00, 0.00)), + std::pair(b2Vec2(0.05, 0.20), b2Vec2(0.05, 0.05)), + std::pair(b2Vec2(0.10, 0.15), b2Vec2(0.10, 0.10)), + std::pair(b2Vec2(0.15, 0.15), b2Vec2(0.15, 0.10)), + std::pair(b2Vec2(0.20, 0.20), b2Vec2(0.20, 0.05)), + std::pair(b2Vec2(0.25, 0.25), b2Vec2(0.25, 0.00))}; std::vector layer0_edges; - for (b2Fixture *f = w->layers_[0]->body_->physics_body_->GetFixtureList(); f; - f = f->GetNext()) { + for (b2Fixture * f = w->layers_[0]->body_->physics_body_->GetFixtureList(); f; f = f->GetNext()) { b2EdgeShape e = *(dynamic_cast(f->GetShape())); layer0_edges.push_back(e); @@ -595,26 +596,25 @@ TEST_F(LoadWorldTest, simple_test_A) { // layer[1] has origin of [0, 0, 0], so there should be no transform, just // apply the inversion of y coordinates and scale by resolution std::vector> layer1_expected_edges = { - std::pair(b2Vec2(0.0, 7.5), b2Vec2(1.5, 7.5)), - std::pair(b2Vec2(0.0, 7.5), b2Vec2(0.0, 4.5)), - std::pair(b2Vec2(0.0, 4.5), b2Vec2(4.5, 4.5)), - std::pair(b2Vec2(4.5, 4.5), b2Vec2(4.5, 1.5)), - std::pair(b2Vec2(6.0, 3.0), b2Vec2(6.0, 6.0)), - std::pair(b2Vec2(6.0, 6.0), b2Vec2(1.5, 6.0)), - std::pair(b2Vec2(1.5, 7.5), b2Vec2(1.5, 6.0)), - std::pair(b2Vec2(3.0, 3.0), b2Vec2(6.0, 3.0)), - std::pair(b2Vec2(3.0, 0.0), b2Vec2(3.0, 3.0)), - std::pair(b2Vec2(1.5, 1.5), b2Vec2(4.5, 1.5)), - std::pair(b2Vec2(1.5, 0.0), b2Vec2(3.0, 0.0)), - std::pair(b2Vec2(1.5, 1.5), b2Vec2(1.5, 0.0)), - std::pair(b2Vec2(6.0, 1.5), b2Vec2(7.5, 1.5)), - std::pair(b2Vec2(6.0, 1.5), b2Vec2(6.0, 0.0)), - std::pair(b2Vec2(7.5, 1.5), b2Vec2(7.5, 0.0)), - std::pair(b2Vec2(6.0, 0.0), b2Vec2(7.5, 0.0))}; + std::pair(b2Vec2(0.0, 7.5), b2Vec2(1.5, 7.5)), + std::pair(b2Vec2(0.0, 7.5), b2Vec2(0.0, 4.5)), + std::pair(b2Vec2(0.0, 4.5), b2Vec2(4.5, 4.5)), + std::pair(b2Vec2(4.5, 4.5), b2Vec2(4.5, 1.5)), + std::pair(b2Vec2(6.0, 3.0), b2Vec2(6.0, 6.0)), + std::pair(b2Vec2(6.0, 6.0), b2Vec2(1.5, 6.0)), + std::pair(b2Vec2(1.5, 7.5), b2Vec2(1.5, 6.0)), + std::pair(b2Vec2(3.0, 3.0), b2Vec2(6.0, 3.0)), + std::pair(b2Vec2(3.0, 0.0), b2Vec2(3.0, 3.0)), + std::pair(b2Vec2(1.5, 1.5), b2Vec2(4.5, 1.5)), + std::pair(b2Vec2(1.5, 0.0), b2Vec2(3.0, 0.0)), + std::pair(b2Vec2(1.5, 1.5), b2Vec2(1.5, 0.0)), + std::pair(b2Vec2(6.0, 1.5), b2Vec2(7.5, 1.5)), + std::pair(b2Vec2(6.0, 1.5), b2Vec2(6.0, 0.0)), + std::pair(b2Vec2(7.5, 1.5), b2Vec2(7.5, 0.0)), + std::pair(b2Vec2(6.0, 0.0), b2Vec2(7.5, 0.0))}; std::vector layer1_edges; - for (b2Fixture *f = w->layers_[1]->body_->physics_body_->GetFixtureList(); f; - f = f->GetNext()) { + for (b2Fixture * f = w->layers_[1]->body_->physics_body_->GetFixtureList(); f; f = f->GetNext()) { b2EdgeShape e = *(dynamic_cast(f->GetShape())); layer1_edges.push_back(e); @@ -627,13 +627,12 @@ TEST_F(LoadWorldTest, simple_test_A) { // check layer[2] data std::vector> layer2_expected_edges = { - std::pair(b2Vec2(0.1, 0.2), b2Vec2(0.3, 0.4)), - std::pair(b2Vec2(-0.1, -0.2), b2Vec2(-0.3, -0.4)), - std::pair(b2Vec2(0.01, 0.02), b2Vec2(0.03, 0.04))}; + std::pair(b2Vec2(0.1, 0.2), b2Vec2(0.3, 0.4)), + std::pair(b2Vec2(-0.1, -0.2), b2Vec2(-0.3, -0.4)), + std::pair(b2Vec2(0.01, 0.02), b2Vec2(0.03, 0.04))}; std::vector layer2_edges; - for (b2Fixture *f = w->layers_[2]->body_->physics_body_->GetFixtureList(); f; - f = f->GetNext()) { + for (b2Fixture * f = w->layers_[2]->body_->physics_body_->GetFixtureList(); f; f = f->GetNext()) { b2EdgeShape e = *(dynamic_cast(f->GetShape())); layer2_edges.push_back(e); @@ -646,94 +645,94 @@ TEST_F(LoadWorldTest, simple_test_A) { // Check loaded model data // Check model 0 - Model *m0 = w->models_[0]; + Model * m0 = w->models_[0]; EXPECT_STREQ(m0->name_.c_str(), "turtlebot1"); EXPECT_STREQ(m0->namespace_.c_str(), ""); ASSERT_EQ(m0->bodies_.size(), (unsigned int)5); ASSERT_EQ(m0->joints_.size(), (unsigned int)4); // check model 0 body 0 - EXPECT_TRUE(BodyEq(m0->bodies_[0], "base", b2_dynamicBody, {0, 0, 0}, - {1, 1, 0, 0.25}, 0.1, 0.125)); + EXPECT_TRUE( + BodyEq(m0->bodies_[0], "base", b2_dynamicBody, {0, 0, 0}, {1, 1, 0, 0.25}, 0.1, 0.125)); auto fs = GetBodyFixtures(m0->bodies_[0]); ASSERT_EQ(fs.size(), (unsigned int)2); EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0xFFFF, 0xFFFF, 0, 0, 0)); EXPECT_TRUE(CircleEq(fs[0], 0, 0, 1.777)); EXPECT_TRUE(FixtureEq(fs[1], false, 0, 0xFFFF, 0xFFFF, 982.24, 0.59, 0.234)); - EXPECT_TRUE( - PolygonEq(fs[1], {{-0.1, 0.1}, {-0.1, -0.1}, {0.1, -0.1}, {0.1, 0.1}})); + EXPECT_TRUE(PolygonEq(fs[1], {{-0.1, 0.1}, {-0.1, -0.1}, {0.1, -0.1}, {0.1, 0.1}})); // check model 0 body 1 - EXPECT_TRUE(BodyEq(m0->bodies_[1], "left_wheel", b2_dynamicBody, {-1, 0, 0}, - {1, 0, 0, 0.25}, 0, 0)); + EXPECT_TRUE( + BodyEq(m0->bodies_[1], "left_wheel", b2_dynamicBody, {-1, 0, 0}, {1, 0, 0, 0.25}, 0, 0)); fs = GetBodyFixtures(m0->bodies_[1]); ASSERT_EQ(fs.size(), (unsigned int)1); EXPECT_TRUE(FixtureEq(fs[0], true, 0, 0b01, 0b01, 0, 0, 0)); - EXPECT_TRUE(PolygonEq( - fs[0], {{-0.2, 0.75}, {-0.2, -0.75}, {0.2, -0.75}, {0.2, 0.75}})); + EXPECT_TRUE(PolygonEq(fs[0], {{-0.2, 0.75}, {-0.2, -0.75}, {0.2, -0.75}, {0.2, 0.75}})); // check model 0 body 2 - EXPECT_TRUE(BodyEq(m0->bodies_[2], "right_wheel", b2_dynamicBody, {1, 0, 0}, - {0, 1, 0, 0.25}, 0, 0)); + EXPECT_TRUE( + BodyEq(m0->bodies_[2], "right_wheel", b2_dynamicBody, {1, 0, 0}, {0, 1, 0, 0.25}, 0, 0)); fs = GetBodyFixtures(m0->bodies_[2]); ASSERT_EQ(fs.size(), (unsigned int)1); EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0xFFFF, 0xFFFF, 0, 0, 0)); - EXPECT_TRUE(PolygonEq( - fs[0], {{-0.2, 0.75}, {-0.2, -0.75}, {0.2, -0.75}, {0.2, 0.75}})); + EXPECT_TRUE(PolygonEq(fs[0], {{-0.2, 0.75}, {-0.2, -0.75}, {0.2, -0.75}, {0.2, 0.75}})); // check model 0 body 3 - EXPECT_TRUE(BodyEq(m0->bodies_[3], "tail", b2_dynamicBody, {0, 0, 0.52}, - {0, 0, 0, 0.5}, 0, 0)); + EXPECT_TRUE(BodyEq(m0->bodies_[3], "tail", b2_dynamicBody, {0, 0, 0.52}, {0, 0, 0, 0.5}, 0, 0)); fs = GetBodyFixtures(m0->bodies_[3]); ASSERT_EQ(fs.size(), (unsigned int)1); EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0b10, 0b10, 0, 0, 0)); EXPECT_TRUE(PolygonEq(fs[0], {{-0.2, 0}, {-0.2, -5}, {0.2, -5}, {0.2, 0}})); // check model 0 body 4 - EXPECT_TRUE(BodyEq(m0->bodies_[4], "antenna", b2_dynamicBody, {0, 0.5, 0}, - {0.2, 0.4, 0.6, 1}, 0, 0)); + EXPECT_TRUE( + BodyEq(m0->bodies_[4], "antenna", b2_dynamicBody, {0, 0.5, 0}, {0.2, 0.4, 0.6, 1}, 0, 0)); fs = GetBodyFixtures(m0->bodies_[4]); ASSERT_EQ(fs.size(), (unsigned int)1); EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0b0, 0b0, 0, 0, 0)); EXPECT_TRUE(CircleEq(fs[0], 0.01, 0.02, 0.25)); // Check loaded joint data - EXPECT_TRUE(JointEq(m0->joints_[0], "left_wheel_weld", {0.1, 0.2, 0.3, 0.4}, - m0->bodies_[0], {-1, 0}, m0->bodies_[1], {0, 0}, true)); + EXPECT_TRUE(JointEq( + m0->joints_[0], "left_wheel_weld", {0.1, 0.2, 0.3, 0.4}, m0->bodies_[0], {-1, 0}, + m0->bodies_[1], {0, 0}, true)); EXPECT_TRUE(WeldEq(m0->joints_[0], 1.57079633, 10, 0.5)); - EXPECT_TRUE(JointEq(m0->joints_[1], "right_wheel_weld", {1, 1, 1, 0.5}, - m0->bodies_[0], {1, 0}, m0->bodies_[2], {0, 0}, false)); + EXPECT_TRUE(JointEq( + m0->joints_[1], "right_wheel_weld", {1, 1, 1, 0.5}, m0->bodies_[0], {1, 0}, m0->bodies_[2], + {0, 0}, false)); EXPECT_TRUE(WeldEq(m0->joints_[1], 0, 0, 0)); - EXPECT_TRUE(JointEq(m0->joints_[2], "tail_revolute", {1, 1, 1, 0.5}, - m0->bodies_[0], {0, 0}, m0->bodies_[3], {0, 0}, false)); + EXPECT_TRUE(JointEq( + m0->joints_[2], "tail_revolute", {1, 1, 1, 0.5}, m0->bodies_[0], {0, 0}, m0->bodies_[3], {0, 0}, + false)); EXPECT_TRUE(RevoluteEq(m0->joints_[2], false, {})); - EXPECT_TRUE(JointEq(m0->joints_[3], "antenna_revolute", {1, 1, 1, 0.5}, - m0->bodies_[0], {0, 0}, m0->bodies_[4], {0, 0}, true)); + EXPECT_TRUE(JointEq( + m0->joints_[3], "antenna_revolute", {1, 1, 1, 0.5}, m0->bodies_[0], {0, 0}, m0->bodies_[4], + {0, 0}, true)); EXPECT_TRUE(RevoluteEq(m0->joints_[3], true, {-0.002, 3.735})); // Check model 1 is same yaml file as model 1, simply do a simple sanity check - Model *m1 = w->models_[1]; + Model * m1 = w->models_[1]; EXPECT_STREQ(m1->name_.c_str(), "turtlebot2"); EXPECT_STREQ(m1->namespace_.c_str(), "robot2"); ASSERT_EQ(m1->bodies_.size(), (unsigned int)5); ASSERT_EQ(m1->joints_.size(), (unsigned int)4); // check the applied transformation just for the first body - EXPECT_TRUE(BodyEq(m1->bodies_[0], "base", b2_dynamicBody, {3, 4.5, 3.14159}, - {1, 1, 0, 0.25}, 0.1, 0.125)); + EXPECT_TRUE( + BodyEq(m1->bodies_[0], "base", b2_dynamicBody, {3, 4.5, 3.14159}, {1, 1, 0, 0.25}, 0.1, 0.125)); // Check model 2 which is the chair - Model *m2 = w->models_[2]; + Model * m2 = w->models_[2]; EXPECT_STREQ(m2->name_.c_str(), "chair1"); ASSERT_EQ(m2->bodies_.size(), (unsigned int)1); ASSERT_EQ(m2->joints_.size(), (unsigned int)0); // Check model 2 fixtures - EXPECT_TRUE(BodyEq(m2->bodies_[0], "chair", b2_staticBody, {1.2, 3.5, 2.123}, - {1, 1, 1, 0.5}, 0, 0)); + EXPECT_TRUE( + BodyEq(m2->bodies_[0], "chair", b2_staticBody, {1.2, 3.5, 2.123}, {1, 1, 1, 0.5}, 0, 0)); fs = GetBodyFixtures(m2->bodies_[0]); ASSERT_EQ(fs.size(), (unsigned int)2); EXPECT_TRUE(FixtureEq(fs[0], false, 0, 0b1100, 0b1100, 0, 0, 0)); @@ -743,35 +742,35 @@ TEST_F(LoadWorldTest, simple_test_A) { EXPECT_TRUE(CircleEq(fs[1], 0, 0, 0.2)); // Check model 3 which is the chair - Model *m3 = w->models_[3]; + Model * m3 = w->models_[3]; EXPECT_STREQ(m3->name_.c_str(), "person1"); ASSERT_EQ(m3->bodies_.size(), (unsigned int)1); ASSERT_EQ(m3->joints_.size(), (unsigned int)0); // check the body only - EXPECT_TRUE(BodyEq(m3->bodies_[0], "body", b2_kinematicBody, {0, 1, 2}, - {0, 0.75, 0.75, 0.25}, 0, 0)); + EXPECT_TRUE( + BodyEq(m3->bodies_[0], "body", b2_kinematicBody, {0, 1, 2}, {0, 0.75, 0.75, 0.25}, 0, 0)); } /** * This test tries to loads a non-existent world yaml file. It should throw * an exception */ -TEST_F(LoadWorldTest, wrong_world_path) { - world_yaml = - this_file_dir / fs::path("load_world_tests/random_path/world.yaml"); +TEST_F(LoadWorldTest, wrong_world_path) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/random_path/world.yaml"); test_yaml_fail( - "Flatland YAML: File does not exist, " - "path=\".*/load_world_tests/random_path/world.yaml\".*"); + "Flatland YAML: File does not exist, " + "path=\".*/load_world_tests/random_path/world.yaml\".*"); } /** * This test tries to loads a invalid world yaml file. It should throw * an exception. */ -TEST_F(LoadWorldTest, world_invalid_A) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_A/world.yaml"); +TEST_F(LoadWorldTest, world_invalid_A) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/world_invalid_A/world.yaml"); test_yaml_fail("Flatland YAML: Entry \"properties\" does not exist"); } @@ -779,34 +778,34 @@ TEST_F(LoadWorldTest, world_invalid_A) { * This test tries to loads a invalid world yaml file. It should throw * an exception. */ -TEST_F(LoadWorldTest, world_invalid_B) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_B/world.yaml"); +TEST_F(LoadWorldTest, world_invalid_B) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/world_invalid_B/world.yaml"); test_yaml_fail( - "Flatland YAML: Entry \"color\" must have size of exactly 4 \\(in " - "\"layers\" index=0\\)"); + "Flatland YAML: Entry \"color\" must have size of exactly 4 \\(in " + "\"layers\" index=0\\)"); } /** * This test tries to loads a invalid world yaml file. It should throw * an exception. */ -TEST_F(LoadWorldTest, world_invalid_C) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_C/world.yaml"); +TEST_F(LoadWorldTest, world_invalid_C) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/world_invalid_C/world.yaml"); test_yaml_fail( - "Flatland YAML: Entry index=0 contains unrecognized entry\\(s\\) " - "\\{\"random_param_1\", \"random_param_2\", \"random_param_3\"\\} \\(in " - "\"layers\"\\)"); + "Flatland YAML: Entry index=0 contains unrecognized entry\\(s\\) " + "\\{\"random_param_1\", \"random_param_2\", \"random_param_3\"\\} \\(in " + "\"layers\"\\)"); } /** * This test tries to loads a invalid world yaml file. It should throw * an exception. */ -TEST_F(LoadWorldTest, world_invalid_D) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_D/world.yaml"); +TEST_F(LoadWorldTest, world_invalid_D) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/world_invalid_D/world.yaml"); test_yaml_fail("Flatland YAML: Layer with name \"layer\" already exists"); } @@ -814,9 +813,9 @@ TEST_F(LoadWorldTest, world_invalid_D) { * This test tries to loads a invalid world yaml file. It should throw * an exception. */ -TEST_F(LoadWorldTest, world_invalid_E) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_E/world.yaml"); +TEST_F(LoadWorldTest, world_invalid_E) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/world_invalid_E/world.yaml"); test_yaml_fail("Flatland YAML: Model with name \"turtlebot\" already exists"); } @@ -824,24 +823,24 @@ TEST_F(LoadWorldTest, world_invalid_E) { * This test tries to loads a invalid world yaml file. It should throw * an exception. */ -TEST_F(LoadWorldTest, world_invalid_F) { - world_yaml = - this_file_dir / fs::path("load_world_tests/world_invalid_F/world.yaml"); +TEST_F(LoadWorldTest, world_invalid_F) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/world_invalid_F/world.yaml"); test_yaml_fail( - "Flatland YAML: Unable to add 3 additional layer\\(s\\) \\{layer_15, " - "layer_16, layer_17\\}, current layers count is 14, max allowed is 16"); + "Flatland YAML: Unable to add 3 additional layer\\(s\\) \\{layer_15, " + "layer_16, layer_17\\}, current layers count is 14, max allowed is 16"); } /** * This test tries to loads valid world yaml file which in turn tries to * load a invalid map yaml file. It should throw an exception. */ -TEST_F(LoadWorldTest, map_invalid_A) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_A/world.yaml"); +TEST_F(LoadWorldTest, map_invalid_A) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/map_invalid_A/world.yaml"); test_yaml_fail( - "Flatland YAML: Entry \"origin\" does not exist \\(in layer " - "\"2d\"\\)"); + "Flatland YAML: Entry \"origin\" does not exist \\(in layer " + "\"2d\"\\)"); } /** @@ -849,9 +848,9 @@ TEST_F(LoadWorldTest, map_invalid_A) { * file which then inturn tries to load a non-exists map image file. It should * throw an exception */ -TEST_F(LoadWorldTest, map_invalid_B) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_B/world.yaml"); +TEST_F(LoadWorldTest, map_invalid_B) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/map_invalid_B/world.yaml"); test_yaml_fail("Flatland YAML: Failed to load \".*\\.png\" in layer \"2d\""); } @@ -859,150 +858,149 @@ TEST_F(LoadWorldTest, map_invalid_B) { * This test tries to load a invalid map configuration, an exception should be * thrown */ -TEST_F(LoadWorldTest, map_invalid_C) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_C/world.yaml"); - test_yaml_fail( - "Flatland File: Failed to load \".*/map_invalid_C/random_file.dat\""); +TEST_F(LoadWorldTest, map_invalid_C) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/map_invalid_C/world.yaml"); + test_yaml_fail("Flatland File: Failed to load \".*/map_invalid_C/random_file.dat\""); } /** * This test tries to load a invalid map configuration, an exception should be * thrown */ -TEST_F(LoadWorldTest, map_invalid_D) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_D/world.yaml"); +TEST_F(LoadWorldTest, map_invalid_D) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/map_invalid_D/world.yaml"); test_yaml_fail( - "Flatland File: Failed to read line segment from line 6, in file " - "\"map_lines.dat\""); + "Flatland File: Failed to read line segment from line 6, in file " + "\"map_lines.dat\""); } /** * This test tries to load a invalid map configuration, an exception should be * thrown */ -TEST_F(LoadWorldTest, map_invalid_E) { - world_yaml = - this_file_dir / fs::path("load_world_tests/map_invalid_E/world.yaml"); - test_yaml_fail( - "Flatland YAML: Entry \"scale\" does not exist \\(in layer \"lines\"\\)"); +TEST_F(LoadWorldTest, map_invalid_E) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/map_invalid_E/world.yaml"); + test_yaml_fail("Flatland YAML: Entry \"scale\" does not exist \\(in layer \"lines\"\\)"); } /**` * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_A) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_A/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_A) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_A/world.yaml"); test_yaml_fail( - "Flatland YAML: Entry \"bodies\" must be a list \\(in model " - "\"turtlebot\"\\)"); + "Flatland YAML: Entry \"bodies\" must be a list \\(in model " + "\"turtlebot\"\\)"); } /** * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_B) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_B/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_B) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_B/world.yaml"); test_yaml_fail( - "Flatland YAML: Entry \"points\" must have size >= 3 \\(in model " - "\"turtlebot\" body \"base\" \"footprints\" index=1\\)"); + "Flatland YAML: Entry \"points\" must have size >= 3 \\(in model " + "\"turtlebot\" body \"base\" \"footprints\" index=1\\)"); } /** * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_C) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_C/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_C) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_C/world.yaml"); test_yaml_fail( - "Flatland YAML: Entry \"anchor\" must have size of exactly 2 \\(in model " - "\"turtlebot\" joint \"right_wheel_weld\" \"bodies\" index=1\\)"); + "Flatland YAML: Entry \"anchor\" must have size of exactly 2 \\(in model " + "\"turtlebot\" joint \"right_wheel_weld\" \"bodies\" index=1\\)"); } /** * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_D) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_D/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_D) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_D/world.yaml"); test_yaml_fail( - "Flatland YAML: Cannot find body with name \"left_wheel_123\" in model " - "\"turtlebot\" joint \"left_wheel_weld\" \"bodies\" index=1"); + "Flatland YAML: Cannot find body with name \"left_wheel_123\" in model " + "\"turtlebot\" joint \"left_wheel_weld\" \"bodies\" index=1"); } /** * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_E) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_E/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_E) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_E/world.yaml"); test_yaml_fail( - "Flatland YAML: Invalid footprint \"layers\" in model \"turtlebot\" body " - "\"left_wheel\" \"footprints\" index=0, \\{random_layer\\} layer\\(s\\) " - "does not exist"); + "Flatland YAML: Invalid footprint \"layers\" in model \"turtlebot\" body " + "\"left_wheel\" \"footprints\" index=0, \\{random_layer\\} layer\\(s\\) " + "does not exist"); } /** * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_F) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_F/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_F) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_F/world.yaml"); test_yaml_fail( - "Flatland YAML: Entry index=0 contains unrecognized entry\\(s\\) " - "\\{\"random_paramter\"\\} \\(in model \"turtlebot\" body \"base\" " - "\"footprints\"\\)"); + "Flatland YAML: Entry index=0 contains unrecognized entry\\(s\\) " + "\\{\"random_paramter\"\\} \\(in model \"turtlebot\" body \"base\" " + "\"footprints\"\\)"); } /** * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_G) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_G/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_G) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_G/world.yaml"); test_yaml_fail( - "Flatland YAML: Entry body \"base\" contains unrecognized entry\\(s\\) " - "\\{\"random_paramter\"\\} \\(in model \"turtlebot\"\\)"); + "Flatland YAML: Entry body \"base\" contains unrecognized entry\\(s\\) " + "\\{\"random_paramter\"\\} \\(in model \"turtlebot\"\\)"); } /** * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_H) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_H/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_H) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_H/world.yaml"); test_yaml_fail( - "Flatland YAML: Invalid \"bodies\" in \"turtlebot\" model, body with " - "name \"base\" already exists"); + "Flatland YAML: Invalid \"bodies\" in \"turtlebot\" model, body with " + "name \"base\" already exists"); } /** * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_I) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_I/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_I) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_I/world.yaml"); test_yaml_fail( - "Flatland YAML: Invalid \"joints\" in \"turtlebot\" model, joint with " - "name \"wheel_weld\" already exists"); + "Flatland YAML: Invalid \"joints\" in \"turtlebot\" model, joint with " + "name \"wheel_weld\" already exists"); } /** * This test tries to load a invalid model yaml file, it should fail */ -TEST_F(LoadWorldTest, model_invalid_J) { - world_yaml = - this_file_dir / fs::path("load_world_tests/model_invalid_J/world.yaml"); +TEST_F(LoadWorldTest, model_invalid_J) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/model_invalid_J/world.yaml"); test_yaml_fail( - "Flatland YAML: Entry \"points\" must have size <= 8 \\(in model " - "\"turtlebot\" body \"base\" \"footprints\" index=1\\)"); + "Flatland YAML: Entry \"points\" must have size <= 8 \\(in model " + "\"turtlebot\" body \"base\" \"footprints\" index=1\\)"); } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_server/test/model_test.cpp b/flatland_server/test/model_test.cpp index 684f131b..807e568b 100644 --- a/flatland_server/test/model_test.cpp +++ b/flatland_server/test/model_test.cpp @@ -54,18 +54,16 @@ #include "flatland_server/collision_filter_registry.h" // Test the NameSpaceTF method -TEST(TestSuite, testNameSpaceTF) { - std::shared_ptr node = - rclcpp::Node::make_shared("testNameSpaceTF_node"); - flatland_server::Model has_ns(node, nullptr, nullptr, std::string("foo"), - std::string("has_ns")); +TEST(TestSuite, testNameSpaceTF) +{ + std::shared_ptr node = rclcpp::Node::make_shared("testNameSpaceTF_node"); + flatland_server::Model has_ns(node, nullptr, nullptr, std::string("foo"), std::string("has_ns")); // namespace "foo" onto tf "bar" => foo_bar EXPECT_EQ(has_ns.NameSpaceTF("bar"), "foo_bar"); // namespace "foo" onto tf "/bar" => bar EXPECT_EQ(has_ns.NameSpaceTF("/bar"), "bar"); - flatland_server::Model no_ns(node, nullptr, nullptr, std::string(""), - std::string("no_ns")); + flatland_server::Model no_ns(node, nullptr, nullptr, std::string(""), std::string("no_ns")); // namespace "" onto tf "bar" => bar EXPECT_EQ(no_ns.NameSpaceTF("bar"), "bar"); // namespace "" onto tf "/bar" => bar @@ -73,18 +71,16 @@ TEST(TestSuite, testNameSpaceTF) { } // Test the NameSpaceTopic method -TEST(TestSuite, testNameSpaceTopic) { - std::shared_ptr node = - rclcpp::Node::make_shared("testNameSpaceTopic_node"); - flatland_server::Model has_ns(node, nullptr, nullptr, std::string("foo"), - std::string("has_ns")); +TEST(TestSuite, testNameSpaceTopic) +{ + std::shared_ptr node = rclcpp::Node::make_shared("testNameSpaceTopic_node"); + flatland_server::Model has_ns(node, nullptr, nullptr, std::string("foo"), std::string("has_ns")); // namespace "foo" onto tf "bar" => foo_bar EXPECT_EQ(has_ns.NameSpaceTopic("bar"), "foo/bar"); // namespace "foo" onto tf "/bar" => bar EXPECT_EQ(has_ns.NameSpaceTopic("/bar"), "bar"); - flatland_server::Model no_ns(node, nullptr, nullptr, std::string(""), - std::string("no_ns")); + flatland_server::Model no_ns(node, nullptr, nullptr, std::string(""), std::string("no_ns")); // namespace "" onto tf "bar" => bar EXPECT_EQ(no_ns.NameSpaceTopic("bar"), "bar"); // namespace "" onto tf "/bar" => bar @@ -92,7 +88,8 @@ TEST(TestSuite, testNameSpaceTopic) { } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_server/test/null.cpp b/flatland_server/test/null.cpp index f3d90818..d86ef810 100644 --- a/flatland_server/test/null.cpp +++ b/flatland_server/test/null.cpp @@ -53,7 +53,8 @@ TEST(TestSuite, testA) { EXPECT_EQ(1, 1); } TEST(TestSuite, testB) { EXPECT_TRUE(true); } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/flatland_server/test/plugin_manager_test.cpp b/flatland_server/test/plugin_manager_test.cpp index e1d4f686..08f9fb04 100644 --- a/flatland_server/test/plugin_manager_test.cpp +++ b/flatland_server/test/plugin_manager_test.cpp @@ -57,20 +57,22 @@ namespace fs = boost::filesystem; using namespace flatland_server; -class TestModelPlugin : public ModelPlugin { - public: +class TestModelPlugin : public ModelPlugin +{ +public: // variables used for testing double timestep_before; double timestep_after; - Entity *entity; - b2Fixture *fixture_A; - b2Fixture *fixture_B; + Entity * entity; + b2Fixture * fixture_A; + b2Fixture * fixture_B; std::map function_called; TestModelPlugin() { ClearTestingVariables(); } - void ClearTestingVariables() { + void ClearTestingVariables() + { entity = nullptr; fixture_A = nullptr; fixture_B = nullptr; @@ -84,89 +86,93 @@ class TestModelPlugin : public ModelPlugin { function_called["PostSolve"] = false; } - void OnInitialize(const YAML::Node &config) override { - function_called["OnInitialize"] = true; - } + void OnInitialize(const YAML::Node & config) override { function_called["OnInitialize"] = true; } - void BeforePhysicsStep(const Timekeeper &timekeeper) override { + void BeforePhysicsStep(const Timekeeper & timekeeper) override + { function_called["BeforePhysicsStep"] = true; } - void AfterPhysicsStep(const Timekeeper &timekeeper) override { + void AfterPhysicsStep(const Timekeeper & timekeeper) override + { function_called["AfterPhysicsStep"] = true; } - void BeginContact(b2Contact *contact) override { + void BeginContact(b2Contact * contact) override + { function_called["BeginContact"] = true; FilterContact(contact, entity, fixture_A, fixture_B); } - void EndContact(b2Contact *contact) override { + void EndContact(b2Contact * contact) override + { function_called["EndContact"] = true; FilterContact(contact, entity, fixture_A, fixture_B); } - void PreSolve(b2Contact *contact, const b2Manifold *oldManifold) override { + void PreSolve(b2Contact * contact, const b2Manifold * oldManifold) override + { function_called["PreSolve"] = true; FilterContact(contact, entity, fixture_A, fixture_B); } - void PostSolve(b2Contact *contact, const b2ContactImpulse *impulse) override { + void PostSolve(b2Contact * contact, const b2ContactImpulse * impulse) override + { function_called["PostSolve"] = true; FilterContact(contact, entity, fixture_A, fixture_B); } }; -class PluginManagerTest : public ::testing::Test { - protected: +class PluginManagerTest : public ::testing::Test +{ +protected: boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; - World *w; + World * w; - void SetUp() override { + void SetUp() override + { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); w = nullptr; } - void TearDown() override { + void TearDown() override + { if (w != nullptr) { delete w; } } - PluginManagerTest() { - this_file_dir = boost::filesystem::path(__FILE__).parent_path(); - } + PluginManagerTest() { this_file_dir = boost::filesystem::path(__FILE__).parent_path(); } - bool fltcmp(double n1, double n2) { + bool fltcmp(double n1, double n2) + { bool ret = std::fabs(n1 - n2) < 1e-7; return ret; } // checks if tow maps have the same keys - bool key_compare(std::map const &lhs, - std::map const &rhs) { - auto pred = [](decltype(*lhs.begin()) a, decltype(a) b) { - return a.first == b.first; - }; - - return lhs.size() == rhs.size() && - std::equal(lhs.begin(), lhs.end(), rhs.begin(), pred); + bool key_compare(std::map const & lhs, std::map const & rhs) + { + auto pred = [](decltype(*lhs.begin()) a, decltype(a) b) { return a.first == b.first; }; + + return lhs.size() == rhs.size() && std::equal(lhs.begin(), lhs.end(), rhs.begin(), pred); } // Check the true/false if the function is called - bool FunctionCallEq(TestModelPlugin *p, - std::map function_called) { + bool FunctionCallEq(TestModelPlugin * p, std::map function_called) + { if (!key_compare(p->function_called, function_called)) { printf("Two maps does not have the same keys (set of function)\n"); return false; } - for (const auto &func : p->function_called) { + for (const auto & func : p->function_called) { if (func.second != function_called[func.first]) { - printf("%s is %s, expected to be %s\n", func.first.c_str(), - func.second ? "called" : "not called", - function_called[func.first] ? "called" : "not called"); + printf( + "%s is %s, expected to be %s\n", func.first.c_str(), + func.second ? "called" : "not called", + function_called[func.first] ? "called" : "not called"); return false; } } @@ -178,26 +184,24 @@ class PluginManagerTest : public ::testing::Test { * This test moves bodies around and test if all the correct functions are * called with the expected inputs */ -TEST_F(PluginManagerTest, collision_test) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/collision_test/world.yaml"); - std::shared_ptr node = - rclcpp::Node::make_shared("PluginManagerTest_node"); +TEST_F(PluginManagerTest, collision_test) +{ + world_yaml = this_file_dir / fs::path("plugin_manager_tests/collision_test/world.yaml"); + std::shared_ptr node = rclcpp::Node::make_shared("PluginManagerTest_node"); Timekeeper timekeeper(node); timekeeper.SetMaxStepSize(1.0); w = World::MakeWorld(node, world_yaml.string()); - Layer *l = w->layers_[0]; - Model *m0 = w->models_[0]; - Model *m1 = w->models_[1]; - Body *b0 = m0->bodies_[0]; - Body *b1 = m1->bodies_[0]; - PluginManager *pm = &w->plugin_manager_; + Layer * l = w->layers_[0]; + Model * m0 = w->models_[0]; + Model * m1 = w->models_[1]; + Body * b0 = m0->bodies_[0]; + Body * b1 = m1->bodies_[0]; + PluginManager * pm = &w->plugin_manager_; std::shared_ptr shared_p(new TestModelPlugin()); - shared_p->Initialize(node, "TestModelPlugin", "test_model_plugin", m0, - YAML::Node()); + shared_p->Initialize(node, "TestModelPlugin", "test_model_plugin", m0, YAML::Node()); pm->model_plugins_.push_back(shared_p); - TestModelPlugin *p = shared_p.get(); + TestModelPlugin * p = shared_p.get(); // step the world with everything at zero velocity, this should make sure // the collision callbacks gets called @@ -208,13 +212,14 @@ TEST_F(PluginManagerTest, collision_test) { // begin contact should trigger, as well as before and after physics step. // Note that pre and post solve are never called because the fixtures are // set as sensors - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", true}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", true}, - {"EndContact", false}, - {"PreSolve", false}, - {"PostSolve", false}})); + EXPECT_TRUE(FunctionCallEq( + p, {{"OnInitialize", true}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", true}, + {"EndContact", false}, + {"PreSolve", false}, + {"PostSolve", false}})); EXPECT_EQ(p->entity, l); EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); EXPECT_EQ(p->fixture_B->GetType(), b2Shape::e_edge); @@ -226,13 +231,14 @@ TEST_F(PluginManagerTest, collision_test) { // takes two steps for Box2D to genreate collision events, not sure why w->Update(timekeeper); w->Update(timekeeper); - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", false}, - {"EndContact", true}, - {"PreSolve", false}, - {"PostSolve", false}})); + EXPECT_TRUE(FunctionCallEq( + p, {{"OnInitialize", false}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", false}, + {"EndContact", true}, + {"PreSolve", false}, + {"PostSolve", false}})); EXPECT_EQ(p->entity, l); EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); EXPECT_EQ(p->fixture_B->GetType(), b2Shape::e_edge); @@ -244,13 +250,14 @@ TEST_F(PluginManagerTest, collision_test) { b1->physics_body_->SetLinearVelocity(b2Vec2(0, -0.5)); w->Update(timekeeper); w->Update(timekeeper); - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", true}, - {"EndContact", false}, - {"PreSolve", false}, - {"PostSolve", false}})); + EXPECT_TRUE(FunctionCallEq( + p, {{"OnInitialize", false}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", true}, + {"EndContact", false}, + {"PreSolve", false}, + {"PostSolve", false}})); EXPECT_EQ(p->entity, m1); EXPECT_EQ(p->fixture_B, b1->physics_body_->GetFixtureList()); EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); @@ -262,13 +269,14 @@ TEST_F(PluginManagerTest, collision_test) { b1->physics_body_->SetLinearVelocity(b2Vec2(0, -1)); w->Update(timekeeper); w->Update(timekeeper); - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", false}, - {"EndContact", true}, - {"PreSolve", false}, - {"PostSolve", false}})); + EXPECT_TRUE(FunctionCallEq( + p, {{"OnInitialize", false}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", false}, + {"EndContact", true}, + {"PreSolve", false}, + {"PostSolve", false}})); EXPECT_EQ(p->entity, m1); EXPECT_EQ(p->fixture_B, b1->physics_body_->GetFixtureList()); EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); @@ -286,13 +294,14 @@ TEST_F(PluginManagerTest, collision_test) { b0->physics_body_->SetTransform(b2Vec2(0, 0), 0); w->Update(timekeeper); w->Update(timekeeper); - EXPECT_TRUE(FunctionCallEq(p, {{"OnInitialize", false}, - {"BeforePhysicsStep", true}, - {"AfterPhysicsStep", true}, - {"BeginContact", true}, - {"EndContact", false}, - {"PreSolve", true}, - {"PostSolve", true}})); + EXPECT_TRUE(FunctionCallEq( + p, {{"OnInitialize", false}, + {"BeforePhysicsStep", true}, + {"AfterPhysicsStep", true}, + {"BeginContact", true}, + {"EndContact", false}, + {"PreSolve", true}, + {"PostSolve", true}})); EXPECT_EQ(p->entity, l); EXPECT_EQ(p->fixture_A, b0->physics_body_->GetFixtureList()); EXPECT_EQ(p->fixture_B->GetType(), b2Shape::e_edge); @@ -303,103 +312,102 @@ TEST_F(PluginManagerTest, collision_test) { // ros::spin(); } -TEST_F(PluginManagerTest, load_dummy_test) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); +TEST_F(PluginManagerTest, load_dummy_test) +{ + world_yaml = this_file_dir / fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); - ModelPlugin *p = w->plugin_manager_.model_plugins_[0].get(); + ModelPlugin * p = w->plugin_manager_.model_plugins_[0].get(); EXPECT_STREQ(p->GetType().c_str(), "DummyModelPlugin"); EXPECT_STREQ(p->GetName().c_str(), "dummy_test_plugin"); } -TEST_F(PluginManagerTest, plugin_throws_exception) { - world_yaml = - this_file_dir / - fs::path("plugin_manager_tests/plugin_throws_exception/world.yaml"); +TEST_F(PluginManagerTest, plugin_throws_exception) +{ + world_yaml = this_file_dir / fs::path("plugin_manager_tests/plugin_throws_exception/world.yaml"); try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException &e) { + } catch (const PluginException & e) { // do a regex match against error message std::string regex_str = - ".*dummy_param_float must be dummy_test_123456, instead it was " - "\"wrong_message\".*"; + ".*dummy_param_float must be dummy_test_123456, instead it was " + "\"wrong_message\".*"; std::cmatch match; std::regex regex(regex_str); EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception &e) { + << "Exception Message '" + std::string(e.what()) + "'" + " did not match against regex '" + + regex_str + "'"; + } catch (const std::exception & e) { ADD_FAILURE() << "Was expecting a PluginException, another exception was " "caught instead: " << e.what(); } } -TEST_F(PluginManagerTest, nonexistent_plugin) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/nonexistent_plugin/world.yaml"); +TEST_F(PluginManagerTest, nonexistent_plugin) +{ + world_yaml = this_file_dir / fs::path("plugin_manager_tests/nonexistent_plugin/world.yaml"); try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; - } catch (const PluginException &e) { + } catch (const PluginException & e) { std::cmatch match; std::string regex_str = - ".*RandomPlugin with base class type flatland_server::ModelPlugin does " - "not exist.*"; + ".*RandomPlugin with base class type flatland_server::ModelPlugin does " + "not exist.*"; std::regex regex(regex_str); EXPECT_TRUE(std::regex_match(e.what(), match, regex)) - << "Exception Message '" + std::string(e.what()) + "'" + - " did not match against regex '" + regex_str + "'"; - } catch (const std::exception &e) { + << "Exception Message '" + std::string(e.what()) + "'" + " did not match against regex '" + + regex_str + "'"; + } catch (const std::exception & e) { ADD_FAILURE() << "Was expecting a PluginException, another exception was " "caught instead: " << e.what(); } } -TEST_F(PluginManagerTest, invalid_plugin_yaml) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/invalid_plugin_yaml/world.yaml"); +TEST_F(PluginManagerTest, invalid_plugin_yaml) +{ + world_yaml = this_file_dir / fs::path("plugin_manager_tests/invalid_plugin_yaml/world.yaml"); try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; - } catch (const YAMLException &e) { + } catch (const YAMLException & e) { EXPECT_STREQ( - "Flatland YAML: Entry \"name\" does not exist (in model \"turtlebot1\" " - "\"plugins\" index=0)", - e.what()); - } catch (const std::exception &e) { + "Flatland YAML: Entry \"name\" does not exist (in model \"turtlebot1\" " + "\"plugins\" index=0)", + e.what()); + } catch (const std::exception & e) { ADD_FAILURE() << "Was expecting a YAMLException, another exception was " "caught instead: " << e.what(); } } -TEST_F(PluginManagerTest, duplicate_plugin) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/duplicate_plugin/world.yaml"); +TEST_F(PluginManagerTest, duplicate_plugin) +{ + world_yaml = this_file_dir / fs::path("plugin_manager_tests/duplicate_plugin/world.yaml"); try { std::shared_ptr node = rclcpp::Node::make_shared("test_node"); w = World::MakeWorld(node, world_yaml.string()); FAIL() << "Expected an exception, but none were raised"; - } catch (const YAMLException &e) { + } catch (const YAMLException & e) { EXPECT_STREQ( - "Flatland YAML: Invalid \"plugins\" in \"turtlebot1\" model, plugin " - "with name \"dummy_test_plugin\" already exists", - e.what()); - } catch (const std::exception &e) { + "Flatland YAML: Invalid \"plugins\" in \"turtlebot1\" model, plugin " + "with name \"dummy_test_plugin\" already exists", + e.what()); + } catch (const std::exception & e) { ADD_FAILURE() << "Was expecting a YAMLException, another exception was " "caught instead: " << e.what(); @@ -407,7 +415,8 @@ TEST_F(PluginManagerTest, duplicate_plugin) { } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_server/test/service_manager_test.cpp b/flatland_server/test/service_manager_test.cpp index 25a0d8cc..59b9670c 100644 --- a/flatland_server/test/service_manager_test.cpp +++ b/flatland_server/test/service_manager_test.cpp @@ -60,16 +60,18 @@ namespace fs = boost::filesystem; using namespace flatland_server; using namespace std::chrono_literals; -class ServiceManagerTest : public ::testing::Test { - public: - explicit ServiceManagerTest(const std::shared_ptr& node) - : timekeeper(node), node(node) {} +class ServiceManagerTest : public ::testing::Test +{ +public: + explicit ServiceManagerTest(const std::shared_ptr & node) + : timekeeper(node), node(node) + { + } - ServiceManagerTest() - : ServiceManagerTest(rclcpp::Node::make_shared("test_service_manager")) {} + ServiceManagerTest() : ServiceManagerTest(rclcpp::Node::make_shared("test_service_manager")) {} - protected: - SimulationManager* sim_man; +protected: + SimulationManager * sim_man; boost::filesystem::path this_file_dir; boost::filesystem::path world_yaml; boost::filesystem::path robot_yaml; @@ -77,25 +79,28 @@ class ServiceManagerTest : public ::testing::Test { rclcpp::Node::SharedPtr node; std::thread simulation_thread; - void SetUp() override { + void SetUp() override + { sim_man = nullptr; this_file_dir = boost::filesystem::path(__FILE__).parent_path(); timekeeper.SetMaxStepSize(1.0); } - void TearDown() override { + void TearDown() override + { StopSimulationThread(); delete sim_man; } - void StartSimulationThread() { - sim_man = new SimulationManager(node, world_yaml.string(), 1000, 1 / 1000.0, - false, 0); - simulation_thread = std::thread(&ServiceManagerTest::SimulationThread, - dynamic_cast(this)); + void StartSimulationThread() + { + sim_man = new SimulationManager(node, world_yaml.string(), 1000, 1 / 1000.0, false, 0); + simulation_thread = + std::thread(&ServiceManagerTest::SimulationThread, dynamic_cast(this)); } - void StopSimulationThread() { + void StopSimulationThread() + { sim_man->Shutdown(); simulation_thread.join(); } @@ -106,12 +111,11 @@ class ServiceManagerTest : public ::testing::Test { /** * Testing service for loading a model which should succeed */ -TEST_F(ServiceManagerTest, spawn_valid_model) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); +TEST_F(ServiceManagerTest, spawn_valid_model) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); - robot_yaml = this_file_dir / - fs::path("load_world_tests/simple_test_A/person.model.yaml"); + robot_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/person.model.yaml"); auto request = std::make_shared(); request->name = "service_manager_test_robot"; @@ -121,8 +125,7 @@ TEST_F(ServiceManagerTest, spawn_valid_model) { request->pose.y = 102.1; request->pose.theta = 0.23; - auto client = - node->create_client("spawn_model"); + auto client = node->create_client("spawn_model"); // Threading is required since client.call blocks executing until return StartSimulationThread(); @@ -130,22 +133,19 @@ TEST_F(ServiceManagerTest, spawn_valid_model) { ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); ASSERT_TRUE(response->success); ASSERT_STREQ("", response->message.c_str()); - World* w = sim_man->world_; + World * w = sim_man->world_; ASSERT_EQ(5UL, w->models_.size()); EXPECT_STREQ("service_manager_test_robot", w->models_[4]->name_.c_str()); EXPECT_STREQ("robot123", w->models_[4]->namespace_.c_str()); - EXPECT_FLOAT_EQ(101.1, - w->models_[4]->bodies_[0]->physics_body_->GetPosition().x); - EXPECT_FLOAT_EQ(102.1, - w->models_[4]->bodies_[0]->physics_body_->GetPosition().y); + EXPECT_FLOAT_EQ(101.1, w->models_[4]->bodies_[0]->physics_body_->GetPosition().x); + EXPECT_FLOAT_EQ(102.1, w->models_[4]->bodies_[0]->physics_body_->GetPosition().y); EXPECT_FLOAT_EQ(0.23, w->models_[4]->bodies_[0]->physics_body_->GetAngle()); EXPECT_EQ(1UL, w->models_[4]->bodies_.size()); } @@ -153,9 +153,9 @@ TEST_F(ServiceManagerTest, spawn_valid_model) { /** * Testing service for loading a model which should fail */ -TEST_F(ServiceManagerTest, spawn_invalid_model) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); +TEST_F(ServiceManagerTest, spawn_invalid_model) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); robot_yaml = this_file_dir / fs::path("random_path/turtlebot.model.yaml"); @@ -166,16 +166,14 @@ TEST_F(ServiceManagerTest, spawn_invalid_model) { request->pose.y = 2; request->pose.theta = 3; - auto client = - node->create_client("spawn_model"); + auto client = node->create_client("spawn_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); @@ -183,20 +181,20 @@ TEST_F(ServiceManagerTest, spawn_invalid_model) { std::cmatch match; std::string regex_str = - "Flatland YAML: File does not exist, " - "path=\".*/random_path/turtlebot.model.yaml\".*"; + "Flatland YAML: File does not exist, " + "path=\".*/random_path/turtlebot.model.yaml\".*"; std::regex regex(regex_str); EXPECT_TRUE(std::regex_match(response->message.c_str(), match, regex)) - << "Error Message '" + response->message + "'" + - " did not match against regex '" + regex_str + "'"; + << "Error Message '" + response->message + "'" + " did not match against regex '" + regex_str + + "'"; } /** * Testing service for moving a valid model */ -TEST_F(ServiceManagerTest, move_model) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); +TEST_F(ServiceManagerTest, move_model) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); auto request = std::make_shared(); request->name = "turtlebot1"; @@ -204,35 +202,31 @@ TEST_F(ServiceManagerTest, move_model) { request->pose.y = 9.9; request->pose.theta = 0.77; - auto client = - node->create_client("move_model"); + auto client = node->create_client("move_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); ASSERT_TRUE(response->success); - World* w = sim_man->world_; - EXPECT_NEAR(5.5, w->models_[0]->bodies_[0]->physics_body_->GetPosition().x, - 1e-2); - EXPECT_NEAR(9.9, w->models_[0]->bodies_[0]->physics_body_->GetPosition().y, - 1e-2); + World * w = sim_man->world_; + EXPECT_NEAR(5.5, w->models_[0]->bodies_[0]->physics_body_->GetPosition().x, 1e-2); + EXPECT_NEAR(9.9, w->models_[0]->bodies_[0]->physics_body_->GetPosition().y, 1e-2); EXPECT_NEAR(0.77, w->models_[0]->bodies_[0]->physics_body_->GetAngle(), 1e-2); } /** * Testing service for moving a nonexistent model */ -TEST_F(ServiceManagerTest, move_nonexistent_model) { - world_yaml = - this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); +TEST_F(ServiceManagerTest, move_nonexistent_model) +{ + world_yaml = this_file_dir / fs::path("load_world_tests/simple_test_A/world.yaml"); auto request = std::make_shared(); request->name = "not_a_robot"; @@ -240,92 +234,86 @@ TEST_F(ServiceManagerTest, move_nonexistent_model) { request->pose.y = 5; request->pose.theta = 0; - auto client = - node->create_client("move_model"); + auto client = node->create_client("move_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); ASSERT_FALSE(response->success); EXPECT_STREQ( - "Flatland World: failed to move model, model with name " - "\"not_a_robot\" does not exist", - response->message.c_str()); + "Flatland World: failed to move model, model with name " + "\"not_a_robot\" does not exist", + response->message.c_str()); } /** * Testing service for deleting a model */ -TEST_F(ServiceManagerTest, delete_model) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); +TEST_F(ServiceManagerTest, delete_model) +{ + world_yaml = this_file_dir / fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); auto request = std::make_shared(); request->name = "turtlebot1"; - auto client = - node->create_client("delete_model"); + auto client = node->create_client("delete_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); ASSERT_TRUE(response->success); - World* w = sim_man->world_; + World * w = sim_man->world_; // after deleting a mode, there should be one less model, and one less plugin ASSERT_EQ(w->models_.size(), 0UL); ASSERT_EQ(w->plugin_manager_.model_plugins_.size(), 0UL); - size_t count = - std::count_if(w->models_.begin(), w->models_.end(), - [](Model* m) { return m->name_ == "turtlebot1"; }); + size_t count = std::count_if( + w->models_.begin(), w->models_.end(), [](Model * m) { return m->name_ == "turtlebot1"; }); ASSERT_EQ(count, 0UL); } /** * Testing service for deleting a model that does not exist, should fail */ -TEST_F(ServiceManagerTest, delete_nonexistent_model) { - world_yaml = this_file_dir / - fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); +TEST_F(ServiceManagerTest, delete_nonexistent_model) +{ + world_yaml = this_file_dir / fs::path("plugin_manager_tests/load_dummy_test/world.yaml"); auto request = std::make_shared(); request->name = "random_model"; - auto client = - node->create_client("delete_model"); + auto client = node->create_client("delete_model"); StartSimulationThread(); ASSERT_TRUE(client->wait_for_service(1s)); auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != - rclcpp::FutureReturnCode::SUCCESS) { + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { FAIL(); } auto response = result.get(); ASSERT_FALSE(response->success); EXPECT_STREQ( - "Flatland World: failed to delete model, model with name " - "\"random_model\" does not exist", - response->message.c_str()); + "Flatland World: failed to delete model, model with name " + "\"random_model\" does not exist", + response->message.c_str()); } // Run all the tests that were declared with TEST() -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp index 4591ba76..c3088a47 100644 --- a/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp +++ b/flatland_server/test/yaml_preprocessor/yaml_preprocessor_test.cpp @@ -53,45 +53,43 @@ namespace fs = boost::filesystem; using namespace flatland_server; -void compareNodes(const char *p1, YAML::Node &a, YAML::Node &b) { +void compareNodes(const char * p1, YAML::Node & a, YAML::Node & b) +{ try { - EXPECT_STREQ(b[p1].as().c_str(), - a[p1].as().c_str()) - << "at " << p1; + EXPECT_STREQ(b[p1].as().c_str(), a[p1].as().c_str()) << "at " << p1; } catch (...) { ADD_FAILURE() << "Failure to compare " << p1; } } -void compareNodes(const char *p1, int p2, YAML::Node &a, YAML::Node &b) { +void compareNodes(const char * p1, int p2, YAML::Node & a, YAML::Node & b) +{ try { - EXPECT_STREQ(b[p1][p2].as().c_str(), - a[p1][p2].as().c_str()) - << "at " << p1 << ":" << p2; + EXPECT_STREQ(b[p1][p2].as().c_str(), a[p1][p2].as().c_str()) + << "at " << p1 << ":" << p2; } catch (...) { ADD_FAILURE() << "Failure to compare " << p1 << ":" << p2; } } -void compareNodes(const char *p1, const char *p2, YAML::Node &a, - YAML::Node &b) { +void compareNodes(const char * p1, const char * p2, YAML::Node & a, YAML::Node & b) +{ try { - EXPECT_STREQ(b[p1][p2].as().c_str(), - a[p1][p2].as().c_str()) - << "at " << p1 << ":" << p2; + EXPECT_STREQ(b[p1][p2].as().c_str(), a[p1][p2].as().c_str()) + << "at " << p1 << ":" << p2; } catch (...) { ADD_FAILURE() << "Failure to compare " << p1 << ":" << p2; } } // Test the bodyToMarkers method on a polygon shape -TEST(YamlPreprocTest, testEvalStrings) { +TEST(YamlPreprocTest, testEvalStrings) +{ // env vars setenv("VALIDSTRING", "Bar", 1); setenv("VALIDNUMBER", "123.4", 1); - std::shared_ptr node = - rclcpp::Node::make_shared("test_yaml_preprocessor"); + std::shared_ptr node = rclcpp::Node::make_shared("test_yaml_preprocessor"); // ros params node->declare_parameter("/string", "Foo"); node->declare_parameter("/int", 7); @@ -100,11 +98,10 @@ TEST(YamlPreprocTest, testEvalStrings) { YamlPreprocessor yamlPreprocessor(node); boost::filesystem::path cwd = fs::path(__FILE__).parent_path(); - YAML::Node in = yamlPreprocessor.LoadParse( - (cwd / fs::path("/yaml/eval.strings.yaml")).string()); + YAML::Node in = yamlPreprocessor.LoadParse((cwd / fs::path("/yaml/eval.strings.yaml")).string()); - YAML::Node out = yamlPreprocessor.LoadParse( - (cwd / fs::path("/yaml/eval.strings.out.yaml")).string()); + YAML::Node out = + yamlPreprocessor.LoadParse((cwd / fs::path("/yaml/eval.strings.out.yaml")).string()); // check that the two strings match compareNodes("foo", in, out); @@ -141,7 +138,8 @@ TEST(YamlPreprocTest, testEvalStrings) { } // Run all the tests that were declared with TEST() -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/flatland_viz/include/flatland_viz/flatland_viz.h b/flatland_viz/include/flatland_viz/flatland_viz.h index 16969f91..c13fa4fa 100644 --- a/flatland_viz/include/flatland_viz/flatland_viz.h +++ b/flatland_viz/include/flatland_viz/flatland_viz.h @@ -47,7 +47,6 @@ #ifndef FLATLAND_VIZ_FLATLAND_VIZ_H #define FLATLAND_VIZ_FLATLAND_VIZ_H -#include #include #include #include @@ -58,23 +57,22 @@ #include #include #include -#include - -#include "flatland_msgs/msg/debug_topic_list.hpp" +#include #include +#include +#include #include +#include #include #include +#include +#include #include +#include - -#include -#include -#include +#include "flatland_msgs/msg/debug_topic_list.hpp" #include "rviz_common/load_resource.hpp" -#include class QSplashScreen; class QAction; @@ -84,25 +82,27 @@ class QDockWidget; class QLabel; class QToolButton; -namespace rviz_common { +namespace rviz_common +{ class PanelFactory; class Tool; class Display; class RenderPanel; class VisualizationManager; class WidgetGeometryChangeDetector; -} +} // namespace rviz_common class FlatlandWindow; -class FlatlandViz : public QWidget { +class FlatlandViz : public QWidget +{ Q_OBJECT public : - /** - * @brief Construct FlatlandViz and subscribe to debug topic list - * - * @param parent The parent widget - */ - FlatlandViz(FlatlandWindow* parent = 0); + /** + * @brief Construct FlatlandViz and subscribe to debug topic list + * + * @param parent The parent widget + */ + FlatlandViz(FlatlandWindow * parent = 0); /** * @brief Recieve a new DebugTopicList msg and add any new displays required @@ -116,33 +116,33 @@ class FlatlandViz : public QWidget { */ virtual ~FlatlandViz(); - rviz_common::VisualizationManager* manager_; + rviz_common::VisualizationManager * manager_; - private: - rviz_common::RenderPanel* render_panel_; +private: + rviz_common::RenderPanel * render_panel_; - rviz_common::Display* grid_; - rviz_common::Display* interactive_markers_; - std::map debug_displays_; + rviz_common::Display * grid_; + rviz_common::Display * interactive_markers_; + std::map debug_displays_; rclcpp::Subscription::SharedPtr debug_topic_subscriber_; rviz_common::properties::PropertyTreeWidget * tree_widget_; - FlatlandWindow* parent_; + FlatlandWindow * parent_; - QMenu* file_menu_; - QMenu* recent_configs_menu_; - QMenu* view_menu_; - QMenu* delete_view_menu_; - QMenu* plugins_menu_; + QMenu * file_menu_; + QMenu * recent_configs_menu_; + QMenu * view_menu_; + QMenu * delete_view_menu_; + QMenu * plugins_menu_; - QToolBar* toolbar_; + QToolBar * toolbar_; - QActionGroup* toolbar_actions_; - std::map action_to_tool_map_; - std::map tool_to_action_map_; + QActionGroup * toolbar_actions_; + std::map action_to_tool_map_; + std::map tool_to_action_map_; bool show_choose_new_master_option_; - QAction* add_tool_action_; - QMenu* remove_tool_menu_; + QAction * add_tool_action_; + QMenu * remove_tool_menu_; /// Indicates if the toolbar should be visible outside of fullscreen mode. bool toolbar_visible_; @@ -151,12 +151,12 @@ class FlatlandViz : public QWidget { void fullScreenChange(bool hidden); void setDisplayConfigModified(); - void addTool(rviz_common::Tool*); - void removeTool(rviz_common::Tool*); - void refreshTool(rviz_common::Tool*); - void indicateToolIsCurrent(rviz_common::Tool*); - void onToolbarActionTriggered(QAction* action); - void onToolbarRemoveTool(QAction* remove_tool_menu_action); + void addTool(rviz_common::Tool *); + void removeTool(rviz_common::Tool *); + void refreshTool(rviz_common::Tool *); + void indicateToolIsCurrent(rviz_common::Tool *); + void onToolbarActionTriggered(QAction * action); + void onToolbarRemoveTool(QAction * remove_tool_menu_action); void initToolbars(); void initMenus(); void openNewToolDialog(); diff --git a/flatland_viz/include/flatland_viz/flatland_window.h b/flatland_viz/include/flatland_viz/flatland_window.h index b25c6e9d..b3fec4f0 100644 --- a/flatland_viz/include/flatland_viz/flatland_window.h +++ b/flatland_viz/include/flatland_viz/flatland_window.h @@ -46,43 +46,40 @@ // namespace rviz; -#include - -#include -#include -#include -#include #include #include #include #include #include #include +#include #include +#include #include #include #include #include #include - -#include "flatland_viz/flatland_viz.h" - +#include #include #include -class FlatlandWindow : public QMainWindow { +#include "flatland_viz/flatland_viz.h" + +class FlatlandWindow : public QMainWindow +{ Q_OBJECT - public: - FlatlandWindow(QWidget* parent = 0); - rviz_common::VisualizationManager* visualization_manager_; - rviz_common::RenderPanel* render_panel_; +public: + FlatlandWindow(QWidget * parent = 0); + rviz_common::VisualizationManager * visualization_manager_; + rviz_common::RenderPanel * render_panel_; - rviz_common::VisualizationManager* getManager(); + rviz_common::VisualizationManager * getManager(); - protected Q_SLOTS: +protected Q_SLOTS: void openNewToolDialog(); - private: - FlatlandViz* viz_; +private: + FlatlandViz * viz_; }; diff --git a/flatland_viz/include/flatland_viz/load_model_dialog.h b/flatland_viz/include/flatland_viz/load_model_dialog.h index 148f0ae8..0143123a 100644 --- a/flatland_viz/include/flatland_viz/load_model_dialog.h +++ b/flatland_viz/include/flatland_viz/load_model_dialog.h @@ -49,68 +49,67 @@ #define LOAD_MODEL_DIALOG_H // #include +#include +#include +#include +#include + #include +#include #include #include #include #include #include #include +#include #include #include -#include -#include - -#include -#include -#include -#include //#include +#include #include - #include #include - -#include - -namespace flatland_viz { +namespace flatland_viz +{ class SpawnModelTool; } -class LoadModelDialog : public QDialog { - public: +class LoadModelDialog : public QDialog +{ +public: /** * @name LoadModelDialog * @brief Launch load model dialog * @param parent, parent widget pointer * @param tool, pointer to this so dialog can call methods - */ - LoadModelDialog(QWidget *parent, flatland_viz::SpawnModelTool *tool); + */ + LoadModelDialog(QWidget * parent, flatland_viz::SpawnModelTool * tool); - private: +private: /** * @name ChooseFile * @brief Launch file selection dialog - */ + */ QString ChooseFile(); /** * @name AddNumberAndUpdateName * @brief Add numbering to name and show in the name widget - */ + */ void AddNumberAndUpdateName(); static QString path_to_model_file; // full path to model file - static int count; // counter for adding unique number to filename - static bool numbering; // flag to use unique numbering + static int count; // counter for adding unique number to filename + static bool numbering; // flag to use unique numbering - flatland_viz::SpawnModelTool *tool_; - QLineEdit *n_edit; // name lineEdit widget - QLabel *p_label; // path label widget - QCheckBox *n_checkbox; // checkbox widget + flatland_viz::SpawnModelTool * tool_; + QLineEdit * n_edit; // name lineEdit widget + QLabel * p_label; // path label widget + QCheckBox * n_checkbox; // checkbox widget - public Q_SLOTS: +public Q_SLOTS: /** * @name NumberCheckBoxChanged * @brief Checkbox was clicked, toggle numbering of names diff --git a/flatland_viz/include/flatland_viz/model_dialog.h b/flatland_viz/include/flatland_viz/model_dialog.h index 3a0f526a..f350a630 100644 --- a/flatland_viz/include/flatland_viz/model_dialog.h +++ b/flatland_viz/include/flatland_viz/model_dialog.h @@ -48,18 +48,18 @@ #ifndef MODEL_DIALOG_H #define MODEL_DIALOG_H +#include +#include +#include + #include #include #include #include - -#include -#include -#include -#include -#include #include +#include #include +#include #include //#include #include @@ -74,15 +74,16 @@ class DialogOptionsWidget; namespace fs = boost::filesystem; using namespace flatland_server; -class ModelDialog : public QDialog { +class ModelDialog : public QDialog +{ Q_OBJECT - public: +public: static QColor saved_color_; - ModelDialog(QWidget* parent = 0); + ModelDialog(QWidget * parent = 0); - private Q_SLOTS: +private Q_SLOTS: /** * @name SetColor * @brief Callback to pop up a ColorDialog @@ -109,7 +110,7 @@ class ModelDialog : public QDialog { * @param[in] QColor, color to set button to (incl alpha) * @param[in] QPushButton, button to set color on */ - void SetButtonColor(const QColor* c, QPushButton* b); + void SetButtonColor(const QColor * c, QPushButton * b); /** * @name SpawnModelClient * @brief Makes a call to spawn model ros service @@ -117,17 +118,17 @@ class ModelDialog : public QDialog { void SpawnModelClient(); - private: - QPushButton* color_button; +private: + QPushButton * color_button; QString path_to_model_file; QLineEdit *x_edit, *y_edit, *a_edit, *n_edit; - protected: +protected: boost::filesystem::path this_file_dir; ros::NodeHandle nh; ros::ServiceClient client; flatland_msgs::msg::SpawnModel srv; - World* w; + World * w; }; #endif \ No newline at end of file diff --git a/flatland_viz/include/flatland_viz/pause_sim_tool.h b/flatland_viz/include/flatland_viz/pause_sim_tool.h index 119066be..8e49afc3 100644 --- a/flatland_viz/include/flatland_viz/pause_sim_tool.h +++ b/flatland_viz/include/flatland_viz/pause_sim_tool.h @@ -46,19 +46,21 @@ #include #include -namespace flatland_viz { +namespace flatland_viz +{ /** * @name PauseSimTool * @brief Rviz tool to support pausing and unpausing the * simulation. */ -class PauseSimTool : public rviz_common::Tool { - public: +class PauseSimTool : public rviz_common::Tool +{ +public: PauseSimTool(); ~PauseSimTool(); - private: +private: /** * @name onInitialize * @brief Initializes tools currently loaded when rviz starts @@ -80,6 +82,6 @@ class PauseSimTool : public rviz_common::Tool { rclcpp::Client::SharedPtr pause_service_; }; -} +} // namespace flatland_viz #endif // PAUSE_SIM_TOOL_H diff --git a/flatland_viz/include/flatland_viz/spawn_model_tool.h b/flatland_viz/include/flatland_viz/spawn_model_tool.h index 04cce3c7..f640d5f7 100644 --- a/flatland_viz/include/flatland_viz/spawn_model_tool.h +++ b/flatland_viz/include/flatland_viz/spawn_model_tool.h @@ -48,37 +48,38 @@ #ifndef SPAWN_MODEL_TOOL_H #define SPAWN_MODEL_TOOL_H -#include -#include -#include - -#include - #include #include #include #include + +#include +#include +#include +#include //#include #include -#include +#include #include -#include #include -#include "flatland_viz/load_model_dialog.h" +#include +#include "flatland_viz/load_model_dialog.h" -namespace flatland_viz { +namespace flatland_viz +{ /** * @name SpawnModelTool * @brief Every tool which can be added to the tool bar is a * subclass of rviz_common::Tool. */ -class SpawnModelTool : public rviz_common::Tool { +class SpawnModelTool : public rviz_common::Tool +{ Q_OBJECT - public: +public: SpawnModelTool(); ~SpawnModelTool(); @@ -99,12 +100,12 @@ class SpawnModelTool : public rviz_common::Tool { */ void SaveName(QString n); /** - * @name SpawnModelInFlatland - * @brief Spawns a model using ros service - */ + * @name SpawnModelInFlatland + * @brief Spawns a model using ros service + */ void SpawnModelInFlatland(); - private: +private: /** * @name onInitialize * @brief Initializes tools currently loaded when rviz starts @@ -126,12 +127,12 @@ class SpawnModelTool : public rviz_common::Tool { * @brief Main loop of tool * @param event Mouse event */ - virtual int processMouseEvent(rviz_common::ViewportMouseEvent &event); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent & event); /** - * @name SetMovingModelColor - * @brief Set the color of the moving model - * @param c QColor to set the 3d model - */ + * @name SetMovingModelColor + * @brief Set the color of the moving model + * @param c QColor to set the 3d model + */ void SetMovingModelColor(QColor c); /** * @name LoadPreview @@ -144,30 +145,29 @@ class SpawnModelTool : public rviz_common::Tool { * @param footprint The footprint yaml node * @param pose x,y,theta pose of footprint */ - void LoadPolygonFootprint(flatland_server::YamlReader &footprint, - const flatland_server::Pose pose); + void LoadPolygonFootprint( + flatland_server::YamlReader & footprint, const flatland_server::Pose pose); /** * @name LoadCircleFootprint * @brief Load a vector preview of the model's circle footprint * @param footprint The footprint yaml node * @param pose x,y,theta pose of footprint */ - void LoadCircleFootprint(flatland_server::YamlReader &footprint, - const flatland_server::Pose pose); + void LoadCircleFootprint( + flatland_server::YamlReader & footprint, const flatland_server::Pose pose); - Ogre::Vector3 - intersection; // location cursor intersects ground plane, ie the - // location to create the model - float initial_angle; // the angle to create the model at - Ogre::SceneNode *moving_model_node_; // the node for the 3D object + Ogre::Vector3 intersection; // location cursor intersects ground plane, ie + // the location to create the model + float initial_angle; // the angle to create the model at + Ogre::SceneNode * moving_model_node_; // the node for the 3D object enum ModelState { m_hidden, m_dragging, m_rotating }; - ModelState model_state; // model state, first hidden, then dragging to - // intersection point, then rotating to desired angle + ModelState model_state; // model state, first hidden, then dragging to + // intersection point, then rotating to desired angle static QString path_to_model_file_; // full path to model file (yaml) - static QString model_name; // base file name with path and extension removed + static QString model_name; // base file name with path and extension removed - protected: - rviz_rendering::Arrow *arrow_; // Rviz 3d arrow to show axis of rotation +protected: + rviz_rendering::Arrow * arrow_; // Rviz 3d arrow to show axis of rotation rclcpp::Client::SharedPtr client; std::vector> lines_list_; }; diff --git a/flatland_viz/src/flatland_viz.cpp b/flatland_viz/src/flatland_viz.cpp index c42cf067..e0590d7c 100644 --- a/flatland_viz/src/flatland_viz.cpp +++ b/flatland_viz/src/flatland_viz.cpp @@ -44,7 +44,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "flatland_viz/flatland_viz.h" + #include +#include #include #include @@ -64,18 +67,14 @@ #include #include #include -#include #include - #include -#include #include "flatland_viz/flatland_window.h" -#include "flatland_viz/flatland_viz.h" - // Constructor. -FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { +FlatlandViz::FlatlandViz(FlatlandWindow * parent) : QWidget((QWidget *)parent) +{ parent_ = parent; toolbar_ = parent->addToolBar("Tools"); @@ -86,7 +85,7 @@ FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { // Construct and lay out render panel. render_panel_ = new rviz_common::RenderPanel(); - QVBoxLayout* main_layout = new QVBoxLayout; + QVBoxLayout * main_layout = new QVBoxLayout; main_layout->setMargin(0); main_layout->addWidget(render_panel_); @@ -103,17 +102,19 @@ FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { render_panel_->initialize(manager_->getSceneManager(), manager_); // bind toolbar events - rviz_common::ToolManager* tool_man = manager_->getToolManager(); + rviz_common::ToolManager * tool_man = manager_->getToolManager(); - connect(manager_, SIGNAL(configChanged()), this, - SLOT(setDisplayConfigModified())); + connect(manager_, SIGNAL(configChanged()), this, SLOT(setDisplayConfigModified())); connect(tool_man, &rviz_common::ToolManager::toolAdded, this, &FlatlandViz::addTool); - connect(tool_man, SIGNAL(toolRemoved(rviz_common::Tool*)), this, - SLOT(removeTool(rviz_common::Tool*))); - connect(tool_man, SIGNAL(toolRefreshed(rviz_common::Tool*)), this, - SLOT(refreshTool(rviz_common::Tool*))); - connect(tool_man, SIGNAL(toolChanged(rviz_common::Tool*)), this, - SLOT(indicateToolIsCurrent(rviz_common::Tool*))); + connect( + tool_man, SIGNAL(toolRemoved(rviz_common::Tool *)), this, + SLOT(removeTool(rviz_common::Tool *))); + connect( + tool_man, SIGNAL(toolRefreshed(rviz_common::Tool *)), this, + SLOT(refreshTool(rviz_common::Tool *))); + connect( + tool_man, SIGNAL(toolChanged(rviz_common::Tool *)), this, + SLOT(indicateToolIsCurrent(rviz_common::Tool *))); manager_->initialize(); @@ -141,43 +142,45 @@ FlatlandViz::FlatlandViz(FlatlandWindow* parent) : QWidget((QWidget*)parent) { grid_->subProp("Alpha")->setValue(0.1); // Create interactive markers display - interactive_markers_ = - manager_->createDisplay("rviz/InteractiveMarkers", "Move Objects", false); + interactive_markers_ = manager_->createDisplay("rviz/InteractiveMarkers", "Move Objects", false); if (interactive_markers_ == nullptr) { RCLCPP_WARN(rclcpp::get_logger("flatland_viz"), "Interactive markers failed to instantiate"); exit(1); } - interactive_markers_->subProp("Update Topic") - ->setValue("/interactive_model_markers/update"); + interactive_markers_->subProp("Update Topic")->setValue("/interactive_model_markers/update"); std::shared_ptr node = rclcpp::Node::make_shared("flatland_viz"); // Subscribe to debug topics topic using std::placeholders::_1; debug_topic_subscriber_ = node->create_subscription( - "/flatland_server/debug/topics", 0, std::bind(&FlatlandViz::RecieveDebugTopics, this, _1)); + "/flatland_server/debug/topics", 0, std::bind(&FlatlandViz::RecieveDebugTopics, this, _1)); } // Destructor. -FlatlandViz::~FlatlandViz() { +FlatlandViz::~FlatlandViz() +{ delete render_panel_; delete manager_; } -void FlatlandViz::indicateToolIsCurrent(rviz_common::Tool* tool) { - QAction* action = tool_to_action_map_[tool]; +void FlatlandViz::indicateToolIsCurrent(rviz_common::Tool * tool) +{ + QAction * action = tool_to_action_map_[tool]; if (action) { action->setChecked(true); } } -void FlatlandViz::setDisplayConfigModified() { +void FlatlandViz::setDisplayConfigModified() +{ RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "setDisplayConfigModified called"); } -void FlatlandViz::addTool(rviz_common::Tool* tool) { +void FlatlandViz::addTool(rviz_common::Tool * tool) +{ RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "addTool called"); - QAction* action = new QAction(tool->getName(), toolbar_actions_); + QAction * action = new QAction(tool->getName(), toolbar_actions_); action->setIcon(tool->getIcon()); action->setIconText(tool->getName()); action->setCheckable(true); @@ -188,11 +191,12 @@ void FlatlandViz::addTool(rviz_common::Tool* tool) { remove_tool_menu_->addAction(tool->getName()); } -void FlatlandViz::onToolbarActionTriggered(QAction* action) { +void FlatlandViz::onToolbarActionTriggered(QAction * action) +{ RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "onToolbarActionTriggered called"); - rviz_common::Tool* current_tool = manager_->getToolManager()->getCurrentTool(); - rviz_common::Tool* tool = action_to_tool_map_[action]; + rviz_common::Tool * current_tool = manager_->getToolManager()->getCurrentTool(); + rviz_common::Tool * tool = action_to_tool_map_[action]; if (tool) { manager_->getToolManager()->setCurrentTool(tool); @@ -215,9 +219,10 @@ void FlatlandViz::onToolbarActionTriggered(QAction* action) { } } -void FlatlandViz::removeTool(rviz_common::Tool* tool) { +void FlatlandViz::removeTool(rviz_common::Tool * tool) +{ RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "removeTool called"); - QAction* action = tool_to_action_map_[tool]; + QAction * action = tool_to_action_map_[tool]; if (action) { toolbar_actions_->removeAction(action); toolbar_->removeAction(action); @@ -225,10 +230,11 @@ void FlatlandViz::removeTool(rviz_common::Tool* tool) { action_to_tool_map_.erase(action); } QString tool_name = tool->getName(); - QList remove_tool_actions = remove_tool_menu_->actions(); + QList remove_tool_actions = remove_tool_menu_->actions(); for (int i = 0; i < remove_tool_actions.size(); i++) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("flatland_viz"), "Removing --------> " << tool_name.toStdString()); - QAction* removal_action = remove_tool_actions.at(i); + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("flatland_viz"), "Removing --------> " << tool_name.toStdString()); + QAction * removal_action = remove_tool_actions.at(i); if (removal_action->text() == tool_name) { remove_tool_menu_->removeAction(removal_action); break; @@ -236,17 +242,18 @@ void FlatlandViz::removeTool(rviz_common::Tool* tool) { } } -void FlatlandViz::initMenus() { +void FlatlandViz::initMenus() +{ file_menu_ = parent_->menuBar()->addMenu("&File"); - QAction* file_menu_open_action = file_menu_->addAction( - "&Open Config", this, SLOT(onOpen()), QKeySequence("Ctrl+O")); + QAction * file_menu_open_action = + file_menu_->addAction("&Open Config", this, SLOT(onOpen()), QKeySequence("Ctrl+O")); this->addAction(file_menu_open_action); - QAction* file_menu_save_action = file_menu_->addAction( - "&Save Config", this, SLOT(onSave()), QKeySequence("Ctrl+S")); + QAction * file_menu_save_action = + file_menu_->addAction("&Save Config", this, SLOT(onSave()), QKeySequence("Ctrl+S")); this->addAction(file_menu_save_action); - QAction* file_menu_save_as_action = file_menu_->addAction( - "Save Config &As", this, SLOT(onSaveAs()), QKeySequence("Ctrl+Shift+S")); + QAction * file_menu_save_as_action = + file_menu_->addAction("Save Config &As", this, SLOT(onSaveAs()), QKeySequence("Ctrl+Shift+S")); this->addAction(file_menu_save_as_action); recent_configs_menu_ = file_menu_->addMenu("&Recent Configs"); @@ -257,8 +264,8 @@ void FlatlandViz::initMenus() { } file_menu_->addSeparator(); - QAction* file_menu_quit_action = file_menu_->addAction( - "&Quit", this, SLOT(close()), QKeySequence("Ctrl+Q")); + QAction * file_menu_quit_action = + file_menu_->addAction("&Quit", this, SLOT(close()), QKeySequence("Ctrl+Q")); this->addAction(file_menu_quit_action); view_menu_ = parent_->menuBar()->addMenu("&Panels"); @@ -266,8 +273,8 @@ void FlatlandViz::initMenus() { delete_view_menu_ = view_menu_->addMenu("&Delete Panel"); delete_view_menu_->setEnabled(false); - QAction* fullscreen_action = view_menu_->addAction( - "&Fullscreen", this, SLOT(setFullScreen(bool)), Qt::Key_F11); + QAction * fullscreen_action = + view_menu_->addAction("&Fullscreen", this, SLOT(setFullScreen(bool)), Qt::Key_F11); fullscreen_action->setCheckable(true); this->addAction(fullscreen_action); // Also add to window, or the shortcut // doest work when the menu is hidden. @@ -277,14 +284,15 @@ void FlatlandViz::initMenus() { new QShortcut(Qt::Key_Escape, this, SLOT(exitFullScreen())); view_menu_->addSeparator(); - QMenu* help_menu = parent_->menuBar()->addMenu("&Help"); + QMenu * help_menu = parent_->menuBar()->addMenu("&Help"); help_menu->addAction("Show &Help panel", this, SLOT(showHelpPanel())); help_menu->addAction("Open rviz wiki in browser", this, SLOT(onHelpWiki())); help_menu->addSeparator(); help_menu->addAction("&About", this, SLOT(onHelpAbout())); } -void FlatlandViz::initToolbars() { +void FlatlandViz::initToolbars() +{ QFont font; font.setPointSize(font.pointSizeF() * 0.9); @@ -295,39 +303,35 @@ void FlatlandViz::initToolbars() { toolbar_->setObjectName("Tools"); toolbar_->setToolButtonStyle(Qt::ToolButtonTextBesideIcon); toolbar_actions_ = new QActionGroup(this); - connect(toolbar_actions_, &QActionGroup::triggered, this, - &FlatlandViz::onToolbarActionTriggered); + connect(toolbar_actions_, &QActionGroup::triggered, this, &FlatlandViz::onToolbarActionTriggered); add_tool_action_ = new QAction("", toolbar_actions_); add_tool_action_->setToolTip("Add a new tool"); add_tool_action_->setIcon(rviz_common::loadPixmap("package://rviz_common/icons/plus.png")); toolbar_->addAction(add_tool_action_); - connect(add_tool_action_, &QAction::triggered, this, - &FlatlandViz::openNewToolDialog); + connect(add_tool_action_, &QAction::triggered, this, &FlatlandViz::openNewToolDialog); remove_tool_menu_ = new QMenu(); - QToolButton* remove_tool_button = new QToolButton(); + QToolButton * remove_tool_button = new QToolButton(); remove_tool_button->setMenu(remove_tool_menu_); remove_tool_button->setPopupMode(QToolButton::InstantPopup); remove_tool_button->setToolTip("Remove a tool from the toolbar"); - remove_tool_button->setIcon( - rviz_common::loadPixmap("package://rviz_common/icons/minus.png")); + remove_tool_button->setIcon(rviz_common::loadPixmap("package://rviz_common/icons/minus.png")); toolbar_->addWidget(remove_tool_button); - connect(remove_tool_menu_, &QMenu::triggered, this, - &FlatlandViz::onToolbarRemoveTool); + connect(remove_tool_menu_, &QMenu::triggered, this, &FlatlandViz::onToolbarRemoveTool); } -void FlatlandViz::openNewToolDialog() { +void FlatlandViz::openNewToolDialog() +{ RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "openNewToolDialog called"); QString class_id; QStringList empty; - rviz_common::ToolManager* tool_man = manager_->getToolManager(); + rviz_common::ToolManager * tool_man = manager_->getToolManager(); - rviz_common::NewObjectDialog* dialog = - new rviz_common::NewObjectDialog(tool_man->getFactory(), "Tool", empty, - tool_man->getToolClasses(), &class_id); + rviz_common::NewObjectDialog * dialog = new rviz_common::NewObjectDialog( + tool_man->getFactory(), "Tool", empty, tool_man->getToolClasses(), &class_id); manager_->stopUpdate(); if (dialog->exec() == QDialog::Accepted) { tool_man->addTool(class_id); @@ -336,12 +340,13 @@ void FlatlandViz::openNewToolDialog() { activateWindow(); // Force keyboard focus back on main window. } -void FlatlandViz::onToolbarRemoveTool(QAction* remove_tool_menu_action) { +void FlatlandViz::onToolbarRemoveTool(QAction * remove_tool_menu_action) +{ RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "onToolbarRemoveTool called"); QString name = remove_tool_menu_action->text(); for (int i = 0; i < manager_->getToolManager()->numTools(); i++) { - rviz_common::Tool* tool = manager_->getToolManager()->getTool(i); + rviz_common::Tool * tool = manager_->getToolManager()->getTool(i); if (tool->getName() == name) { RCLCPP_ERROR_(rclcpp::get_logger("flatland_viz"), _STREAM("Removing --------> " << name.toStdString()); manager_->getToolManager()->removeTool(i); @@ -351,13 +356,15 @@ void FlatlandViz::onToolbarRemoveTool(QAction* remove_tool_menu_action) { } } -void FlatlandViz::refreshTool(rviz_common::Tool* tool) { - QAction* action = tool_to_action_map_[tool]; +void FlatlandViz::refreshTool(rviz_common::Tool * tool) +{ + QAction * action = tool_to_action_map_[tool]; action->setIcon(tool->getIcon()); action->setIconText(tool->getName()); } -void FlatlandViz::setFullScreen(bool full_screen) { +void FlatlandViz::setFullScreen(bool full_screen) +{ // Q_EMIT(fullScreenChange(full_screen)); if (full_screen) toolbar_visible_ = toolbar_->isVisible(); @@ -372,11 +379,12 @@ void FlatlandViz::setFullScreen(bool full_screen) { show(); } -void FlatlandViz::RecieveDebugTopics(const flatland_msgs::msg::DebugTopicList::SharedPtr msg) { +void FlatlandViz::RecieveDebugTopics(const flatland_msgs::msg::DebugTopicList::SharedPtr msg) +{ std::vector topics = msg->topics; // check for deleted topics - for (auto& topic : debug_displays_) { + for (auto & topic : debug_displays_) { if (std::count(topics.begin(), topics.end(), topic.first) == 0) { delete debug_displays_[topic.first]; debug_displays_.erase(topic.first); @@ -384,17 +392,17 @@ void FlatlandViz::RecieveDebugTopics(const flatland_msgs::msg::DebugTopicList::S } // check for new topics - for (const auto& topic : topics) { + for (const auto & topic : topics) { if (debug_displays_.count(topic) == 0) { // Create the marker display and set its topic - debug_displays_[topic] = manager_->createDisplay( - "rviz/MarkerArray", QString::fromLocal8Bit(topic.c_str()), true); + debug_displays_[topic] = + manager_->createDisplay("rviz/MarkerArray", QString::fromLocal8Bit(topic.c_str()), true); if (debug_displays_[topic] == nullptr) { RCLCPP_WARN(rclcpp::get_logger("flatland_viz"), "MarkerArray failed to instantiate"); exit(1); } - QString topic_qt = QString::fromLocal8Bit( - (std::string("/flatland_server/debug/") + topic).c_str()); + QString topic_qt = + QString::fromLocal8Bit((std::string("/flatland_server/debug/") + topic).c_str()); debug_displays_[topic]->subProp("Marker Topic")->setValue(topic_qt); } } diff --git a/flatland_viz/src/flatland_viz_node.cpp b/flatland_viz/src/flatland_viz_node.cpp index 502e95e5..eb39c9e1 100644 --- a/flatland_viz/src/flatland_viz_node.cpp +++ b/flatland_viz/src/flatland_viz_node.cpp @@ -44,19 +44,22 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include + #include +#include + #include "flatland_viz/flatland_window.h" -FlatlandWindow* window = nullptr; +FlatlandWindow * window = nullptr; /** * @name SigintHandler * @brief Interrupt handler - sends shutdown signal to simulation_manager * @param[in] sig: signal itself */ -void SigintHandler(int sig) { +void SigintHandler(int sig) +{ RCLCPP_WARN(rclcpp::get_logger("Node"), "*** Shutting down... ***"); if (window != nullptr) { @@ -67,7 +70,8 @@ void SigintHandler(int sig) { rclcpp::shutdown(); } -int main(int argc, char** argv) { +int main(int argc, char ** argv) +{ if (!rclcpp::isInitialized()) { rclcpp::init(argc, argv); } diff --git a/flatland_viz/src/flatland_window.cpp b/flatland_viz/src/flatland_window.cpp index 580e7574..ef5c3202 100644 --- a/flatland_viz/src/flatland_window.cpp +++ b/flatland_viz/src/flatland_window.cpp @@ -47,16 +47,16 @@ #include "flatland_viz/flatland_window.h" -void FlatlandWindow::openNewToolDialog() { +void FlatlandWindow::openNewToolDialog() +{ QString class_id; QStringList empty; } -rviz_common::VisualizationManager *FlatlandWindow::getManager() { - return visualization_manager_; -} +rviz_common::VisualizationManager * FlatlandWindow::getManager() { return visualization_manager_; } -FlatlandWindow::FlatlandWindow(QWidget *parent) : QMainWindow(parent) { +FlatlandWindow::FlatlandWindow(QWidget * parent) : QMainWindow(parent) +{ // Create the main viewport viz_ = new FlatlandViz(this); setCentralWidget(viz_); diff --git a/flatland_viz/src/load_model_dialog.cpp b/flatland_viz/src/load_model_dialog.cpp index 6eb96408..dd3df893 100644 --- a/flatland_viz/src/load_model_dialog.cpp +++ b/flatland_viz/src/load_model_dialog.cpp @@ -53,14 +53,6 @@ #include //#include -#include - -#include -#include - -#include -#include -#include #include #include @@ -73,8 +65,13 @@ #include #include #include - #include +#include +#include +#include +#include +#include +#include #include "flatland_viz/spawn_model_tool.h" @@ -82,26 +79,26 @@ QString LoadModelDialog::path_to_model_file; int LoadModelDialog::count; bool LoadModelDialog::numbering; -LoadModelDialog::LoadModelDialog(QWidget *parent, - flatland_viz::SpawnModelTool *tool) - : QDialog(parent), tool_(tool) { +LoadModelDialog::LoadModelDialog(QWidget * parent, flatland_viz::SpawnModelTool * tool) +: QDialog(parent), tool_(tool) +{ RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "ModelDialog::ModelDialog"); - QVBoxLayout *v_layout = new QVBoxLayout; + QVBoxLayout * v_layout = new QVBoxLayout; setLayout(v_layout); // we are injecting horizontal layouts into the master vertical layout - QHBoxLayout *h0_layout = new QHBoxLayout; - QHBoxLayout *h1_layout = new QHBoxLayout; - QHBoxLayout *h2_layout = new QHBoxLayout; - QHBoxLayout *h3_layout = new QHBoxLayout; + QHBoxLayout * h0_layout = new QHBoxLayout; + QHBoxLayout * h1_layout = new QHBoxLayout; + QHBoxLayout * h2_layout = new QHBoxLayout; + QHBoxLayout * h3_layout = new QHBoxLayout; // create widgets - QPushButton *pathButton = new QPushButton("choose file"); + QPushButton * pathButton = new QPushButton("choose file"); p_label = new QLabel; n_checkbox = new QCheckBox; n_edit = new QLineEdit; - QPushButton *okButton = new QPushButton("ok"); - QPushButton *cancelButton = new QPushButton("cancel"); + QPushButton * okButton = new QPushButton("ok"); + QPushButton * cancelButton = new QPushButton("cancel"); // set focus policy, otherwise cr in textfield triggers all the slots pathButton->setFocusPolicy(Qt::NoFocus); @@ -111,14 +108,10 @@ LoadModelDialog::LoadModelDialog(QWidget *parent, okButton->setFocusPolicy(Qt::NoFocus); cancelButton->setFocusPolicy(Qt::NoFocus); - connect(pathButton, &QAbstractButton::clicked, this, - &LoadModelDialog::on_PathButtonClicked); - connect(okButton, &QAbstractButton::clicked, this, - &LoadModelDialog::OkButtonClicked); - connect(cancelButton, &QAbstractButton::clicked, this, - &LoadModelDialog::CancelButtonClicked); - connect(n_checkbox, &QAbstractButton::clicked, this, - &LoadModelDialog::NumberCheckBoxChanged); + connect(pathButton, &QAbstractButton::clicked, this, &LoadModelDialog::on_PathButtonClicked); + connect(okButton, &QAbstractButton::clicked, this, &LoadModelDialog::OkButtonClicked); + connect(cancelButton, &QAbstractButton::clicked, this, &LoadModelDialog::CancelButtonClicked); + connect(n_checkbox, &QAbstractButton::clicked, this, &LoadModelDialog::NumberCheckBoxChanged); // path button h0_layout->addWidget(pathButton); @@ -157,13 +150,15 @@ LoadModelDialog::LoadModelDialog(QWidget *parent, this->setAttribute(Qt::WA_DeleteOnClose, true); } -void LoadModelDialog::CancelButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::CancelButtonClicked"); +void LoadModelDialog::CancelButtonClicked() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::CancelButtonClicked"); this->close(); } -void LoadModelDialog::OkButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::OkButtonClicked"); +void LoadModelDialog::OkButtonClicked() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::OkButtonClicked"); QString name = n_edit->displayText(); @@ -174,12 +169,11 @@ void LoadModelDialog::OkButtonClicked() { this->close(); } -void LoadModelDialog::AddNumberAndUpdateName() { - std::string bsfn = - boost::filesystem::basename(path_to_model_file.toStdString()); +void LoadModelDialog::AddNumberAndUpdateName() +{ + std::string bsfn = boost::filesystem::basename(path_to_model_file.toStdString()); QString name = QString::fromStdString(bsfn); - if (numbering) { name = name.append(QString::number(count++)); } @@ -188,8 +182,9 @@ void LoadModelDialog::AddNumberAndUpdateName() { n_edit->setText(name); } -void LoadModelDialog::on_PathButtonClicked() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::on_PathButtonClicked"); +void LoadModelDialog::on_PathButtonClicked() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "LoadModelDialog::on_PathButtonClicked"); path_to_model_file = ChooseFile(); AddNumberAndUpdateName(); @@ -197,23 +192,23 @@ void LoadModelDialog::on_PathButtonClicked() { n_edit->setFocus(); } -void LoadModelDialog::NumberCheckBoxChanged(bool i) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "NumberCheckBoxChanged"); +void LoadModelDialog::NumberCheckBoxChanged(bool i) +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("ModelDialog"), "NumberCheckBoxChanged"); numbering = !numbering; AddNumberAndUpdateName(); } -QString LoadModelDialog::ChooseFile() { - QString fileName = - QFileDialog::getOpenFileName(NULL, tr("Open model file"), "", ""); +QString LoadModelDialog::ChooseFile() +{ + QString fileName = QFileDialog::getOpenFileName(NULL, tr("Open model file"), "", ""); if (fileName.isEmpty()) return fileName; else { QFile file(fileName); if (!file.open(QIODevice::ReadOnly)) { - QMessageBox::information(NULL, tr("Unable to open file"), - file.errorString()); + QMessageBox::information(NULL, tr("Unable to open file"), file.errorString()); return fileName; } file.close(); diff --git a/flatland_viz/src/model_dialog.cpp b/flatland_viz/src/model_dialog.cpp index d77ea4bd..2379295f 100644 --- a/flatland_viz/src/model_dialog.cpp +++ b/flatland_viz/src/model_dialog.cpp @@ -45,6 +45,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "flatland_viz/model_dialog.h" + #include #include #include @@ -53,22 +55,19 @@ #include #include -#include "flatland_viz/model_dialog.h" - // Initialize static variables QColor ModelDialog::saved_color_; -QString ModelDialog::SelectFile() { - QString fileName = - QFileDialog::getOpenFileName(this, tr("Open model file"), "", ""); +QString ModelDialog::SelectFile() +{ + QString fileName = QFileDialog::getOpenFileName(this, tr("Open model file"), "", ""); if (fileName.isEmpty()) return fileName; else { QFile file(fileName); if (!file.open(QIODevice::ReadOnly)) { - QMessageBox::information(this, tr("Unable to open file"), - file.errorString()); + QMessageBox::information(this, tr("Unable to open file"), file.errorString()); return fileName; } file.close(); @@ -77,30 +76,30 @@ QString ModelDialog::SelectFile() { } } -ModelDialog::ModelDialog(QWidget *parent) : QDialog(parent) { +ModelDialog::ModelDialog(QWidget * parent) : QDialog(parent) +{ RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "ModelDialog::ModelDialog"); path_to_model_file = SelectFile(); - QVBoxLayout *v_layout = new QVBoxLayout; + QVBoxLayout * v_layout = new QVBoxLayout; // we are injecting horizontal layouts into the master vertical layout - QHBoxLayout *h1_layout = new QHBoxLayout; - QHBoxLayout *h2_layout = new QHBoxLayout; - QHBoxLayout *h3_layout = new QHBoxLayout; - QHBoxLayout *h4_layout = new QHBoxLayout; - QHBoxLayout *h5_layout = new QHBoxLayout; + QHBoxLayout * h1_layout = new QHBoxLayout; + QHBoxLayout * h2_layout = new QHBoxLayout; + QHBoxLayout * h3_layout = new QHBoxLayout; + QHBoxLayout * h4_layout = new QHBoxLayout; + QHBoxLayout * h5_layout = new QHBoxLayout; - QLabel *colorLabel = new QLabel; + QLabel * colorLabel = new QLabel; int frameStyle = QFrame::Sunken | QFrame::Panel; colorLabel->setFrameStyle(frameStyle); color_button = new QPushButton(""); - QPushButton *okButton = new QPushButton("ok"); - QPushButton *cancelButton = new QPushButton("cancel"); - connect(color_button, &QAbstractButton::clicked, this, - &ModelDialog::SetColor); + QPushButton * okButton = new QPushButton("ok"); + QPushButton * cancelButton = new QPushButton("cancel"); + connect(color_button, &QAbstractButton::clicked, this, &ModelDialog::SetColor); // path label and path - QLabel *path = new QLabel; + QLabel * path = new QLabel; path->setText(path_to_model_file); h1_layout->addWidget(new QLabel("path:")); h1_layout->addWidget(path); @@ -111,8 +110,7 @@ ModelDialog::ModelDialog(QWidget *parent) : QDialog(parent) { h2_layout->addWidget(n_edit); // set the default name to the filename parsed using boost - std::string bsfn = - boost::filesystem::basename(path_to_model_file.toStdString()); + std::string bsfn = boost::filesystem::basename(path_to_model_file.toStdString()); QString fn = QString::fromStdString(bsfn); n_edit->setText(fn); @@ -150,15 +148,13 @@ ModelDialog::ModelDialog(QWidget *parent) : QDialog(parent) { // ok button h5_layout->addWidget(okButton); - connect(okButton, &QAbstractButton::clicked, this, - &ModelDialog::OkButtonClicked); + connect(okButton, &QAbstractButton::clicked, this, &ModelDialog::OkButtonClicked); ModelDialog::SetButtonColor(&color, color_button); // cancel button h5_layout->addWidget(cancelButton); - connect(cancelButton, &QAbstractButton::clicked, this, - &ModelDialog::CancelButtonClicked); + connect(cancelButton, &QAbstractButton::clicked, this, &ModelDialog::CancelButtonClicked); // add the horizontal layouts to the vertical layout v_layout->addLayout(h1_layout); @@ -174,24 +170,25 @@ ModelDialog::ModelDialog(QWidget *parent) : QDialog(parent) { this->setAttribute(Qt::WA_DeleteOnClose, true); } -void ModelDialog::CancelButtonClicked() { +void ModelDialog::CancelButtonClicked() +{ RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "Cancel clicked"); this->close(); } -void ModelDialog::OkButtonClicked() { +void ModelDialog::OkButtonClicked() +{ RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "Ok clicked"); RCLCPP_ERROR(rclcpp::get_logger("flatland_viz"), "connect to ROS model service"); ModelDialog::SpawnModelClient(); } -void ModelDialog::SetColor() { - const QColorDialog::ColorDialogOptions options = - (QColorDialog::ShowAlphaChannel); +void ModelDialog::SetColor() +{ + const QColorDialog::ColorDialogOptions options = (QColorDialog::ShowAlphaChannel); - const QColor color = - QColorDialog::getColor(Qt::green, this, "Select Color", options); + const QColor color = QColorDialog::getColor(Qt::green, this, "Select Color", options); if (color.isValid()) { color_button->setText(color.name()); } @@ -200,7 +197,8 @@ void ModelDialog::SetColor() { saved_color_ = color; } -void ModelDialog::SetButtonColor(const QColor *c, QPushButton *b) { +void ModelDialog::SetButtonColor(const QColor * c, QPushButton * b) +{ b->setText(c->name()); b->setPalette(QPalette(*c)); b->setAutoFillBackground(true); @@ -210,7 +208,8 @@ void ModelDialog::SetButtonColor(const QColor *c, QPushButton *b) { color_button->setStyleSheet(qs); } -void ModelDialog::SpawnModelClient() { +void ModelDialog::SpawnModelClient() +{ srv.request.name = "service_manager_test_robot"; srv.request.ns = n_edit->text().toStdString(); srv.request.yaml_path = path_to_model_file.toStdString(); diff --git a/flatland_viz/src/pause_sim_tool.cpp b/flatland_viz/src/pause_sim_tool.cpp index 972fb19a..6828b407 100644 --- a/flatland_viz/src/pause_sim_tool.cpp +++ b/flatland_viz/src/pause_sim_tool.cpp @@ -41,7 +41,8 @@ #include -namespace flatland_viz { +namespace flatland_viz +{ PauseSimTool::PauseSimTool() {} @@ -49,17 +50,19 @@ PauseSimTool::PauseSimTool() {} PauseSimTool::~PauseSimTool() { rclcpp::shutdown(); /*pause_service_->shutdown();*/ } // When the tool is initially loaded, connect to the pause toggle service -void PauseSimTool::onInitialize() { - std::shared_ptr node = rclcpp::Node::make_shared("pause_sim_tool"); // TODO - pause_service_ = node->create_client("toggle_pause"); - setName("Pause/Resume"); +void PauseSimTool::onInitialize() +{ + std::shared_ptr node = rclcpp::Node::make_shared("pause_sim_tool"); // TODO + pause_service_ = node->create_client("toggle_pause"); + setName("Pause/Resume"); } // Every time the user presses the tool's Rviz toolbar button, call the pause // toggle service -void PauseSimTool::activate() { - auto request = std::make_shared(); - auto result = pause_service_->async_send_request(request); +void PauseSimTool::activate() +{ + auto request = std::make_shared(); + auto result = pause_service_->async_send_request(request); } void PauseSimTool::deactivate() {} diff --git a/flatland_viz/src/spawn_model_tool.cpp b/flatland_viz/src/spawn_model_tool.cpp index 3ec7961d..2656529a 100644 --- a/flatland_viz/src/spawn_model_tool.cpp +++ b/flatland_viz/src/spawn_model_tool.cpp @@ -45,6 +45,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "flatland_viz/spawn_model_tool.h" + +#include + #include #include #include @@ -54,39 +58,31 @@ #include #include #include - -#include "flatland_viz/spawn_model_tool.h" - - -#include //#include #include #include - -#include - #include - #include +#include class DialogOptionsWidget; -namespace flatland_viz { +namespace flatland_viz +{ QString SpawnModelTool::path_to_model_file_; QString SpawnModelTool::model_name; // Set the "shortcut_key_" member variable defined in the // superclass to declare which key will activate the tool. -SpawnModelTool::SpawnModelTool() : moving_model_node_(nullptr) { - shortcut_key_ = 'm'; -} +SpawnModelTool::SpawnModelTool() : moving_model_node_(nullptr) { shortcut_key_ = 'm'; } // The destructor destroys the Ogre scene nodes for the models so they // disappear from the 3D scene. The destructor for a Tool subclass is // only called when the tool is removed from the toolbar with the "-" // button. -SpawnModelTool::~SpawnModelTool() { +SpawnModelTool::~SpawnModelTool() +{ scene_manager_->destroySceneNode(arrow_->getSceneNode()); scene_manager_->destroySceneNode(moving_model_node_); } @@ -102,23 +98,22 @@ SpawnModelTool::~SpawnModelTool() { // In this case we load a mesh object with the shape and appearance of // the model, create an Ogre::SceneNode for the moving model, and then // set it invisible. -void SpawnModelTool::onInitialize() { +void SpawnModelTool::onInitialize() +{ // hide 3d model until the user clicks the span model tool button model_state = m_hidden; // make an arrow to show axis of rotation arrow_ = new rviz_rendering::Arrow(scene_manager_, NULL, 2.0f, 0.2f, 0.3f, 0.35f); - arrow_->setColor(0.0f, 0.0f, 1.0f, 1.0f); // blue - arrow_->getSceneNode()->setVisible( - false); // will only be visible during rotation phase + arrow_->setColor(0.0f, 0.0f, 1.0f, 1.0f); // blue + arrow_->getSceneNode()->setVisible(false); // will only be visible during rotation phase // set arrow to point up (along z) Ogre::Quaternion orientation(Ogre::Radian(M_PI), Ogre::Vector3(1, 0, 0)); arrow_->setOrientation(orientation); // create an Ogre child scene node for rendering the preview outline - moving_model_node_ = - scene_manager_->getRootSceneNode()->createChildSceneNode(); + moving_model_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); moving_model_node_->setVisible(false); SetMovingModelColor(Qt::green); @@ -138,16 +133,18 @@ void SpawnModelTool::onInitialize() { // if it were writable the model should really change position when the // user edits the property. This is a fine idea, and is possible, but // is left as an exercise for the reader. -void SpawnModelTool::activate() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::activate "); +void SpawnModelTool::activate() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::activate "); - LoadModelDialog *model_dialog = new LoadModelDialog(nullptr, this); + LoadModelDialog * model_dialog = new LoadModelDialog(nullptr, this); model_dialog->setModal(true); model_dialog->show(); } -void SpawnModelTool::BeginPlacement() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::BeginPlacement"); +void SpawnModelTool::BeginPlacement() +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::BeginPlacement"); model_state = m_dragging; if (moving_model_node_) { @@ -163,15 +160,18 @@ void SpawnModelTool::BeginPlacement() { // property, so that doesn't need to be done in a separate step. If // we didn't delete it here, it would stay in the list of models when // we switch to another tool. -void SpawnModelTool::deactivate() { +void SpawnModelTool::deactivate() +{ if (moving_model_node_) { moving_model_node_->setVisible(false); } } -void SpawnModelTool::SpawnModelInFlatland() { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), - "SpawnModelTool::SpawnModelInFlatland name:" << model_name.toStdString()); +void SpawnModelTool::SpawnModelInFlatland() +{ + RCLCPP_INFO_STREAM( + rclcpp::get_logger("SpawnModelTool"), + "SpawnModelTool::SpawnModelInFlatland name:" << model_name.toStdString()); auto srv = std::make_shared(); // model names can not have embeded period char @@ -196,7 +196,7 @@ void SpawnModelTool::SpawnModelInFlatland() { msgBox.setText("Error: You must have a client running."); msgBox.exec(); } else { - if (!srv->success) { + if (!srv->success) { QMessageBox msgBox; msgBox.setText(srv->message.c_str()); msgBox.exec(); @@ -204,18 +204,18 @@ void SpawnModelTool::SpawnModelInFlatland() { } } -void SpawnModelTool::SetMovingModelColor(QColor c) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::SetMovingModelColor"); +void SpawnModelTool::SetMovingModelColor(QColor c) +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("SpawnModelTool"), "SpawnModelTool::SetMovingModelColor"); try { - Ogre::Entity *m_pEntity = - static_cast(moving_model_node_->getAttachedObject(0)); + Ogre::Entity * m_pEntity = + static_cast(moving_model_node_->getAttachedObject(0)); const Ogre::MaterialPtr m_pMat = m_pEntity->getSubEntity(0)->getMaterial(); m_pMat->getTechnique(0)->getPass(0)->setAmbient(1, 0, 0); - m_pMat->getTechnique(0)->getPass(0)->setDiffuse(c.redF(), c.greenF(), - c.blueF(), 0); + m_pMat->getTechnique(0)->getPass(0)->setDiffuse(c.redF(), c.greenF(), c.blueF(), 0); } catch (Ogre::InvalidParametersException e) { - //RCLCPP_WARN("Invalid preview model"); + // RCLCPP_WARN("Invalid preview model"); } } @@ -231,7 +231,8 @@ void SpawnModelTool::SetMovingModelColor(QColor c) { // place and drop the pointer to the VectorProperty. Dropping the // pointer means when the tool is deactivated the VectorProperty won't // be deleted, which is what we want. -int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { +int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ if (!moving_model_node_) { return Render; } @@ -240,8 +241,8 @@ int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f); if (model_state == m_dragging) { - if (rviz_rendering::getPointOnPlaneFromWindowXY (event.viewport, ground_plane, event.x, - event.y, intersection)) { + if (rviz_rendering::getPointOnPlaneFromWindowXY( + event.viewport, ground_plane, event.x, event.y, intersection)) { moving_model_node_->setVisible(true); moving_model_node_->setPosition(intersection); @@ -253,15 +254,14 @@ int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { return Render; } } else { - moving_model_node_->setVisible( - false); // If the mouse is not pointing at the - // ground plane, don't show the model. + moving_model_node_->setVisible(false); // If the mouse is not pointing at the + // ground plane, don't show the model. } } if (model_state == m_rotating) { // model_state is m_rotating - if (rviz_rendering::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, - event.y, intersection2)) { + if (rviz_rendering::getPointOnPlaneFromWindowXY( + event.viewport, ground_plane, event.x, event.y, intersection2)) { if (event.leftDown()) { model_state = m_hidden; arrow_->getSceneNode()->setVisible(false); @@ -278,15 +278,15 @@ int SpawnModelTool::processMouseEvent(rviz_common::ViewportMouseEvent &event) { moving_model_node_->setPosition(intersection); Ogre::Vector3 dir = intersection2 - intersection; initial_angle = atan2(dir.y, dir.x); - Ogre::Quaternion orientation(Ogre::Radian(initial_angle), - Ogre::Vector3(0, 0, 1)); + Ogre::Quaternion orientation(Ogre::Radian(initial_angle), Ogre::Vector3(0, 0, 1)); moving_model_node_->setOrientation(orientation); } } return Render; } -void SpawnModelTool::LoadPreview() { +void SpawnModelTool::LoadPreview() +{ // Clear the old preview, if there is one moving_model_node_->removeAllChildren(); lines_list_.clear(); @@ -298,11 +298,11 @@ void SpawnModelTool::LoadPreview() { try { flatland_server::YamlReader bodies_reader = - reader.Subnode("bodies", flatland_server::YamlReader::LIST); + reader.Subnode("bodies", flatland_server::YamlReader::LIST); // Iterate each body and add to the preview for (int i = 0; i < bodies_reader.NodeSize(); i++) { flatland_server::YamlReader body_reader = - bodies_reader.Subnode(i, flatland_server::YamlReader::MAP); + bodies_reader.Subnode(i, flatland_server::YamlReader::MAP); if (!body_reader.Get("enabled", "true")) { // skip if disabled continue; } @@ -310,18 +310,17 @@ void SpawnModelTool::LoadPreview() { auto pose = body_reader.GetPose("pose", flatland_server::Pose()); flatland_server::YamlReader footprints_node = - body_reader.Subnode("footprints", flatland_server::YamlReader::LIST); + body_reader.Subnode("footprints", flatland_server::YamlReader::LIST); for (int j = 0; j < footprints_node.NodeSize(); j++) { flatland_server::YamlReader footprint = - footprints_node.Subnode(j, flatland_server::YamlReader::MAP); + footprints_node.Subnode(j, flatland_server::YamlReader::MAP); lines_list_.push_back(std::make_shared( - context_->getSceneManager(), moving_model_node_)); + context_->getSceneManager(), moving_model_node_)); auto lines = lines_list_.back(); lines->setColor(0.0, 1.0, 0.0, 0.75); // Green lines->setLineWidth(0.05); - lines->setOrientation( - Ogre::Quaternion(Ogre::Radian(pose.theta), Ogre::Vector3(0, 0, 1))); + lines->setOrientation(Ogre::Quaternion(Ogre::Radian(pose.theta), Ogre::Vector3(0, 0, 1))); lines->setPosition(Ogre::Vector3(pose.x, pose.y, 0)); std::string type = footprint.Get("type"); @@ -334,16 +333,17 @@ void SpawnModelTool::LoadPreview() { } } } - } catch (const flatland_server::YAMLException &e) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("SpawnModelTool"), "Couldn't load model bodies for preview" << e.what()); + } catch (const flatland_server::YAMLException & e) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("SpawnModelTool"), "Couldn't load model bodies for preview" << e.what()); } } void SpawnModelTool::LoadPolygonFootprint( - flatland_server::YamlReader &footprint, const flatland_server::Pose pose) { + flatland_server::YamlReader & footprint, const flatland_server::Pose pose) +{ auto lines = lines_list_.back(); - auto points = footprint.GetList("points", 3, - b2_maxPolygonVertices); + auto points = footprint.GetList("points", 3, b2_maxPolygonVertices); for (auto p : points) { lines->addPoint(Ogre::Vector3(p.x, p.y, 0.)); } @@ -353,21 +353,21 @@ void SpawnModelTool::LoadPolygonFootprint( } } -void SpawnModelTool::LoadCircleFootprint(flatland_server::YamlReader &footprint, - const flatland_server::Pose pose) { +void SpawnModelTool::LoadCircleFootprint( + flatland_server::YamlReader & footprint, const flatland_server::Pose pose) +{ auto lines = lines_list_.back(); auto center = footprint.GetVec2("center", flatland_server::Vec2()); auto radius = footprint.Get("radius", 1.0); for (float a = 0.; a < M_PI * 2.0; a += M_PI / 8.) { // 16 point circle - lines->addPoint(Ogre::Vector3(center.x + radius * cos(a), - center.y + radius * sin(a), 0.)); + lines->addPoint(Ogre::Vector3(center.x + radius * cos(a), center.y + radius * sin(a), 0.)); } - lines->addPoint( - Ogre::Vector3(center.x + radius, center.y, 0.)); // close the loop + lines->addPoint(Ogre::Vector3(center.x + radius, center.y, 0.)); // close the loop } -void SpawnModelTool::SavePath(QString p) { +void SpawnModelTool::SavePath(QString p) +{ path_to_model_file_ = p; LoadPreview(); } From 31cf5231220adaf2322a0d80407351a006d18cec Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Tue, 7 Feb 2023 17:26:03 +0000 Subject: [PATCH 63/66] added AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS exception --- .github/workflows/industrial_ci_action.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci_action.yml b/.github/workflows/industrial_ci_action.yml index f42e77d5..22c8ec9b 100644 --- a/.github/workflows/industrial_ci_action.yml +++ b/.github/workflows/industrial_ci_action.yml @@ -7,7 +7,7 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: humble, ROS_REPO: main} + - {ROS_DISTRO: humble, ROS_REPO: main, AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS: true} runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v1 From b36ac17eea6e5a3d218d4cab8d9b81fd90aa1373 Mon Sep 17 00:00:00 2001 From: Joseph Duchesne Date: Tue, 7 Feb 2023 15:30:23 -0500 Subject: [PATCH 64/66] Cleaned up header file paths and includes, and removed agressive ament linting --- flatland_rviz_plugins/CMakeLists.txt | 5 ++++- .../flatland_rviz_plugins}/change_rate_dialog.hpp | 6 +++--- .../flatland_rviz_plugins}/load_model_dialog.hpp | 6 +++--- flatland_rviz_plugins/package.xml | 3 --- flatland_rviz_plugins/src/change_rate_dialog.cpp | 2 +- flatland_rviz_plugins/src/change_rate_tool.cpp | 2 +- flatland_rviz_plugins/src/interactive_tool.cpp | 2 +- flatland_rviz_plugins/src/load_model_dialog.cpp | 3 ++- flatland_rviz_plugins/src/spawn_model_tool.cpp | 2 +- 9 files changed, 16 insertions(+), 15 deletions(-) rename flatland_rviz_plugins/{src => include/flatland_rviz_plugins}/change_rate_dialog.hpp (94%) rename flatland_rviz_plugins/{src => include/flatland_rviz_plugins}/load_model_dialog.hpp (96%) diff --git a/flatland_rviz_plugins/CMakeLists.txt b/flatland_rviz_plugins/CMakeLists.txt index 883e9dc5..311549e7 100644 --- a/flatland_rviz_plugins/CMakeLists.txt +++ b/flatland_rviz_plugins/CMakeLists.txt @@ -41,7 +41,10 @@ set(flatland_rviz_plugins_headers_to_moc include/flatland_rviz_plugins/change_rate_tool.hpp include/flatland_rviz_plugins/pause_tool.hpp include/flatland_rviz_plugins/interactive_tool.hpp - include/flatland_rviz_plugins/spawn_model_tool.hpp) + include/flatland_rviz_plugins/spawn_model_tool.hpp + include/flatland_rviz_plugins/change_rate_dialog.hpp + include/flatland_rviz_plugins/load_model_dialog.hpp + ) include_directories( include diff --git a/flatland_rviz_plugins/src/change_rate_dialog.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_dialog.hpp similarity index 94% rename from flatland_rviz_plugins/src/change_rate_dialog.hpp rename to flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_dialog.hpp index c444d628..377747fe 100644 --- a/flatland_rviz_plugins/src/change_rate_dialog.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/change_rate_dialog.hpp @@ -39,8 +39,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef CHANGE_RATE_DIALOG_HPP_ -#define CHANGE_RATE_DIALOG_HPP_ +#ifndef FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ +#define FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ #include #include @@ -87,4 +87,4 @@ public Q_SLOTS: } // namespace flatland_rviz_plugins -#endif // CHANGE_RATE_DIALOG_HPP_ +#endif // FLATLAND_RVIZ_PLUGINS__CHANGE_RATE_DIALOG_HPP_ diff --git a/flatland_rviz_plugins/src/load_model_dialog.hpp b/flatland_rviz_plugins/include/flatland_rviz_plugins/load_model_dialog.hpp similarity index 96% rename from flatland_rviz_plugins/src/load_model_dialog.hpp rename to flatland_rviz_plugins/include/flatland_rviz_plugins/load_model_dialog.hpp index 2d712e96..5fc99318 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.hpp +++ b/flatland_rviz_plugins/include/flatland_rviz_plugins/load_model_dialog.hpp @@ -45,8 +45,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef LOAD_MODEL_DIALOG_HPP_ -#define LOAD_MODEL_DIALOG_HPP_ +#ifndef FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ +#define FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ #include #include @@ -134,4 +134,4 @@ public Q_SLOTS: } // namespace flatland_rviz_plugins -#endif // LOAD_MODEL_DIALOG_HPP_ +#endif // FLATLAND_RVIZ_PLUGINS__LOAD_MODEL_DIALOG_HPP_ diff --git a/flatland_rviz_plugins/package.xml b/flatland_rviz_plugins/package.xml index fb8d4807..dd7a4c79 100644 --- a/flatland_rviz_plugins/package.xml +++ b/flatland_rviz_plugins/package.xml @@ -30,9 +30,6 @@ libqt5-opengl libqt5-widgets - ament_lint_common - ament_lint_auto - ament_cmake diff --git a/flatland_rviz_plugins/src/change_rate_dialog.cpp b/flatland_rviz_plugins/src/change_rate_dialog.cpp index f0faff24..120da104 100644 --- a/flatland_rviz_plugins/src/change_rate_dialog.cpp +++ b/flatland_rviz_plugins/src/change_rate_dialog.cpp @@ -39,7 +39,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "change_rate_dialog.hpp" +#include "flatland_rviz_plugins/change_rate_dialog.hpp" #include diff --git a/flatland_rviz_plugins/src/change_rate_tool.cpp b/flatland_rviz_plugins/src/change_rate_tool.cpp index 488b2ecd..61dd18e5 100644 --- a/flatland_rviz_plugins/src/change_rate_tool.cpp +++ b/flatland_rviz_plugins/src/change_rate_tool.cpp @@ -47,7 +47,7 @@ #include #include -#include "change_rate_dialog.hpp" +#include "flatland_rviz_plugins/change_rate_dialog.hpp" namespace flatland_rviz_plugins { diff --git a/flatland_rviz_plugins/src/interactive_tool.cpp b/flatland_rviz_plugins/src/interactive_tool.cpp index 5a04bd2a..6d95d0d7 100644 --- a/flatland_rviz_plugins/src/interactive_tool.cpp +++ b/flatland_rviz_plugins/src/interactive_tool.cpp @@ -39,7 +39,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "include/flatland_rviz_plugins/interactive_tool.hpp" +#include "flatland_rviz_plugins/interactive_tool.hpp" #include #include diff --git a/flatland_rviz_plugins/src/load_model_dialog.cpp b/flatland_rviz_plugins/src/load_model_dialog.cpp index 4234c274..377dc6d3 100644 --- a/flatland_rviz_plugins/src/load_model_dialog.cpp +++ b/flatland_rviz_plugins/src/load_model_dialog.cpp @@ -45,6 +45,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "flatland_rviz_plugins/load_model_dialog.hpp" + #include #include @@ -65,7 +67,6 @@ #include #include -#include "load_model_dialog.hpp" namespace flatland_rviz_plugins { diff --git a/flatland_rviz_plugins/src/spawn_model_tool.cpp b/flatland_rviz_plugins/src/spawn_model_tool.cpp index 57606160..dc002563 100644 --- a/flatland_rviz_plugins/src/spawn_model_tool.cpp +++ b/flatland_rviz_plugins/src/spawn_model_tool.cpp @@ -70,7 +70,7 @@ #include #include -#include "load_model_dialog.hpp" +#include "flatland_rviz_plugins/load_model_dialog.hpp" namespace flatland_rviz_plugins { From 6709f76715161d46ac37012f1e4c4b5b439bbcdb Mon Sep 17 00:00:00 2001 From: Henrique Ribeiro Date: Tue, 7 Feb 2023 21:40:48 +0000 Subject: [PATCH 65/66] Merged with original repo --- flatland_viz/build/.built_by | 1 + flatland_viz/build/COLCON_IGNORE | 0 .../build/flatland_viz/CMakeCache.txt | 353 +++++++ .../CMakeFiles/3.25.0/CMakeCCompiler.cmake | 72 ++ .../CMakeFiles/3.25.0/CMakeCXXCompiler.cmake | 83 ++ .../3.25.0/CMakeDetermineCompilerABI_C.bin | Bin 0 -> 51200 bytes .../3.25.0/CMakeDetermineCompilerABI_CXX.bin | Bin 0 -> 51200 bytes .../CMakeFiles/3.25.0/CMakeRCCompiler.cmake | 6 + .../CMakeFiles/3.25.0/CMakeSystem.cmake | 15 + .../3.25.0/CompilerIdC/CMakeCCompilerId.c | 868 ++++++++++++++++++ .../3.25.0/CompilerIdC/CMakeCCompilerId.exe | Bin 0 -> 96256 bytes .../3.25.0/CompilerIdC/CMakeCCompilerId.obj | Bin 0 -> 1995 bytes .../CompilerIdCXX/CMakeCXXCompilerId.cpp | 857 +++++++++++++++++ .../CompilerIdCXX/CMakeCXXCompilerId.exe | Bin 0 -> 96256 bytes .../CompilerIdCXX/CMakeCXXCompilerId.obj | Bin 0 -> 2062 bytes .../flatland_viz/CMakeFiles/CMakeOutput.log | 65 ++ .../CMakeFiles/ShowIncludes/foo.h | 1 + .../CMakeFiles/ShowIncludes/main.c | 2 + .../CMakeFiles/ShowIncludes/main.obj | Bin 0 -> 682 bytes .../flatland_viz/CMakeFiles/cmake.check_cache | 1 + .../build/flatland_viz/cmake_args.last | 1 + .../build/flatland_viz/colcon_build.rc | 1 + .../colcon_command_prefix_build.bat | 2 + .../colcon_command_prefix_build.bat.env | 50 + .../colcon_command_prefix_build.ps1 | 1 + .../colcon_command_prefix_build.ps1.env | 56 ++ flatland_viz/install/.colcon_install_layout | 1 + flatland_viz/install/COLCON_IGNORE | 0 flatland_viz/install/_local_setup_util_bat.py | 404 ++++++++ flatland_viz/install/_local_setup_util_ps1.py | 404 ++++++++ flatland_viz/install/flatland_viz/.catkin | 0 .../share/colcon-core/packages/flatland_viz | 1 + .../share/flatland_viz/package.bat | 16 + .../share/flatland_viz/package.dsv | 0 .../share/flatland_viz/package.ps1 | 108 +++ flatland_viz/install/local_setup.bat | 134 +++ flatland_viz/install/local_setup.ps1 | 55 ++ flatland_viz/install/setup.bat | 23 + flatland_viz/install/setup.ps1 | 26 + flatland_viz/log/COLCON_IGNORE | 0 .../log/build_2022-11-19_18-56-57/events.log | 30 + .../flatland_viz/command.log | 6 + .../flatland_viz/stderr.log | 11 + .../flatland_viz/stdout.log | 2 + .../flatland_viz/stdout_stderr.log | 13 + .../flatland_viz/streams.log | 19 + .../build_2022-11-19_18-56-57/logger_all.log | 70 ++ .../log/build_2022-11-19_19-03-07/events.log | 65 ++ .../flatland_viz/command.log | 6 + .../flatland_viz/stderr.log | 16 + .../flatland_viz/stdout.log | 14 + .../flatland_viz/stdout_stderr.log | 30 + .../flatland_viz/streams.log | 36 + .../build_2022-11-19_19-03-07/logger_all.log | 71 ++ .../log/build_2022-11-19_19-05-41/events.log | 27 + .../flatland_viz/command.log | 6 + .../flatland_viz/stderr.log | 16 + .../flatland_viz/stdout.log | 2 + .../flatland_viz/stdout_stderr.log | 18 + .../flatland_viz/streams.log | 24 + .../build_2022-11-19_19-05-41/logger_all.log | 71 ++ flatland_viz/log/latest | 1 + flatland_viz/log/latest_build | 1 + 63 files changed, 4162 insertions(+) create mode 100644 flatland_viz/build/.built_by create mode 100644 flatland_viz/build/COLCON_IGNORE create mode 100644 flatland_viz/build/flatland_viz/CMakeCache.txt create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeCCompiler.cmake create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeCXXCompiler.cmake create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeDetermineCompilerABI_C.bin create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeDetermineCompilerABI_CXX.bin create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeRCCompiler.cmake create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeSystem.cmake create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdC/CMakeCCompilerId.c create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdC/CMakeCCompilerId.exe create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdC/CMakeCCompilerId.obj create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.cpp create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.exe create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.obj create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/foo.h create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/main.c create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/main.obj create mode 100644 flatland_viz/build/flatland_viz/CMakeFiles/cmake.check_cache create mode 100644 flatland_viz/build/flatland_viz/cmake_args.last create mode 100644 flatland_viz/build/flatland_viz/colcon_build.rc create mode 100644 flatland_viz/build/flatland_viz/colcon_command_prefix_build.bat create mode 100644 flatland_viz/build/flatland_viz/colcon_command_prefix_build.bat.env create mode 100644 flatland_viz/build/flatland_viz/colcon_command_prefix_build.ps1 create mode 100644 flatland_viz/build/flatland_viz/colcon_command_prefix_build.ps1.env create mode 100644 flatland_viz/install/.colcon_install_layout create mode 100644 flatland_viz/install/COLCON_IGNORE create mode 100644 flatland_viz/install/_local_setup_util_bat.py create mode 100644 flatland_viz/install/_local_setup_util_ps1.py create mode 100644 flatland_viz/install/flatland_viz/.catkin create mode 100644 flatland_viz/install/flatland_viz/share/colcon-core/packages/flatland_viz create mode 100644 flatland_viz/install/flatland_viz/share/flatland_viz/package.bat create mode 100644 flatland_viz/install/flatland_viz/share/flatland_viz/package.dsv create mode 100644 flatland_viz/install/flatland_viz/share/flatland_viz/package.ps1 create mode 100644 flatland_viz/install/local_setup.bat create mode 100644 flatland_viz/install/local_setup.ps1 create mode 100644 flatland_viz/install/setup.bat create mode 100644 flatland_viz/install/setup.ps1 create mode 100644 flatland_viz/log/COLCON_IGNORE create mode 100644 flatland_viz/log/build_2022-11-19_18-56-57/events.log create mode 100644 flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/command.log create mode 100644 flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stderr.log create mode 100644 flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stdout.log create mode 100644 flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stdout_stderr.log create mode 100644 flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/streams.log create mode 100644 flatland_viz/log/build_2022-11-19_18-56-57/logger_all.log create mode 100644 flatland_viz/log/build_2022-11-19_19-03-07/events.log create mode 100644 flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/command.log create mode 100644 flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stderr.log create mode 100644 flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stdout.log create mode 100644 flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stdout_stderr.log create mode 100644 flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/streams.log create mode 100644 flatland_viz/log/build_2022-11-19_19-03-07/logger_all.log create mode 100644 flatland_viz/log/build_2022-11-19_19-05-41/events.log create mode 100644 flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/command.log create mode 100644 flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stderr.log create mode 100644 flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stdout.log create mode 100644 flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stdout_stderr.log create mode 100644 flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/streams.log create mode 100644 flatland_viz/log/build_2022-11-19_19-05-41/logger_all.log create mode 120000 flatland_viz/log/latest create mode 120000 flatland_viz/log/latest_build diff --git a/flatland_viz/build/.built_by b/flatland_viz/build/.built_by new file mode 100644 index 00000000..06e74acb --- /dev/null +++ b/flatland_viz/build/.built_by @@ -0,0 +1 @@ +colcon diff --git a/flatland_viz/build/COLCON_IGNORE b/flatland_viz/build/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/flatland_viz/build/flatland_viz/CMakeCache.txt b/flatland_viz/build/flatland_viz/CMakeCache.txt new file mode 100644 index 00000000..ed3e3319 --- /dev/null +++ b/flatland_viz/build/flatland_viz/CMakeCache.txt @@ -0,0 +1,353 @@ +# This is the CMakeCache file. +# For build in directory: c:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz +# It was generated by CMake: C:/Program Files/CMake/bin/cmake.exe +# You can edit this file to change values found and used by cmake. +# If you do not want to change any of the values, simply exit the editor. +# If you do want to change a value, simply edit, save, and exit the editor. +# The syntax for the file is as follows: +# KEY:TYPE=VALUE +# KEY is the name of a variable in the cache. +# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!. +# VALUE is the current value for the KEY. + +######################## +# EXTERNAL cache entries +######################## + +//No help, variable specified on the command line. +CATKIN_INSTALL_INTO_PREFIX_ROOT:UNINITIALIZED=0 + +//Path to a program. +CMAKE_AR:FILEPATH=C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/lib.exe + +//Choose the type of build, options are: None Debug Release RelWithDebInfo +// MinSizeRel ... +CMAKE_BUILD_TYPE:STRING=Debug + +//CXX compiler +CMAKE_CXX_COMPILER:FILEPATH=C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe + +//Flags used by the CXX compiler during all build types. +CMAKE_CXX_FLAGS:STRING=/DWIN32 /D_WINDOWS /W3 /GR /EHsc + +//Flags used by the CXX compiler during DEBUG builds. +CMAKE_CXX_FLAGS_DEBUG:STRING=/MDd /Zi /Ob0 /Od /RTC1 + +//Flags used by the CXX compiler during MINSIZEREL builds. +CMAKE_CXX_FLAGS_MINSIZEREL:STRING=/MD /O1 /Ob1 /DNDEBUG + +//Flags used by the CXX compiler during RELEASE builds. +CMAKE_CXX_FLAGS_RELEASE:STRING=/MD /O2 /Ob2 /DNDEBUG + +//Flags used by the CXX compiler during RELWITHDEBINFO builds. +CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=/MD /Zi /O2 /Ob1 /DNDEBUG + +//Libraries linked by default with all C++ applications. +CMAKE_CXX_STANDARD_LIBRARIES:STRING=kernel32.lib user32.lib gdi32.lib winspool.lib shell32.lib ole32.lib oleaut32.lib uuid.lib comdlg32.lib advapi32.lib + +//C compiler +CMAKE_C_COMPILER:FILEPATH=C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe + +//Flags used by the C compiler during all build types. +CMAKE_C_FLAGS:STRING=/DWIN32 /D_WINDOWS /W3 + +//Flags used by the C compiler during DEBUG builds. +CMAKE_C_FLAGS_DEBUG:STRING=/MDd /Zi /Ob0 /Od /RTC1 + +//Flags used by the C compiler during MINSIZEREL builds. +CMAKE_C_FLAGS_MINSIZEREL:STRING=/MD /O1 /Ob1 /DNDEBUG + +//Flags used by the C compiler during RELEASE builds. +CMAKE_C_FLAGS_RELEASE:STRING=/MD /O2 /Ob2 /DNDEBUG + +//Flags used by the C compiler during RELWITHDEBINFO builds. +CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=/MD /Zi /O2 /Ob1 /DNDEBUG + +//Libraries linked by default with all C applications. +CMAKE_C_STANDARD_LIBRARIES:STRING=kernel32.lib user32.lib gdi32.lib winspool.lib shell32.lib ole32.lib oleaut32.lib uuid.lib comdlg32.lib advapi32.lib + +//Flags used by the linker during all build types. +CMAKE_EXE_LINKER_FLAGS:STRING=/machine:x64 + +//Flags used by the linker during DEBUG builds. +CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING=/debug /INCREMENTAL + +//Flags used by the linker during MINSIZEREL builds. +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING=/INCREMENTAL:NO + +//Flags used by the linker during RELEASE builds. +CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING=/INCREMENTAL:NO + +//Flags used by the linker during RELWITHDEBINFO builds. +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING=/debug /INCREMENTAL + +//No help, variable specified on the command line. +CMAKE_EXPORT_COMPILE_COMMANDS:UNINITIALIZED=ON + +//Value Computed by CMake. +CMAKE_FIND_PACKAGE_REDIRECTS_DIR:STATIC=C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/pkgRedirects + +//Install path prefix, prepended onto install directories. +CMAKE_INSTALL_PREFIX:PATH=C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/install/flatland_viz + +//Path to a program. +CMAKE_LINKER:FILEPATH=C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/link.exe + +//make program +CMAKE_MAKE_PROGRAM:FILEPATH=C:/PROGRA~2/MIB055~1/2019/COMMUN~1/Common7/IDE/COMMON~1/MICROS~1/CMake/Ninja/ninja.exe + +//Flags used by the linker during the creation of modules during +// all build types. +CMAKE_MODULE_LINKER_FLAGS:STRING=/machine:x64 + +//Flags used by the linker during the creation of modules during +// DEBUG builds. +CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING=/debug /INCREMENTAL + +//Flags used by the linker during the creation of modules during +// MINSIZEREL builds. +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING=/INCREMENTAL:NO + +//Flags used by the linker during the creation of modules during +// RELEASE builds. +CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING=/INCREMENTAL:NO + +//Flags used by the linker during the creation of modules during +// RELWITHDEBINFO builds. +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING=/debug /INCREMENTAL + +//Path to a program. +CMAKE_MT:FILEPATH=C:/Program Files (x86)/Windows Kits/10/bin/10.0.19041.0/x86/mt.exe + +//Value Computed by CMake +CMAKE_PROJECT_DESCRIPTION:STATIC= + +//Value Computed by CMake +CMAKE_PROJECT_HOMEPAGE_URL:STATIC= + +//Value Computed by CMake +CMAKE_PROJECT_NAME:STATIC=flatland_viz + +//RC compiler +CMAKE_RC_COMPILER:FILEPATH=C:/Program Files (x86)/Windows Kits/10/bin/10.0.19041.0/x86/rc.exe + +//Flags for Windows Resource Compiler during all build types. +CMAKE_RC_FLAGS:STRING=-DWIN32 + +//Flags for Windows Resource Compiler during DEBUG builds. +CMAKE_RC_FLAGS_DEBUG:STRING=-D_DEBUG + +//Flags for Windows Resource Compiler during MINSIZEREL builds. +CMAKE_RC_FLAGS_MINSIZEREL:STRING= + +//Flags for Windows Resource Compiler during RELEASE builds. +CMAKE_RC_FLAGS_RELEASE:STRING= + +//Flags for Windows Resource Compiler during RELWITHDEBINFO builds. +CMAKE_RC_FLAGS_RELWITHDEBINFO:STRING= + +//Flags used by the linker during the creation of shared libraries +// during all build types. +CMAKE_SHARED_LINKER_FLAGS:STRING=/machine:x64 + +//Flags used by the linker during the creation of shared libraries +// during DEBUG builds. +CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING=/debug /INCREMENTAL + +//Flags used by the linker during the creation of shared libraries +// during MINSIZEREL builds. +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING=/INCREMENTAL:NO + +//Flags used by the linker during the creation of shared libraries +// during RELEASE builds. +CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING=/INCREMENTAL:NO + +//Flags used by the linker during the creation of shared libraries +// during RELWITHDEBINFO builds. +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING=/debug /INCREMENTAL + +//If set, runtime paths are not added when installing shared libraries, +// but are added when building. +CMAKE_SKIP_INSTALL_RPATH:BOOL=NO + +//If set, runtime paths are not added when using shared libraries. +CMAKE_SKIP_RPATH:BOOL=NO + +//Flags used by the linker during the creation of static libraries +// during all build types. +CMAKE_STATIC_LINKER_FLAGS:STRING=/machine:x64 + +//Flags used by the linker during the creation of static libraries +// during DEBUG builds. +CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during the creation of static libraries +// during MINSIZEREL builds. +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during the creation of static libraries +// during RELEASE builds. +CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during the creation of static libraries +// during RELWITHDEBINFO builds. +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//If this value is on, makefiles will be generated without the +// .SILENT directive, and all commands will be echoed to the console +// during the make. This is useful for debugging only. With Visual +// Studio IDE projects all commands are done without /nologo. +CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE + +//Path to a program. +LSB_RELEASE_EXECUTABLE:FILEPATH=LSB_RELEASE_EXECUTABLE-NOTFOUND + +//Value Computed by CMake +flatland_viz_BINARY_DIR:STATIC=C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz + +//Value Computed by CMake +flatland_viz_IS_TOP_LEVEL:STATIC=ON + +//Value Computed by CMake +flatland_viz_SOURCE_DIR:STATIC=C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz + + +######################## +# INTERNAL cache entries +######################## + +//ADVANCED property for variable: CMAKE_AR +CMAKE_AR-ADVANCED:INTERNAL=1 +//This is the directory where this CMakeCache.txt was created +CMAKE_CACHEFILE_DIR:INTERNAL=c:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz +//Major version of cmake used to create the current loaded cache +CMAKE_CACHE_MAJOR_VERSION:INTERNAL=3 +//Minor version of cmake used to create the current loaded cache +CMAKE_CACHE_MINOR_VERSION:INTERNAL=25 +//Patch version of cmake used to create the current loaded cache +CMAKE_CACHE_PATCH_VERSION:INTERNAL=0 +//Path to CMake executable. +CMAKE_COMMAND:INTERNAL=C:/Program Files/CMake/bin/cmake.exe +//Path to cpack program executable. +CMAKE_CPACK_COMMAND:INTERNAL=C:/Program Files/CMake/bin/cpack.exe +//Path to ctest program executable. +CMAKE_CTEST_COMMAND:INTERNAL=C:/Program Files/CMake/bin/ctest.exe +//ADVANCED property for variable: CMAKE_CXX_COMPILER +CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS +CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG +CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL +CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE +CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO +CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_STANDARD_LIBRARIES +CMAKE_CXX_STANDARD_LIBRARIES-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_COMPILER +CMAKE_C_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS +CMAKE_C_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG +CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL +CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE +CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO +CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_STANDARD_LIBRARIES +CMAKE_C_STANDARD_LIBRARIES-ADVANCED:INTERNAL=1 +//Path to cache edit program executable. +CMAKE_EDIT_COMMAND:INTERNAL=C:/Program Files/CMake/bin/cmake-gui.exe +//Executable file format +CMAKE_EXECUTABLE_FORMAT:INTERNAL=Unknown +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS +CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG +CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE +CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//Name of external makefile project generator. +CMAKE_EXTRA_GENERATOR:INTERNAL= +//Name of generator. +CMAKE_GENERATOR:INTERNAL=Ninja +//Generator instance identifier. +CMAKE_GENERATOR_INSTANCE:INTERNAL= +//Name of generator platform. +CMAKE_GENERATOR_PLATFORM:INTERNAL= +//Name of generator toolset. +CMAKE_GENERATOR_TOOLSET:INTERNAL= +//Source directory with the top level CMakeLists.txt file for this +// project +CMAKE_HOME_DIRECTORY:INTERNAL=C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz +//ADVANCED property for variable: CMAKE_LINKER +CMAKE_LINKER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MAKE_PROGRAM +CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS +CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG +CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE +CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MT +CMAKE_MT-ADVANCED:INTERNAL=1 +//number of local generators +CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=1 +//Platform information initialized +CMAKE_PLATFORM_INFO_INITIALIZED:INTERNAL=1 +//noop for ranlib +CMAKE_RANLIB:INTERNAL=: +//ADVANCED property for variable: CMAKE_RC_COMPILER +CMAKE_RC_COMPILER-ADVANCED:INTERNAL=1 +CMAKE_RC_COMPILER_WORKS:INTERNAL=1 +//ADVANCED property for variable: CMAKE_RC_FLAGS +CMAKE_RC_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_RC_FLAGS_DEBUG +CMAKE_RC_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_RC_FLAGS_MINSIZEREL +CMAKE_RC_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_RC_FLAGS_RELEASE +CMAKE_RC_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_RC_FLAGS_RELWITHDEBINFO +CMAKE_RC_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//Path to CMake installation. +CMAKE_ROOT:INTERNAL=C:/Program Files/CMake/share/cmake-3.25 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS +CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG +CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE +CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH +CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_RPATH +CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS +CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG +CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE +CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE +CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1 + diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeCCompiler.cmake b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeCCompiler.cmake new file mode 100644 index 00000000..99bd9016 --- /dev/null +++ b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeCCompiler.cmake @@ -0,0 +1,72 @@ +set(CMAKE_C_COMPILER "C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe") +set(CMAKE_C_COMPILER_ARG1 "") +set(CMAKE_C_COMPILER_ID "MSVC") +set(CMAKE_C_COMPILER_VERSION "19.29.30147.0") +set(CMAKE_C_COMPILER_VERSION_INTERNAL "") +set(CMAKE_C_COMPILER_WRAPPER "") +set(CMAKE_C_STANDARD_COMPUTED_DEFAULT "90") +set(CMAKE_C_EXTENSIONS_COMPUTED_DEFAULT "OFF") +set(CMAKE_C_COMPILE_FEATURES "c_std_90;c_function_prototypes;c_std_99;c_restrict;c_variadic_macros;c_std_11;c_static_assert;c_std_17") +set(CMAKE_C90_COMPILE_FEATURES "c_std_90;c_function_prototypes") +set(CMAKE_C99_COMPILE_FEATURES "c_std_99;c_restrict;c_variadic_macros") +set(CMAKE_C11_COMPILE_FEATURES "c_std_11;c_static_assert") +set(CMAKE_C17_COMPILE_FEATURES "c_std_17") +set(CMAKE_C23_COMPILE_FEATURES "") + +set(CMAKE_C_PLATFORM_ID "Windows") +set(CMAKE_C_SIMULATE_ID "") +set(CMAKE_C_COMPILER_FRONTEND_VARIANT "") +set(CMAKE_C_SIMULATE_VERSION "") +set(CMAKE_C_COMPILER_ARCHITECTURE_ID x64) + +set(MSVC_C_ARCHITECTURE_ID x64) + +set(CMAKE_AR "C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/lib.exe") +set(CMAKE_C_COMPILER_AR "") +set(CMAKE_RANLIB ":") +set(CMAKE_C_COMPILER_RANLIB "") +set(CMAKE_LINKER "C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/link.exe") +set(CMAKE_MT "C:/Program Files (x86)/Windows Kits/10/bin/10.0.19041.0/x86/mt.exe") +set(CMAKE_COMPILER_IS_GNUCC ) +set(CMAKE_C_COMPILER_LOADED 1) +set(CMAKE_C_COMPILER_WORKS TRUE) +set(CMAKE_C_ABI_COMPILED TRUE) + +set(CMAKE_C_COMPILER_ENV_VAR "CC") + +set(CMAKE_C_COMPILER_ID_RUN 1) +set(CMAKE_C_SOURCE_FILE_EXTENSIONS c;m) +set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) +set(CMAKE_C_LINKER_PREFERENCE 10) + +# Save compiler ABI information. +set(CMAKE_C_SIZEOF_DATA_PTR "8") +set(CMAKE_C_COMPILER_ABI "") +set(CMAKE_C_BYTE_ORDER "LITTLE_ENDIAN") +set(CMAKE_C_LIBRARY_ARCHITECTURE "") + +if(CMAKE_C_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_C_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") +endif() + +if(CMAKE_C_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "") +endif() + +set(CMAKE_C_CL_SHOWINCLUDES_PREFIX "Observação: incluindo arquivo: ") +if(CMAKE_C_CL_SHOWINCLUDES_PREFIX) + set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_C_CL_SHOWINCLUDES_PREFIX}") +endif() + + + + + +set(CMAKE_C_IMPLICIT_INCLUDE_DIRECTORIES "") +set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "") +set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "") +set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeCXXCompiler.cmake b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeCXXCompiler.cmake new file mode 100644 index 00000000..977d926f --- /dev/null +++ b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeCXXCompiler.cmake @@ -0,0 +1,83 @@ +set(CMAKE_CXX_COMPILER "C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe") +set(CMAKE_CXX_COMPILER_ARG1 "") +set(CMAKE_CXX_COMPILER_ID "MSVC") +set(CMAKE_CXX_COMPILER_VERSION "19.29.30147.0") +set(CMAKE_CXX_COMPILER_VERSION_INTERNAL "") +set(CMAKE_CXX_COMPILER_WRAPPER "") +set(CMAKE_CXX_STANDARD_COMPUTED_DEFAULT "14") +set(CMAKE_CXX_EXTENSIONS_COMPUTED_DEFAULT "OFF") +set(CMAKE_CXX_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters;cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates;cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates;cxx_std_17;cxx_std_20;cxx_std_23") +set(CMAKE_CXX98_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters") +set(CMAKE_CXX11_COMPILE_FEATURES "cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates") +set(CMAKE_CXX14_COMPILE_FEATURES "cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates") +set(CMAKE_CXX17_COMPILE_FEATURES "cxx_std_17") +set(CMAKE_CXX20_COMPILE_FEATURES "cxx_std_20") +set(CMAKE_CXX23_COMPILE_FEATURES "cxx_std_23") + +set(CMAKE_CXX_PLATFORM_ID "Windows") +set(CMAKE_CXX_SIMULATE_ID "") +set(CMAKE_CXX_COMPILER_FRONTEND_VARIANT "") +set(CMAKE_CXX_SIMULATE_VERSION "") +set(CMAKE_CXX_COMPILER_ARCHITECTURE_ID x64) + +set(MSVC_CXX_ARCHITECTURE_ID x64) + +set(CMAKE_AR "C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/lib.exe") +set(CMAKE_CXX_COMPILER_AR "") +set(CMAKE_RANLIB ":") +set(CMAKE_CXX_COMPILER_RANLIB "") +set(CMAKE_LINKER "C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/link.exe") +set(CMAKE_MT "C:/Program Files (x86)/Windows Kits/10/bin/10.0.19041.0/x86/mt.exe") +set(CMAKE_COMPILER_IS_GNUCXX ) +set(CMAKE_CXX_COMPILER_LOADED 1) +set(CMAKE_CXX_COMPILER_WORKS TRUE) +set(CMAKE_CXX_ABI_COMPILED TRUE) + +set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") + +set(CMAKE_CXX_COMPILER_ID_RUN 1) +set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;mpp;CPP;ixx;cppm) +set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) + +foreach (lang C OBJC OBJCXX) + if (CMAKE_${lang}_COMPILER_ID_RUN) + foreach(extension IN LISTS CMAKE_${lang}_SOURCE_FILE_EXTENSIONS) + list(REMOVE_ITEM CMAKE_CXX_SOURCE_FILE_EXTENSIONS ${extension}) + endforeach() + endif() +endforeach() + +set(CMAKE_CXX_LINKER_PREFERENCE 30) +set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) + +# Save compiler ABI information. +set(CMAKE_CXX_SIZEOF_DATA_PTR "8") +set(CMAKE_CXX_COMPILER_ABI "") +set(CMAKE_CXX_BYTE_ORDER "LITTLE_ENDIAN") +set(CMAKE_CXX_LIBRARY_ARCHITECTURE "") + +if(CMAKE_CXX_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_CXX_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") +endif() + +if(CMAKE_CXX_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "") +endif() + +set(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX "Observação: incluindo arquivo: ") +if(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX) + set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_CXX_CL_SHOWINCLUDES_PREFIX}") +endif() + + + + + +set(CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES "") +set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "") +set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "") +set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeDetermineCompilerABI_C.bin b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CMakeDetermineCompilerABI_C.bin new file mode 100644 index 0000000000000000000000000000000000000000..cc78672fb629f870f4721d18dc6d6b41a32c54fa GIT binary patch literal 51200 zcmeHw3t(HtmG;P%9LKRE0}(job>qsxZ5}9b0F%TZ#d5d?IjD*AXfepLB*!AMB&4gv zp|G{%7~gYdq;B3*S4^1#+VTym13+LkUn-EpEx>(;>qVeGMW8s z;#1SQ4X&rA)iwp=LNpdx9rHE|K5sZ2k%WeT5KDxGU|4WoxlCw|_ye;G3i7NPYug{c zbD4BgYKk7FAN|m>4*BUv-?uby_<`l0IQ$F8*T4RuCCcePoMHpqar6UAfZJSe1zmh)%;$l5P4OV{V=@u&)xIgN)T_{5ZPM!0M3*-NRTpM~okB zqjD~&R^A8R)WOI3%$5Qz63QM)AR>WC_w;s=6&S0Z9rJr7FJoK348#_MnF!Y-q9WNsNG!%X&*b$#o{sQaBgD;;v8g>2x%WXrwfE4AnYK1us!FuFO3LE z&@CE?`F}6>Bz^ktL2vaxI~9@3f@tNLh_cW#{b%PPDlb4(Ndfk?h2H?w>~m&k*i==;{6+ zE^l!xKr-UmYF;?Xw`T+(;|BD!v{m)au-DX5DQR8{U{#Eo< zfAR~61}OC|MGv2b=ut|oB)Vc^dgH~2x&=hOvk=`uB+nCxM5&*gf#{n=k|0@aZ8AnGC9X5zBC2vLlx-AK_Yik=e?O(x<_5?3Nt5h}R$R744CO`z=8iNRWGt&Qyb zDB0@fxrnl;7k)blQ4^6Iy%fiPloW2f|nD?a#G(-@~xrLd#D>9rsxPsdJ&1S zh=^~Tj;M$PI*klFoA~|cbVUD6Tz-;|=mk=%gm7C4_iK{sLMm{83m-n2C}V+7Y5}pc z)>_!pPk^(B(VRVcwIc-dd zjB4$B3q5;IMn7ynn$5NFy=Cc1RRxU9PgB%ES(Qc>S+a{V{bwU#k29MarDNAXPbdB4&^*h5J3mL!wXN`=3t=&=!uJCG|&Lj2OPDi7&CUV`gqJ$`Lo{_oUa z`=7Z;18~P_?eOyZs2pxJy+CUW@u@A2qc63mj?bT7I98e`pG0}N;y`_3tGo>w;@FuBxySJ?oW4|< z2YTrD-ElaICSS`(5&|bAlTREMdd5td;GmN0*uyKlr*EPLQ?DFm_lSSNZ@1D6yrD zhxQ|)VttzyEiSuDIsYAuQ`w#?LK<#v7?{-QD%gKFTv3uwvq91P@F|2BNN<5jr;?Zg zS+_J{+-87n_s&g1uJPtzx^R4I+a7e+^epny-^3Xv|u{Lb(|F5}&!r{@XwfbP1u0htV5=9#|*R9+BLfI+&?b zhEja5z#UgP_N-6cNo=rMi}Ep1esgCL>L0y(h?w^uCl-frZd7|ntA_HlYWqxUfj98M z)a1EljE~j(_LD$;1g;`E>S@H0_>C_?M^!!Aaa=UCRioLnlt`f%b}!m({JjKAYM|U_HY8rE+0lA|FYx1S6y{FlXR=vLA*(!M}is^3doi zrD~N@y$Y6-k93_}KwZ9a$miDNQ+;wOF`4AEE6dQ>z}W+5k|=hq*K5(iT?OZoEX623 zzE?@(PrNq2j;@Ct^Ju1!umidJoFPTW^0CugwqtSUQ>84q6VF1}(fj$_Nm<@~RJ9Oa zVg&d*Ep8Tgo!+|L{GoP3-AIEgF`i>Qn2&7$o1%&?9-Z??hr>Dzy)z#P@x6m$r>PXFfhy(Y57lWT zq|-yvgjwt^(;3ikQU&TX9s76HT7t^vWy&6BTRKvxv>flxv<+mp0sWCSHPecCt!`#N zVfko!T0bzd-hqlK%eg=25~i;+TKO+guDt!_F~v`AGo%Rv33iWq1~xhmXLcue4OJSG zr~Uj*uj!WNgB0h7_Wjs0(kSaRC~aD4DcKnjm1a>XMkRTf6K)ToPf>ft)bGKHiHUf; zLgpNppgm`iu`zAmTQnpb^nrZO-!)R+htu=4{`H#vfUc!adQG(xseGl_c4QFi-p(^X zlCn4+;XZDXywOTe+vWXFeQ75i9WrIn0Mzc<$RB%P!NKG^h&K;3`8IQ0%5u|A8c`dq zMOlCtYIM34yK1pDf%-VxhN&EQkvzS~BnSC594UdtoQhHr7FnmYMRhBBTR`JT=CX=> zOGBP-RoQ`*rBrL}7-SbawN_Dw=i8Imac;EIcqExysSPgktwVr?>Bh;S!WHJ0{+mWrLI?N7FN4?sFtm;zX4HtBC zZy0LtGr&(zGVee?QGck8IuE3T5+FWt`AG3;zABDCLf`grpEZp$|I5jickh6R)UJ9y zNYLJ^4GoGia3hXHVUE&|io)=rNS%*qMcLMZz(b+a2SI%+FAs`^ewiwlvgk1~#-3c1#QH_Kj<$n)g^wCgBl!PY2j_p^&ExTZW$!2B4+oj%Hue2W5jP<3 z4>)CctYg+}pwnaU(I!#){iu;YsvKJn+Fu&*+-ThC@#HA_E|q@5P6rZCeQ9hhPCt(P z7*DkSs!{7hI@9C-3j3gK3>4EYO7q!9R9w}R<<321z!NWDY4UZM4&HzYrdBjTyzoTE z^z@Btx{z36pwXk8p(6n|DM#8gG+5p;Il6BfL+( zWZ*V8tGNZ3m?!9gli>HfYEz5KZveY8@H>7Fjg)v4|FoV{=|+{qos!39G} zzQS`4iG>oofqF(K6$4&lD?29OjCunw=6_9s{fUmI=&*74l@IaD0zW+InMroG(Ze6~ z=gD(_@=5#h6)PPpu%;h_=IHY&*v5!uduc1w2aH_t+_&+{N*)yDw?w(W9Y@xU2BPdZ zlsLoLwa6&{%GFhHSqw_Of<0gVt1y~Nz8tTaHd|Yf7CUdZ4uZbAz27Q!eGR*^J!01_ zR@zf357VYkUQDl~t^ot`JW()voYL(I$TxF3!Z+b@a9$kvu7ZnD{74Ro0DS#$J1%S* zy9&u_mutz+HZe9C6zkm-46LszZ(X2$pzJGU@;T4a6AGDnO0ba!jlk` z)=ND3m8=47{A@4=2FahWjA(;I07kC3hY<1$coKyXA|+bqGwG|M{J8QE^;3ItmRP|+ zx2_7^`0HyS4drO3D=3R_ErAaTWH?&6kT6}Q=SI>A91Eg>2~hox2(;;j2E1pURvM`t z^ax2qJu>A*>cQKsByY8Rl~66mORMGIebnaQBzlG~4J#bAj^z$|vQS!8mfD3W$XpAa zu95phdACdMuQo4wQ|x$GT0_QjyJoyBc7`xi<=4c{8>~&D{AU;0v#M*|9GCo|!@T{h zh0yndFm=rn)$;$)P;32dDplS7M)sz2#rDUt#jbDQAozwMfrqnrZtId!U6eU5&KRv+ z-eqj5+-E$WIrbSRz~*~P9UjNkj=GQgG!Kv4%-ajaRC42?#Jl2*9x=6l#3->ZTfUNO zzXJNV9oaUS3~zc-CcAU5u?3i_ve&D9N|pRhjr?b){E8?)uk8Ib<_I1-;t(@Ql%G&m zLqV53=!9iT9XqjxrfTF~7jgogKzIJkdLP*A?EfnrsSf-Y%zgXCwm;MB)OZ=@s8&Vn z+GyQ^BL%8bEzDi5%6F+s*Uxw%TAUzuT~HPRL-Ll&SMm68V^W$17jYquE-v|LdCHG4 z24Qx37rrrR?zjTuiJs!ubiA9GD0aZ>uHlT(%$cz1wiI*mC9P!FNt*-MQ)f5it} z4i^Tu9lnF(@Ze1;`VI8&NMVzWzQ>u>MDmdGlWqSer)s^Sp%MGqcw_A zOc|RCi;A1t$|FzIRdFeJsK(>9%MNMkPq=@m_U*iON0QekX02EqhFF-LFKIPhnkE{% zu8I%V40S;FkBA)}wUVqX?)NWz3v6>G-&`~q+UZ~)pPY#BgWcEOvisX3C($ze4y+Q7C+O@O{X>6Qx+TS zGr;HESni-8eNvtTMZgjUia zr)eNemucTfx`86X>Q1wtch+WWIfj#?8zTtLQ<+JPp<8s%`NCn!-)GO+GGQT`>>gOAHqWmq8ze{+*gS3fNrzc13zawv3Pv{$zO`0Wy~hH&#JNme84I%lCGn+ul8l2!xS`4 zx>@Te3}?(28_Y%XW}XJi3F0Qt0rOonD+NatmWmmq?_GT~~sGiiLBePUwSPFzD_0WSKG@TD;qV7%esd0KH zl)uSn9m&~|keYNR8s|V*3Xb9IbJRG8IMe-Cl`}16oO2-_SLM>fFkCb_ITelO?M5q3 z3)kbN|NR|s-1xv37)u5(O_?i_*s@9P;e35g7L)A5`6%UW!+Cd3Hj{+md_Cn0hx1Y7 z6K1?~KsUATH*}gVLqa!2wr&B<{P{M$Bug*h{m|*mx8VuflnFP~b;GTFq^btJc9z<$ z!+bfr5?Y^N0f@EpCK4;F)3gnVUeiBsRG%02nl>W^sv%LSqOF9kJ!4JTh_?Zt_G>h< zsR60>{l-qySCP7PT2=5 zD5CdN)NDx~s)!X7C#LqA{s}ekxEL+oz?s-PO>rdX1wU=qu|zESZ$3ZWpZ!(g(_4n% zeegX0hZ|rR={RTIk17n7nJ4~3w`%LqYF(_fAu>~`C zyoDm{e-<}lKQ!r7j+Cy`_bazpZ5X1s+o-&D3Lnrb^^#inWb6T`SlNI^FdP;l;y8?VlW+BnXu4;X@8hgDqB^WM3kx)Y*HF zDqgNPtqO(r_$ySI=I90KGQE+_WkPGbC9Ki8%3O_U7LBaUR;A-1T#v8k8 ze~SHybqp^SBwGJ&uom&1|8lYG0qYie`_^_O$}MgBk5-Zj8|aRDbP>19D}HX5T-YT7 zx2$cRoxJRvz);z;;WAv<=-SHdZSJ5GHh-B?@D})#9a3ft(MjLHb;Z8`E6UHLEHh9; zlnaJQ_(8&7nU4Pe?`*MgC;UaUoQmr0R=%Qw?l-*t9%{O`2fDSUEVuoH zcgjV)xxJ`P9nnR4Qa=O_sz)_41deKjII0y@%U>62)Fa!~d&nJ#)RLXrz<$|fbyAi# z!uDJ;A8q{6^qaiteP}vmDM6dOqI?*8(v-!*^HA4`_B(!U?r6d4L@!qi=8k4ssBpR9 z-QCF4BlGNQPWkcn_p{6!mLivWoNv1u9T%VFuIVzm%^eGY>{^s^JW>ZEH##q_b-JYq zja8k64*6Mm_j?E1j}@6WoB<;Fak0yBhFC$z0Q1czV0xV{cLNK;1W46}Ssl6tsa0Oq zOp^dJ2)Q~xyO3c0Jd^|aYuGY^O>X8S7+S>U5RA@aHiFV@72o1wF4oxTblpIs->sC> zR-1Qi;vAe??og-VX*3lpb5#*2#bu?n?X{40%1DBC7>e-D*Y(zqPQW|0OAOkpoaOyspUP-!@pA^tIPBdemy}g04 zF84(8g>-gPzNZ!~MUl*ljAY(=WrK>JueOe-Na_eit#kq8C-?~C<5;Owt4!c~uK~?z zaaAx`7Cr^1f!+D7q*~iv()q;O-;do5PDog^d>I}J(j$%)Vpqu8Pv?B@PTm!}u1i&Q z;<3eRjn2-wqWrYjwfv7Y^2=3Si}C7jkS+w!&Z7x#*eO?iLzI`V6XlveDR)gG)0sD3 zi2_1&(}l#wi!t9S2;*}sMCrHYP&&IuedA{CC`Q)2!#!ojn@+rQ%Mr5=I`Fteez6L! z=REVd8zB;Q4>$$ox601PtqTTYdGd!djxCTL7h;8}J&py3V=p5zZ+sqIl-lDgzhDjq zM`DKS;tS3}!rVdrN%{FPn6%TBPveF}d;rLOrW|Aq0~0Wyj?zmB_{Dkvd4J0C!S^)6 zb`XkPHP7(l1nw%q?~r%L*}mo_N|eG=}#l&T!WMHyROc}4pBDX zolwe>KyiiRJ9A+Xi$>w2W)3?zuI1!z_JY{ideEvjZS|*AbJT^hIPI`QTR?Rz} zBZy|HhCOBZf>z@JwMH+xOl`LHAncd2ypPi>o_TC+QOkGW5Nh7IhG&RFVajqC6eRmx zouX^GhqsBwSJDG9IOVDc`2K<#MP49*?L{fe9U5;sx|nxVtz57tFZrdg3FKe2OS9DuP%ZE+Z0qxMfn730y2GFyqF^f=go8$jd!Hhk#6eV!?B33*?>! z3-`rxQauYEj~$hl@9A>j7_-j^U00)p-V%6m9QBOW8u>BR2xN6Au35dc51jD+`989$ z(_D2lW$D9Wj8m;9_6x`jX${KjK}n2xhqboy+KnpiSE>%tlx4Y!ct%C6!a4?oL*B<7 zMB84Pp4u~%Yp(jcK7an$4&@(QVfg+le^N^`5SN#apo6egabXJ4i#(CG2IWdaZz{#z zEIlqg&L(i|`>R#NDk`(~zGLu}0_D_&MZ@N!AP9^rC2Dwgl^LBf-)y%KCO4F%~ zW0DFkegnBCSt#C0^-G7}tnqq?=*`;DB{wvPyz=XP@P;RTBlSr-w6Rh5*W!YLhZFF^ zg9Y~it{z$KxNzRnz_eiEJjffwaXk-@^K%S#(k2kXg%o$$K!LcFWsk#$%V-r)-sy(#K6-Pr=R*goKEHFQVFG>7uC z>6-*PO&_eK9Q8}5DTS)Nre6X>GWD9CL5dtpujzR$rQZz!!-8k2X0OpeEf3@6g6~Mg z*ku%SB;0U;q$5$W6^o}*3v)r)k8_t~yq=@E!S6&WO?x@RQ4>L*KbpCg-@?t63x#cT z(F@Xi6KW_XzG%_rsnc{FtU&g{9a3&D1Lbi(CD3VlB0=_`F`(M$$4IF*`Y$b|t4%g~ z5I9U&o7g$Ufb30QMO$ARSJ3g48wbbdGnOd+okS?FkAoZVejDR}6tioJk&Mbr$VS~u z1L0Qcu(W|tN_pATMxfL57J8`HbQrO!*de4yF@AGxZ6V!5arj!u z>#vrLNwL%T_yCS01(mCy)fsy}mBBNPef9a(LTBw(h>G_B-yt(?GLhD3 z^?e5pcqz*UBI6DYma>n&>$i%E_+7txO2K_;43QCB(ArL_isaqhAqg3B`GNqN5d>I!S$D_&MG8V{KAY*}y1u_=MSRiA8j0G|l$XFm_fs6$* z7RXrO^J)S6a$NEg_-`-N*hRStfqt{Q3is`*>~amjL8TbKO|S)bsf2NEPN&z$?XoHa zwZ2?stuQf`#INpE33Sg7zq(f?(48*1N;nQP7R!-j_H{a5OAEzF_{m*N^ zWc12dAY*}y1u_=MSRiA8j0G|l$XFm_fs6$*7RXp2V*#=NJxa)YK5YwZJDu6-X-(#n zu|UQGA8mncmdt!fx6+o3|427SCY`ZB#sV1&WGwJ0S)h8!;w$IJgVzTljUIngid|(Z zznWMtslIY?!+dGIClHGS{8z=Q7kMsQQdR9-;$Xa-*K1fW1w4_MKM=dhRb5-_s`6AV zaaKE)aEkE-bl-=K=64J@nCqh5Q@a(LZngfdnK=4L4zva=0I~KwtgJ8%oXu@L&92b zEa+_r1%yOA;1?PKjgeSD2!?}F&>IR~?@bqE1Yd6~k_h_+4`c!#oqxbTEJWcQs-D94 zRoJpsgVi3m!(zGhu6r;Ucn!1Yz)LuX&ffA3-V5^`S_&{uu?~?cWBJg@KgtG6ZI7a zFBxRS=!GGw8wH7MLH$Xyfve&T%@o#05<(oE74i$s-uN0&1%!Ac5%Y1IQ;s*um8P6G zO18(qmI6XUP@+0uzc>;T0xjO=XecmW5DM}p`kK6$a6*Mp8Wg4rwwA?M- zUQIUVd?S9E4!T_HVc1Yepx*Vua6U{-GsCV)HFXqvut^IO*pbHEG$i+-ZG+GYc?*Rk9{PA6tWT6^r3t1YtHSzz-M;Y&mGXtbv6% zt%Ndx`B68{qQH6CdgPa~T5u6q4QS%{=VhyrI@xKg$}EWk8X#>!B8OVZ4T%ZI=&|2Pr%W9d_kkvLEbXVlr(If!|#V^ z@%fmUIt{^3NuP9enb?se;TD@c;SIvKK?>NKT=FTPfR~dOg~3VKSfMG zLrgzGOg}$NKRrx8J4`=0OrI4P(@Qb_mtbU8gR)Aaae~T^5gF3t8V$Ud89|KCW{l2R zY#ncP7UmbtssJz)uLd@bIT*wInFUUB5K|xtj>p@JlC_YYxR7@Pm^N zwVdD<8$mJLLn?Z7AL_)%!@He}$WAaz5X)lV0-~oD+`H9%(4Yo&KFy`;eUzpjHN;Pp zvfJ^?thfIwVHnfP@{yi`;Cuk#yHqm#DK`wKKh2bwROKDRb5O+#8XQypcampJJ0F24 z)a0{i(5}JKQTG1`Hksm#1u_=+j96glGUu}So*Q4=zVu-AcXD$|c3=9fUhgL1)&4IWS3)C&Gu4@c=rI0u5A4+)E2CuJc zNCZQpP?a^_H38b^#p`%-nJ?y*d`)$=vGtXa<|qng`67WRxcZuFD?Q$ZvW5$1NBs@l zTGJ3yppU@gOq8*}$G5=SryOS;{`jmiH8K|X+*@G&shRomx$oq^K|#ahEH?Snyr=&LNTp{O!J+xmj-BHnla; zz$WUjLCbH@Ao3Hb{H|f1-xqLR#OA^do&@PLf+y9@HL%=8X%K%>B%kv5Q>b~nbE2kt zUX2;fFt9TkfG4@B{A8gn^JD{?tk)+nY!}8|l@RBikqT}sow$=ITa*qdz?`Z~h8nr(jMETTnO{`#E zCo3@A#Hi($3$ed5sya-yPGG0{Wp?VkZ?aQSK52CMxjCqJDa+|IMzia)+*G&Epw^v+ zzs!|8?-C~182bi7-F6KRFdAo=lFO#}^VpQyiEPR|q?Z;(iQ1s`#Z;{ir@MgzzV;%< zu0_~_@j{S3BgV^J$WtA_si8?M`r+xJ{-eJZMW3ltv)BZK%<1h~pYm}4yBLZLtZ0!Yhd#GXF|bn_ zfYa>F=P~7JY`roHqHgAsu|UQGpKS}^uT`bEuYs7OAxTcSEQIp~MD(FQ&yn_d>T2Oo zBmNHS+`C?yb=!=z0xe1PGoR0{1zPc-{Rl$ee==sXFcv`=M7kC52M7w%xZ}whPRE+| zGpvVzTM=wXjR?np-vXFKC<6W<;HwA%(u07@&jAEY6z~p&T;R6>K88?8^ng>(1q6)^aGrqn zk!}T?G>x%a2oLB%*iU=_e}Zrr=_Fu9G5+Qo()EBpM%YYvz@rE^AdT0tY!`x^PjI|IR@P80C5gzcHUxqD_-Uj#NsUklqS7X%^%~+6EXw$lVT}fR7;*B8?YatP!Dz@PPdY4C%u28M_SO zFzQAD??D&@J_%^Ffj`pedq>(=u9^cKfhU+i*n+fuF8s|K&tV7&%cXi(JPd=1XjpjCrsX>hUzvozSJ z*=?5w@7CZJ4Yq19tid`Bx-@u+rpMpg-Y3hy^wnX?`H<25GS<~JToUpQ0(ba#!*qY3 zxzRVAPfum$b-FeF_H<@mt2WHc>yI~4`@JzF=d}=fQ1UKd!xF?+1Kb@_Xq1 zyC)cKjClO?#y+y%6AH#9#?C~!FA<~5%$`7tFQ8tcKWjX!2REgqK#RmCj@yFVZ6cK(<$k&C(#H_xaL1XO zf?b8yfWQskShE-Rk7Kaac19{T23O-IsVD5k9o%&uZ*28i#;!BKcKXF%y(kb~8;nK5 zbSna9E`w(^_-j<-kw_{J2-M6H!)Z@|xs8FAAb7mSASx11sIoRThX{y zKE^H|&OxrbA+cKb`$(y^w`X}`fz{|-Xf8Fy0$#s|Uu`BccyM(NEqLhedjq~eLC022 zXO)yw2H4Z81mVCs4;P|FZPgcPR%O~f!oJieu@M%+{d#IfqVGtcjWmh068%0adzEw! z_!9Jmk@QNtAs0rFDlhBNbDrxGLEjoxQFeuij2R4rh|M%%RQsaqJ#n1TdD<&cx{3N( z@&_YO6T*i*YvZ`$9+nzWczd>IT{G^YL@@w}wj23)6qnL9gx|Ez7uR?uAWef87x4KA zTx#T(G~Iz1zWLGY4f_I>_+kg7VbiiO=quF;<&H&sxQbitXC-OaTHYr>vZI7uwjMpz zOjns5ahfKSVv+3Xq2ad7g}xNgjqI1krBbNU8HxRyd++0!B^zdn@{81LIZrQ+1u*`0 z5uGL$QQ*ymNiz)ZMm#7o5T)jHShr5E-KECKmM|lGmEB>g)kmMko3SgZmM*DsmCfaY z^&>P3a-UfWB5)8ueisD2pJU{Mh3g9b&vzmkSjm$`+ zgiE^3^S$xr*=y$%;|qr2U?UC|E7Ds7D?wN&#S(Fewg)H4t*n^WL(Mq$g)w|ZP|E`o z3tX2#D*?Yd7F>(X(&|8bs4%^_if<{QwkxnU5E4QZR}_2W)#0^~HGx>MkO(?_v^T3L zZuExYf#ORRotwQ-eTng>c8z$7t9u^iSQ2a!0B7K1EHax5ZCLRuvb zggV*%F~S;t$L$D9E#FtZjddER=3A zSoARUEmpJ!`KfOiiyAmI75x{7SsY*g8Y_x&dSscYb_ zV$6R-E(@2g_35|)R>CG^O+1~g24f7{6 zT3ujOCORNG4MRz`;bq3$JY`^CBt8citJC;#bfJOOBN4ibv2u^p71SGovcu+a{DiS8mXI(y92Ev(F5wbn7OftQUb~d;lTsW2bg)HecgP#x5t_ z2N0;;9SCV3JJH4BG3sz`V+P_JWReJJA3LWDgd!m9Abzkt=eH+~2uaW_7>@aWKleC& z`tCyS^*ucakqa}XZ~3W+vd~j~r{^ImFF;gD1-Cg6Jv$eXlSqD8gy^dj1*l*PMSmbx zQ_n*5jao!?m`{CY5y?7=nu(<8bVMf+-6A5{j`7_$hYFq|-1pG)eLtRu$W6FQNvgk4 z!7NJEoq_1P#OmjSD2md zva%RajH+Ev(F%&55fDuz;!YA*B32P9xcWpy32IHC?3anbYHF>G?EEm<>c-iKvZxpS zGyzc)k-T*gq8&sRq^zF|;UNVtA(Ew}zMJG*MWuI9H$F(wVUqNG5@P`oUq2O5F$r`M z8FnV|`^m|OenVV-mXGK;Qfms~HWTg-B-MFT-~bmsd@@nS0w31`VrQ+ju+wGDT`smC zF5GLhGJHgz(<+Ev1?C$WGw(1O^dG9 z#V*rX@I8aH$e%te-+s8fwc_0SAZ22PW4i%xk7+8hhN`mUUWg3@D?3=WU-{Mhsgx-1 zL(}Db^FBZti9xo5qCo3%l?_!UcHqP7f0^o!sss8`<<}@uX8i-v0;ISQ%5J30Taz8h zMBaAHai(NZ`4VcltDqi0d5vmaj#Nid(rBJ1d;{StReU~(YbZY#(_A@B#b2ZB35eu< zN(*5RBF$TpOiC*i{+XgjhB59yt}F@hOGm3bsQXw6uAlYjwR!pXsloQYbCLStj?>!V z<@ZoI+-iD())>m0RD+#?Mur*eY1G<7J*ZG>gVQ>>y&`lkdNz@-T67E0{VUolmIKKW zm+UoNN~P3Hm!;?{1PcE2WfPA&mfNF%E2Q##{#uX&Q)Z{r<6c$Ff3KvdYg&ys8Om^qSU+< zB4ezXlqJZO9emKBTnk2EUfO>yYFUx80!~*>99Da}GGSQlQ;`Gd^nU89rjIyQ#u`5# z701WR86^5X#dSWqXvC1^Bz`rL%~Ic5#?H|4HguX zx8LJaTO31QYEK=XKR<7@G><=y@^Zz2`ovaw8#KhRGZ%7?;axa=sWJ!j(C<5Aa1_b+ zawT`dUyRfq^<~rLcc=S9*;JjLZ~wb-jal^dVC+BLM!Fk&c9kWE6zit_{!o_q8BkyB zEO`GefW4;o0O+I9puCR?ItyM>%byxvPVHIHo~z4L{bR^rsiz8QtQU$t`+S-Og<|$TQSPAvfeHjlQD467+X{Fl*QwaL8IR1e-*=*Mj_^u357+y1b%6@W=aTFQWZ7WqGAH8p<~ zS5KO&lO<@(TERlO0QwT2*~z|JKo0a2LYEAo*8$zXMx;F=xjA((Q>P53_+EiKu5#>I zpSYdaV6_(IBclAq_F~k3>&`)9-hZ4}9K^X%?E$SC%G0XtF|7vPzz0*4=c-XYR`1)7 z1N9NOg5;>D5l7-Tz5pFn^=QX&{@_-PX3t_Gg<{yfh|~6Iej5_k(y%1*TO9z~3Q{H5y_I zVwpblfZs7Do|xEDmdXEz?{RHnPcLfGupCjJS{Ftk`F@5T$*U!wA8d|y88opU--&kk ztc14ZV)yJjUYK#qG_bzFRCy~L)=B7{xk!lb9S}QBrAYNxDKGw0okl`BJtR$-#qKhl z0SzZrpia|~@2l1lR5mYC_9)xZ;XpCAiE9dkF=?oR>W&{Gy5^iN7B>!fsyqN zR7_dU{1ulleVx(De~WVE?JtZfete4|O&Cb9bHp>Sk$E_?+rewF(x^P`=Wlpbw=^H5 zI6t)S!gU98@AJkT5m1R0>n_G)2-N5 zi>(RN$JsVa<-m*N=|v_v$gkl@2`uJRl!~y(I;}0LThZGB8b>mhRpeV5@_Z}G_NOeR zT5CrjyV$9sp2UuGy_Loz$=ph9aG7sL>aBbCswg}pwQfdIl0ZA5wv#1JqUFN@ zAD^chwyzI{ZQD(K@Ns;E+LIqIhCgjPyqQ#o+2QG^SDTPkT`Iicyl(CdL+!l=_~{Ae zZRjWJ57klUfs{}J#3wEvDn89u#qn3@+dk&ArZMJ!Ioa~gZ4i;#RnG?r+IzL3K~V;7 z*pVpAQTkC)7&;WG^D*rxcoEVFTPCtBGR@&Z8BVytj+H9QBO-l^Uj(qtmZGYIX z0eSUgrXCEAIiW{%i|()ff?c8IuUo-*xE)66uWgXA9@A>8KB^G7if!e-cyn(e$}{aP z#l{2k<)Dj7$?Ufn5;K%vPa#us2bN1kXh}PYHpRu6X9_cx5FIi1M4F+}DmH>v{uGb{tHc;_O;rlz;E) zD!4cXrC!3GukRHY%_U!g*GwC&tw@WVw^|24U)|nk6}!HKUD+n1Dhsgwt4(1Hz?|JGzaiB>!HNpHS!`l_;wU ziSkBnAhGOm?r;qWQ@FXh_bH?El``gX!llb=kbs^pv6a+8eJ z$Qye~e}MMe-%m-UPI;$8KG^4mfn9Pz^(t7*1T7p7K|-b#mZ9)C1f}&7Pktq@6^bDcgin`^0UhBKVXjFp(7446GZtjWhE4J$pcPUrqr<= zYiO!Q-s?h6z!T`sUs~@0o9%u7Lr1FpKLvB&KC$iZ^g1NL5xdq~H{nQus#FVe zSF7?Js?zmKUWgXQiCyQGg}{)!rSe5QK3t!aCc{OXhog&2eo`*_3C1AIPVd6kC(Rv~ zVm#4P{F;t;6XV4WIK6UUP-$Yy5(DoJ&`hN<2Lb9K6g+#Wk>@Y{fXm^+;I_kea2y`I zDaC(;{v9c7veEZAvzkaAQl17J9jtvTuicU4^@&+4R);|rX6Fl9O_!#L#;&X4gH?ka(ESr) zhexd>E7k8vlH^e0br6a2GpfpF=Tp`49;b}cfJ1&9`#+pv=_riNsjPM=O;H*jG)1p@ zjyp{nKp3f4(OC6ddBw1?b{y03KTbk(-X-rZJBY>4w0*-#&GwYV2K)5$IX7B-ash>D zuJe~T3=VUh5kkn%lHXh-cI=myDQo$Q+T`3cr}=zfnA0qngbtyVbkJ!U2-9WSGn}r! zn6SFj?Bkua(OQn-l2hB>s(a9X0D~yg#G1t-Q&gZ)ZJOjWE(kQ?&73l~AAB#bgg%(WvgPc;?tkNW+RJCB;N?MK>wsFdhTEX&^Ww%!G61{0vD7?pC zrph!+FG!c^^=vK^TH`HYjm}kOYfQ6fWNoyPZD?@4@iYck+io)6=vDg@>`$y?c(EYS z`gfhRnD6|Tie2|vH__X-w!=|wY10o{Nh)lhJL=Iz+%7Nqxm|K$mk8Xlwt05)vNHmM zWlM(2aABisGq<<7gHG7|WlF)D;8S)`nKno#eFfJQ{{gHhKb5jfLk&?rgg%p20{^y- zKZ)Z92!B~R{sX+T#m1fR=hJd3s<&JDiVC_v^7^}|>AgMBtuMK%Py;vva}Jl=aTtoZcoXr_e%e-zea;Zo8w!6`B!D;TAE~DGrF(1gT1u4fvbue9`)mi9}pO$yN zccA@9v3cDoAd(*yyBw#86?6uB`5m2%o@^R7*tfpg1k z>Qp?5rebBTN@CGSC}Uq`W)zzf#W6JzxxT2B}nB=o9|Moc}8 zlyZLHG7?5TyAg?vNN7(=Ba9wo04ESm-6zv4Nw@N|0vgAOX6&=KHZa!Z9#6iI&Th*0 z)S|^Gl6jGl%v-OlQ}J`v*6|cc9l@xTE`aAHd#5=bfG5df6k4xm|tKfRhGM~8~B4PJ{ zQ&4`Z?0nQZZy=T@|8v@rdD5dotT469G4D|9MMUQH&!US`yPW0c&cfhGOmkgu?iom! zJIFsNzd8bwcAD~O+>nU(0lCMNgRG%{90t@|^il$Tu^vF)m$H2DJ&mv(gko3CQ~WrA zyGrmoQztNbMiJ}INP0*~99-p99^R{OQqFJh8Pgy>v z)woZsu@_yYHrsjt_Dfmb$LST%Jl40U<=b!wHLqXAGsK}VWjO>2l6|&L(Y4gW+eG8b z>46xWa#aL;e@%@dFOb0Y;*{k!jW-=#%-gD#&)b!lDAz>hJ+)?BcFnKn{XOBVA< zQ>`ZUbI1*94a#dlNsM`iwYKuwjVkW98zQcE)smzNKtgRoU`VG7ZUJdw5r?bQ~NQMo$zy=O(=e)04CPI|fe zttXO6l#Q4{^i~3Fn`rvcdv%mFiMT#aCHBDvxlVTTR(rP9%&)mhQ>lz&k_s+<1Gy?$ zDBeuI41u~GNe;=F&dw7@VfLuysO}$HCPqi;R_YLDe5%c-U7DRKHzILbVtZEi}JGRs{}etAFQSv z^-HHIg{pf^zXgV5+G~0WDRL}(P0wm6{cZ>t7CcQg_Zkh<@(^Aw_=ZG`T}DAi!VMQl zIuaFIv3M%AFc*~lD0fN5>N%Pl{7$6Ow3{;=F%k6nqnT^zP260$P}oKny&%n3p@w4O zixzF3I!)KW3S=+bA?5ZmP#)(~0-dJE5@Zh=1FDUFij-=j|JG8v+GL~qfy0EgiJe6T zWN-K)+WOL%f{vx!7&tzku|)CjBtm(899)O@+ZYF=m|athWK^a@HtJp)2scxQr458q z%FCuU0-dHe(L;MphY+iZ9Yl&0<2Tn<7t%c>H%sJVXDI^9(Gyqafi+&SU|v%rc3BjW;M-DE?x%VJs1Z{=eC|!Y1{L|c{%YBn6g!QN z_TxBGP`LtHowDl_89ZaySD#-kbk=T#sCW6Au=CEaljvF3uhdk-Qo*k=-c1}JsD6#HlFk?9RF{SmSBR_DG`4D3x9Da! zRO8nez2MfLg?}49vVy5866}(ivW&+fkLU7uI*-rbv7N^YczoYCiT*YoZ{qPf9!orK zuw1&N00_KFH(UJl@LVdwBdk#6!bzp_VeKj0G|l$XFm_fs6$*7RXp2V}XnXG8V{KAY*}y1u_=MSRiA8j0HX|78rVe zl#mD4xK%{TSn$En9||FhaJ8ND(V$XFm_ zfs6$*7RXp2V}XnXG8V{KAY*}y1u_=MSb!`*j}kJUPuc=oPG)v`T9f%?EReClhg)Ea zB{N^rt+XlQKhn*SNoOpOu|UQG84G+u7N}md@Upq_;I)BBqsJeWVprJ8uO!xssxMpE zFjrdZ3B)1+{}r+71)hr+RaH9|IT$bJ^%~Yn0Z%0655%r;RoB+KsytPToYjs+oMLPN z-M6aidsKt>XmFnf*$;-V|4SsVZ

@1o6|vF~Jv!#S&2|7zyj#E4^_^kRn37*&7Oh zLRuRQ2tIEj9t^J(LXmh}h%^d*ujJJ$Xi(+b9B7Wj){eoJxgtJqNLcNS1-%WSfRKm> z{6a&ZF%k<1!EjItdPBi$z3GAsMP3R^a-aI*$` zHCTMRir3E&1k;16ht{6d6c`fuEZ*2z!WwUUNPwBT1hpeb&k}-hC`eZ0x~>UIO_7A8 zp$1hB_y;-YoQ@6Ec0!2&^+VMXHy0-yk%VY05)H&cYlXOXHEiqk#i+BqLSrKABfFv# zL!p_1P}>B-@IMhx0I?Pnu|PZ#l87IXki==V`~hFc8v{d9K!}8CFsMYnNO(2B8DbLWrZYLVlsy8(#&gfDn%)Vm@wj%JC+-(vm5z7e-=2pvBu94F%>3LP6emUy~OTPN)z{gThq7*0ONn+4BnWDx19Fm1J|yH{z%1 zpv$!uh7E-T>Rm4k=flKAbJw8Xg3?;*FUqRDM;s!U$j$IPehg24;A7fTxAL(0JZ#7h z?$g(GXi!r}z_tvMi6HU;31s8$UfF_QAUbYgc*ql0_3!pF}$Ixq?QogS;h}DQVa|hu;s;;&U-GbsB=7 zkUr@wd3s75|Dp)j`Vmpk{%Mqsp@j!_`REG?{YWwWJTd(+G5s7d{S-0%3^Dx#G5!27 z{q!*X>@fZ0FnyL`OfSaxUxbla4azEw#tAAvMr25nYb5YuW&|-ln=v|Pur<8Z8JJ%* zs{+7Kyb{)R9;$FsPkD^>0Sb14A5WY09wyI@0ghKxE zL?E_ynHOuLisGUIUx{r?0E9fhgk8oKLdWII0e!1rmF19i4Yv@rvj{mqWF~6`p$GYg zI6@=juY;9HlLX`n!IlzIgkcuADgSB^B;h`gJ(;0GrkYB|9zHjHAZ zhg9^)KGcbihIcy`key(bAeP0zxkOJbxOb}ipaBi)e8Qz`eUzpjHN;PpvRm=Xthauf zFpTMC`A8QbIPXLF4wVdj$_+#5PckJYRe4A898mFs21k|uo8%eQ&W9ihHTi5Bv}>?* zg#AB+O{O?wfs6${B^Fq`#JQw$$;)N#OCEOq;!meMGV$wQ-${P6a&FyHY;xjt?#eo6 z#FuEs*&<%&NQAucx^unZNZqV>pg9niVu8BF)pd;_uN3lz{eub5>fp6?4T)fA1gf&e zyDC8Yym%c?F7d^@lCP<*Hnz4h(i}y>jOM`k4dvlFUvq7x$5wv99N)~Szkyq8GGYq! z5qO-5G8Xvg7I^!FqpZUpomHkr#sZ&t3(P$+GhaUQo%}B-XqcGACZ3oVHQ;Xy8L?Ck zgA26$(V$Mh?&x&miSJIvCc4$<+(oA_R$B~#@#E+mB5AqZuTjLFEybc?* z{5lOHKc33(7~=Uu0p~?*F6`h*kUqnBQr%nw%UzHL@h3&{DUUyenzu72YO3eenBf!y zJEZ}5lAFqp7wR%kG_Z+!eS)N0EK9ZTNPaq93FyXWv+;%^mKD|HNbisIdimKb->`$3 zbUJF6_~aQ`-kcnkH#>)yjLH)oZ%4DO&R>^%EJ%D$GO&}V{jng*C!J|x1#>!Cf#C*5 zEx%ZZ{hd+OVUl$mJJBz*6X$%Dorv-YBg@asLA{GuPOmYVU7zKqy1fRq?qvLBuG~2n zGQq~!R}kv9YIuN=I73k`EAr>DqT2DSXb#eg3!_AB(E4JM)`wHwzyV)-K4VuSY{GaU zNS|TjtP3F|A zxXGvIveW-;WEQD_71ZW4)0{l!E@SL^gwkyq4Zuj;B>V}VoNs2?h`(JWQ{^d9JJoP9Gxru8n2;PF&8;72%Wc5;>=eU{Gu!vbuPJHXL zG&G%1UXGsv9YuJtJ&2e6Bw*uaVxXmaRt>jVQkp#eC}-h3WY zp2pToBOvN#J{b#SEb!^J0RCE4iu)RfITDiOgv&xWM?gd$`tuxVpC_&q4mRTNu+F~Y zg&DU@ODoWlR6q0i^je@5584kS^!|b|n}x9m!T{2(fImV|kj5QP)^IY`v|nO91l){Z zLmGFb*`o;MNFM-v86kx90N~fK2ec#2CNX9~I08Nb;7kMtydChr5Co(*0saUf7wIj4 zPaqT{-3#~*LLt)ZbBvvYAQ2w$u2UhyeCJ#yr{g>Kpb-FT5E$_FfY&1& z0e%x;5}_FQ1Awm}2uKeAE?9AM-wNAO8Zc)H z{um6>g@98K+(@?qK7z0U>0Ut7RPaXH4fqnSlWd}NDPy-IG$EY?d=jA*=>fp+oXuDZ z;Q@b#uz~P^U;P4XiS!o0HxceZx)5`FF+w-et$?>7>?0b22nUcp0Qee$f;5{Bbr1%S zwgaw5ID+(MzzH)TFVZ%^2tw{w@C1AWp%7`j=wgis#e@g!Ltsc3p2OJ12!~KN3V0X7 z0PsmbqYeC#PTxDyzH-GZ=mGL5o!fw=U1*|GZo0JCpGs2xnvkTBi2oED|1KfkR$r9>rsbK842pf>@1)Opb zWB)+96>#PP*p~2sFINI0ZF6G$UyS!-NDqjNeW4mQ>4yA(<(I%7NN)ms06{=H3HS~| zEz*UTVhkY6M_O>5G!4LmcmEds=KA^#yHQ1uTTt%fT(O|uX|98t@tJ%3;gCPyJXz(Tt-l@S~YS5;^ zVh#SIPnEP+gHLL3hXx5&}!LSDFH0aXcg_<7!ZhIdu|I$~6DCa^(_lsCp({M@1I{@6Fmxt*7LUW^cD4(9n z%5YA4ttS+WON^b0a$h1wmzh0*7GFTUM1R^?SPyPWOMw=NjUTfuPdK77z_!}sS?!BS zo{IyLdT*27_S4nP zzOiO6?jOfstF4SwYz(f%O;S(Ti#xb$Jl@#K)r?(ZfbH~)zj{$1ygC?*gy~iU&Rho1 zO7PdH#v+ka9uTORC5Fjl|fV_9#ds)Y{vCxhFgptPt@ZfNqmf5GL(Z{ zcS2&d?hlbtYj4f+!~!ePxzJo{iUquW55L+>X7J$Z99rj6~m&KpSZiX(jr7R`v?%9PlOR3nS^3 zc0w+UAXQ%0qvt%=B!a$Gs-o;t6B#oY1`(TX!l?E|*LvbOqw};^qI47Wv*Zs(peBS5 zdsfGB#XT%FqVU#i&zff3MTuen5N$W|@hC2(YY4w-jW4e8OhB3jFD~Ho5xCgMFKN01 zF?{o**&Fr+D)Ge*NW&&)VbE8q6UrTn_;3}s+Rvt>VQYDx0Lk7W?2@(Usb;#$?1Fq`kf;hvv9&neQOUAk zOu{|;rQtQfFk0-$u8unc4T+U212H$*5bT6G8dlxI!G#ok~X_gr^!xCtJb{?^Sl2sa79-8gIrfty;XO%2hU-50a1Oz%$et z3w(wxP*gbl{>aGu^&`r^^db4ru&Mq(Pz~>Vp(R4=aAgh500bVVj>6+_*)tJkQGU7R%CG11mw8FU1mZiM9vF$*rt}*F()X_JuKgMNrEF6AN6EKq~>i zI~H7x&C<$1e6TRRxQcHnp|&fqIuH^<6jzjZ>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH DEC(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) && __WATCOMC__ < 1200 +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__WATCOMC__) +# define COMPILER_ID "OpenWatcom" + /* __WATCOMC__ = VVRP + 1100 */ +# define COMPILER_VERSION_MAJOR DEC((__WATCOMC__ - 1100) / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__SUNPRO_C) +# define COMPILER_ID "SunPro" +# if __SUNPRO_C >= 0x5100 + /* __SUNPRO_C = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# else + /* __SUNPRO_CC = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# endif + +#elif defined(__HP_cc) +# define COMPILER_ID "HP" + /* __HP_cc = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_cc % 100) + +#elif defined(__DECC) +# define COMPILER_ID "Compaq" + /* __DECC_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECC_VER % 10000) + +#elif defined(__IBMC__) && defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) + +#elif defined(__open_xl__) && defined(__clang__) +# define COMPILER_ID "IBMClang" +# define COMPILER_VERSION_MAJOR DEC(__open_xl_version__) +# define COMPILER_VERSION_MINOR DEC(__open_xl_release__) +# define COMPILER_VERSION_PATCH DEC(__open_xl_modification__) +# define COMPILER_VERSION_TWEAK DEC(__open_xl_ptf_fix_level__) + + +#elif defined(__ibmxl__) && defined(__clang__) +# define COMPILER_ID "XLClang" +# define COMPILER_VERSION_MAJOR DEC(__ibmxl_version__) +# define COMPILER_VERSION_MINOR DEC(__ibmxl_release__) +# define COMPILER_VERSION_PATCH DEC(__ibmxl_modification__) +# define COMPILER_VERSION_TWEAK DEC(__ibmxl_ptf_fix_level__) + + +#elif defined(__IBMC__) && !defined(__COMPILER_VER__) && __IBMC__ >= 800 +# define COMPILER_ID "XL" + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) + +#elif defined(__IBMC__) && !defined(__COMPILER_VER__) && __IBMC__ < 800 +# define COMPILER_ID "VisualAge" + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) + +#elif defined(__NVCOMPILER) +# define COMPILER_ID "NVHPC" +# define COMPILER_VERSION_MAJOR DEC(__NVCOMPILER_MAJOR__) +# define COMPILER_VERSION_MINOR DEC(__NVCOMPILER_MINOR__) +# if defined(__NVCOMPILER_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__NVCOMPILER_PATCHLEVEL__) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE_MAJOR) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__CLANG_FUJITSU) +# define COMPILER_ID "FujitsuClang" +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# define COMPILER_VERSION_INTERNAL_STR __clang_version__ + + +#elif defined(__FUJITSU) +# define COMPILER_ID "Fujitsu" +# if defined(__FCC_version__) +# define COMPILER_VERSION __FCC_version__ +# elif defined(__FCC_major__) +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# endif +# if defined(__fcc_version) +# define COMPILER_VERSION_INTERNAL DEC(__fcc_version) +# elif defined(__FCC_VERSION) +# define COMPILER_VERSION_INTERNAL DEC(__FCC_VERSION) +# endif + + +#elif defined(__ghs__) +# define COMPILER_ID "GHS" +/* __GHS_VERSION_NUMBER = VVVVRP */ +# ifdef __GHS_VERSION_NUMBER +# define COMPILER_VERSION_MAJOR DEC(__GHS_VERSION_NUMBER / 100) +# define COMPILER_VERSION_MINOR DEC(__GHS_VERSION_NUMBER / 10 % 10) +# define COMPILER_VERSION_PATCH DEC(__GHS_VERSION_NUMBER % 10) +# endif + +#elif defined(__TASKING__) +# define COMPILER_ID "Tasking" + # define COMPILER_VERSION_MAJOR DEC(__VERSION__/1000) + # define COMPILER_VERSION_MINOR DEC(__VERSION__ % 100) +# define COMPILER_VERSION_INTERNAL DEC(__VERSION__) + +#elif defined(__TINYC__) +# define COMPILER_ID "TinyCC" + +#elif defined(__BCC__) +# define COMPILER_ID "Bruce" + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__ARMCC_VERSION) && !defined(__clang__) +# define COMPILER_ID "ARMCC" +#if __ARMCC_VERSION >= 1000000 + /* __ARMCC_VERSION = VRRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#else + /* __ARMCC_VERSION = VRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/100000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 10) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#endif + + +#elif defined(__clang__) && defined(__apple_build_version__) +# define COMPILER_ID "AppleClang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# define COMPILER_VERSION_TWEAK DEC(__apple_build_version__) + +#elif defined(__clang__) && defined(__ARMCOMPILER_VERSION) +# define COMPILER_ID "ARMClang" + # define COMPILER_VERSION_MAJOR DEC(__ARMCOMPILER_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCOMPILER_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCOMPILER_VERSION % 10000) +# define COMPILER_VERSION_INTERNAL DEC(__ARMCOMPILER_VERSION) + +#elif defined(__clang__) +# define COMPILER_ID "Clang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif + +#elif defined(__LCC__) && (defined(__GNUC__) || defined(__GNUG__) || defined(__MCST__)) +# define COMPILER_ID "LCC" +# define COMPILER_VERSION_MAJOR DEC(1) +# if defined(__LCC__) +# define COMPILER_VERSION_MINOR DEC(__LCC__- 100) +# endif +# if defined(__LCC_MINOR__) +# define COMPILER_VERSION_PATCH DEC(__LCC_MINOR__) +# endif +# if defined(__GNUC__) && defined(__GNUC_MINOR__) +# define SIMULATE_ID "GNU" +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +# if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif +# endif + +#elif defined(__GNUC__) +# define COMPILER_ID "GNU" +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# if defined(__GNUC_MINOR__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +#elif defined(_ADI_COMPILER) +# define COMPILER_ID "ADSP" +#if defined(__VERSIONNUM__) + /* __VERSIONNUM__ = 0xVVRRPPTT */ +# define COMPILER_VERSION_MAJOR DEC(__VERSIONNUM__ >> 24 & 0xFF) +# define COMPILER_VERSION_MINOR DEC(__VERSIONNUM__ >> 16 & 0xFF) +# define COMPILER_VERSION_PATCH DEC(__VERSIONNUM__ >> 8 & 0xFF) +# define COMPILER_VERSION_TWEAK DEC(__VERSIONNUM__ & 0xFF) +#endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" +# if defined(__VER__) && defined(__ICCARM__) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 1000000) +# define COMPILER_VERSION_MINOR DEC(((__VER__) / 1000) % 1000) +# define COMPILER_VERSION_PATCH DEC((__VER__) % 1000) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# elif defined(__VER__) && (defined(__ICCAVR__) || defined(__ICCRX__) || defined(__ICCRH850__) || defined(__ICCRL78__) || defined(__ICC430__) || defined(__ICCRISCV__) || defined(__ICCV850__) || defined(__ICC8051__) || defined(__ICCSTM8__)) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 100) +# define COMPILER_VERSION_MINOR DEC((__VER__) - (((__VER__) / 100)*100)) +# define COMPILER_VERSION_PATCH DEC(__SUBVERSION__) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# endif + +#elif defined(__SDCC_VERSION_MAJOR) || defined(SDCC) +# define COMPILER_ID "SDCC" +# if defined(__SDCC_VERSION_MAJOR) +# define COMPILER_VERSION_MAJOR DEC(__SDCC_VERSION_MAJOR) +# define COMPILER_VERSION_MINOR DEC(__SDCC_VERSION_MINOR) +# define COMPILER_VERSION_PATCH DEC(__SDCC_VERSION_PATCH) +# else + /* SDCC = VRP */ +# define COMPILER_VERSION_MAJOR DEC(SDCC/100) +# define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10) +# define COMPILER_VERSION_PATCH DEC(SDCC % 10) +# endif + + +/* These compilers are either not known or too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; +#ifdef SIMULATE_ID +char const* info_simulate = "INFO" ":" "simulate[" SIMULATE_ID "]"; +#endif + +#ifdef __QNXNTO__ +char const* qnxnto = "INFO" ":" "qnxnto[]"; +#endif + +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) +char const *info_cray = "INFO" ":" "compiler_wrapper[CrayPrgEnv]"; +#endif + +#define STRINGIFY_HELPER(X) #X +#define STRINGIFY(X) STRINGIFY_HELPER(X) + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__MSYS__) +# define PLATFORM_ID "MSYS" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#elif defined(__WATCOMC__) +# if defined(__LINUX__) +# define PLATFORM_ID "Linux" + +# elif defined(__DOS__) +# define PLATFORM_ID "DOS" + +# elif defined(__OS2__) +# define PLATFORM_ID "OS2" + +# elif defined(__WINDOWS__) +# define PLATFORM_ID "Windows3x" + +# elif defined(__VXWORKS__) +# define PLATFORM_ID "VxWorks" + +# else /* unknown platform */ +# define PLATFORM_ID +# endif + +#elif defined(__INTEGRITY) +# if defined(INT_178B) +# define PLATFORM_ID "Integrity178" + +# else /* regular Integrity */ +# define PLATFORM_ID "Integrity" +# endif + +# elif defined(_ADI_COMPILER) +# define PLATFORM_ID "ADSP" + +#else /* unknown platform */ +# define PLATFORM_ID + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_ARM64EC) +# define ARCHITECTURE_ID "ARM64EC" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM64) +# define ARCHITECTURE_ID "ARM64" + +# elif defined(_M_ARM) +# if _M_ARM == 4 +# define ARCHITECTURE_ID "ARMV4I" +# elif _M_ARM == 5 +# define ARCHITECTURE_ID "ARMV5I" +# else +# define ARCHITECTURE_ID "ARMV" STRINGIFY(_M_ARM) +# endif + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__WATCOMC__) +# if defined(_M_I86) +# define ARCHITECTURE_ID "I86" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# if defined(__ICCARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__ICCRX__) +# define ARCHITECTURE_ID "RX" + +# elif defined(__ICCRH850__) +# define ARCHITECTURE_ID "RH850" + +# elif defined(__ICCRL78__) +# define ARCHITECTURE_ID "RL78" + +# elif defined(__ICCRISCV__) +# define ARCHITECTURE_ID "RISCV" + +# elif defined(__ICCAVR__) +# define ARCHITECTURE_ID "AVR" + +# elif defined(__ICC430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__ICCV850__) +# define ARCHITECTURE_ID "V850" + +# elif defined(__ICC8051__) +# define ARCHITECTURE_ID "8051" + +# elif defined(__ICCSTM8__) +# define ARCHITECTURE_ID "STM8" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__ghs__) +# if defined(__PPC64__) +# define ARCHITECTURE_ID "PPC64" + +# elif defined(__ppc__) +# define ARCHITECTURE_ID "PPC" + +# elif defined(__ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__x86_64__) +# define ARCHITECTURE_ID "x64" + +# elif defined(__i386__) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__TI_COMPILER_VERSION__) +# if defined(__TI_ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__MSP430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__TMS320C28XX__) +# define ARCHITECTURE_ID "TMS320C28x" + +# elif defined(__TMS320C6X__) || defined(_TMS320C6X) +# define ARCHITECTURE_ID "TMS320C6x" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +# elif defined(__ADSPSHARC__) +# define ARCHITECTURE_ID "SHARC" + +# elif defined(__ADSPBLACKFIN__) +# define ARCHITECTURE_ID "Blackfin" + +#elif defined(__TASKING__) + +# if defined(__CTC__) || defined(__CPTC__) +# define ARCHITECTURE_ID "TriCore" + +# elif defined(__CMCS__) +# define ARCHITECTURE_ID "MCS" + +# elif defined(__CARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__CARC__) +# define ARCHITECTURE_ID "ARC" + +# elif defined(__C51__) +# define ARCHITECTURE_ID "8051" + +# elif defined(__CPCP__) +# define ARCHITECTURE_ID "PCP" + +# else +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number. */ +#ifdef COMPILER_VERSION +char const* info_version = "INFO" ":" "compiler_version[" COMPILER_VERSION "]"; + +/* Construct a string literal encoding the version number components. */ +#elif defined(COMPILER_VERSION_MAJOR) +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct a string literal encoding the internal version number. */ +#ifdef COMPILER_VERSION_INTERNAL +char const info_version_internal[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','_', + 'i','n','t','e','r','n','a','l','[', + COMPILER_VERSION_INTERNAL,']','\0'}; +#elif defined(COMPILER_VERSION_INTERNAL_STR) +char const* info_version_internal = "INFO" ":" "compiler_version_internal[" COMPILER_VERSION_INTERNAL_STR "]"; +#endif + +/* Construct a string literal encoding the version number components. */ +#ifdef SIMULATE_VERSION_MAJOR +char const info_simulate_version[] = { + 'I', 'N', 'F', 'O', ':', + 's','i','m','u','l','a','t','e','_','v','e','r','s','i','o','n','[', + SIMULATE_VERSION_MAJOR, +# ifdef SIMULATE_VERSION_MINOR + '.', SIMULATE_VERSION_MINOR, +# ifdef SIMULATE_VERSION_PATCH + '.', SIMULATE_VERSION_PATCH, +# ifdef SIMULATE_VERSION_TWEAK + '.', SIMULATE_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +#if !defined(__STDC__) && !defined(__clang__) +# if defined(_MSC_VER) || defined(__ibmxl__) || defined(__IBMC__) +# define C_VERSION "90" +# else +# define C_VERSION +# endif +#elif __STDC_VERSION__ > 201710L +# define C_VERSION "23" +#elif __STDC_VERSION__ >= 201710L +# define C_VERSION "17" +#elif __STDC_VERSION__ >= 201000L +# define C_VERSION "11" +#elif __STDC_VERSION__ >= 199901L +# define C_VERSION "99" +#else +# define C_VERSION "90" +#endif +const char* info_language_standard_default = + "INFO" ":" "standard_default[" C_VERSION "]"; + +const char* info_language_extensions_default = "INFO" ":" "extensions_default[" +#if (defined(__clang__) || defined(__GNUC__) || defined(__xlC__) || \ + defined(__TI_COMPILER_VERSION__)) && \ + !defined(__STRICT_ANSI__) + "ON" +#else + "OFF" +#endif +"]"; + +/*--------------------------------------------------------------------------*/ + +#ifdef ID_VOID_MAIN +void main() {} +#else +# if defined(__CLASSIC_C__) +int main(argc, argv) int argc; char *argv[]; +# else +int main(int argc, char* argv[]) +# endif +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; + require += info_arch[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif +#ifdef COMPILER_VERSION_INTERNAL + require += info_version_internal[argc]; +#endif +#ifdef SIMULATE_ID + require += info_simulate[argc]; +#endif +#ifdef SIMULATE_VERSION_MAJOR + require += info_simulate_version[argc]; +#endif +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) + require += info_cray[argc]; +#endif + require += info_language_standard_default[argc]; + require += info_language_extensions_default[argc]; + (void)argv; + return require; +} +#endif diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdC/CMakeCCompilerId.exe b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdC/CMakeCCompilerId.exe new file mode 100644 index 0000000000000000000000000000000000000000..279d146cf4a8da57c37bd8e5cb944a0194dd5740 GIT binary patch literal 96256 zcmeF4eSB2K_3(F-Y_fzbHz*51L{8zg zG@I+Tv}$YnYpZ?N*0#2_2>6x|APFz>qE!$T)T(z~U!qh`KpV^KYJWmdjOG zngN}Bw&9FFzwyc4*=P02)bigGU%m3f<&UZN2g^&<`{U(*QTaZ;yO($Ru@9Cvt9NSo zbL#!!^562l{jt>YC)E2R^}gcUvu_dD*`Cx^+iW-An{S&}cE{{&TDNViZE((z(YCdu ztg})}j^|yh!hXBFB%EWj<*TIrx2=`0f;|4^+Ugpzg)(JHwSZ(_k}s1YLEB?l6iv0B zwZO`#`bb}GL&#V6rF{OIt_TKZ1Z=i1_-uQW@6wmsznt%^g>&Z3XCwtjHgIKg@|OAs z{PWvv7Y&I1|KI=&=d_6?-ka<;?fE>rQB<1SX|%=j zO0!?y``Rxbe$AJJYvB&t*Ff9$HDwIfn_v23(1_T&l6L=yc=Hsi;3H#fwx-tT6)JAd zA+f17Qbf)if@YOe;gQfN`SURasmyyC;d~N+>@aTwdFIOk@+S}@u<~iC=eqXQRAn>I zBirW9n`w2KnIe<7m9J&(D&9_h^JfaCBqlEduH=nh&$z@RrNvj1t-|W;Kj?B}tJXM) zxBuMz{^m%j$NVOdmYv}17P~FhYu6gbkiV~NnP<>gI#{?0 zo^MO;vNBAwGE_*0Ka8{4IvjCI%gjqT{VVONu9i*aqokOZ(OliwsmCg9k&|?g@fo#% zhvTZft${>sj<-uUa?|e3Tgh8KAn!2AyTQuq)Q!RBJ*SZ4t+4{F$2{{?d6$@@d54V- z-FV4xnzIh4(+p?GLi9|3`4n`nyU!Hu4-AFQ0Vd z`cl7cL`rp#TX9onJpRC{x565lpA$4TnG=Dyg7>?WTA{|}IzX!dS$HI!&Wzn7j3$UX zd5&)6jjs^A8M-s&(0!eeVcynmUHhA>?pt1<3^s23NOy^@$*E2FC zTCly@;UjMTo`#z@3=#xJ^UiHe4Kp7*Lq!I8yUg22Pkqn=sR%Xo!K$BPqo~c;75x@N zzT`Wm!)jSs)9z>~bo;?v0Iz2XK#9e~Ka==BRXlPQb$I7$^BtoZ9zE8X-lyH|SWN~! z?pVu9qVguXiPr|lL;STrX}6lNn%3IPxgba3q$F&#t*W`!W}92(_;V_qjxN<_yY>=* zH%YvhGH2S~ZwLbE^U+(HY_Lwc68xVS3{75SyQE1Qdhn8LD zCp6Z(gAp6txI0>)-{)S-ifi@58k71dPm^M6*%ira+z~A@FDfUp$vglyvmNfkxgMMH zBQmFTuwG!E$TUr9&?wlwi)8a|5~MC&3%;ZqYqJBUXz7_Wm^iR=RO^AA=}x|swX6B< znW-BelEI@J4*zwu?3r_az30rI(gxkv9*yX+IVDWpnv%5pI~#@8s!Q~Eb#dA~gP75( zpCe(v6;9N6tazEkt34De*4y@abT46Vb+PVk(^u09{sCzm5pRE0G`Wq@e(qYDzr9|0KP=?=3v< zeX7-C&QkL|E0AO1z}s5oD5Vzd!B)xtQPe8;hP-3rmjMPi!Ut#Si6=_^hXB&>VJl;5 z>{>m7=A_*}qi%*b&)Br(wAIXHo4Jk}ppisPcVN{vSdTe^v6N`qJ(0}1@e7k|dg7s- ziZ5+~W2N2W#>_lEBTt(tG=pCVWU%Lk*o;AO%e%HJ`gPvm=f@KyVC9tIt6r7 zr+iF=OKbIblT;%OQHg<74@<4FR#)>q8$U1I2D0>qb%L`S z+BYY?ij}E#NC4a{0O_-> zoi*biXU5$%h*aode~01?GH$YTd30l>ZuBVY`%8tF|ZS(?+ZYMi7EBGQpd6E zNQd&%?phhrSi4jA?OB*qfQ6QCvgpZbS7P*aR@oWHlpXe8$`;F-Tp|F9y{+CZ?bku0 zt=hXgXlx2Jbl)DCGcVBa>9jewM}x)=qubjRNKCa?5AM`{9ccLM+qcgNAxBjY-Ux2; zj9!Sxi3C;@t<__$Q?G`#jPJfMtxajox7#9RQ|j__Y>|_vERfd;jwgG+NXL3SOGp_ zEdsoKo8I=eM<2XRG=EDhu&_r{$1;_0S`gTU403em_oxC^din$KWwA=jg(Vy~?Q7wa9Slufa+~S*L zeH5PZ;CE^FL+})#K|MaNG&5)@6*B`@1!VC2x{<6gdP7EM$mj_hADHWyx;4hPoHfRI zA>*dXn)3hbug;qrFqY_A)9bWLz*Ok*yGkp4UME4{6$b?LI@k%nb9H8sjuV1CNn5s@n4 zT0zLX^9jY?hOChR(Ty!>_Z=h~;X}m%#g9X|n*n6L3`Rr7G=JDwZ_GGU95S{9jT&1h z_hn%fWqc`S|HQnk@$yb^%( z>W=eSnb_W&77MMFgEJBT}E(Ip6=^eIGO;HuV>LP ziWFA`Hx)+<2^A44iZ}!5wbj0D3pWRhu4>=*=mnAo7)2C_mR2LgoRJ!xxY+(Tdmuhd z4@x=uS(Cdiklsc^DV5&9yrZ%ATtzn&F2V^Uy^wKTMaa0dGGr_w{7&Gez>R?$0@p(s zjCq-PH-ns!vW_$Jtw@3S3j(PR)p~EqGE~pKyizwpW#;LiFy%>|wBJ@7 zfhz{ebi*l7VSRSp@UxyjyiWMmI$da4PrsdMH`8SQF%kgxxH*x045+!0!B&-@E(f87|0^%QIOen#oJA?^0IxmsOo zsG+Bx_>$t}t&-ede!nAmJt{)e@7@`L0*!nb01=EiHC&9#Nu?~D{V7x1qc zr6ob;Z*i!jO-L-|v<7?bifn=Gcq^JQ9M#BSh~KyGM+We%jf@+ht;CfW;?s*4=IcJ) zxwv4pn47koX3Y(>Tl)kD^8hOwAv$iwR*n6w#S``6_KWT37YNrrcA@ zViqlcy{w=_+I@U0oWpe+si=7u!+-SH#M5JJ zq`}|B^mFmVebDEWnCBMC6>QV$71fDl_;J6G|E_uB>%u=ZdrEVSSy|Fdz ze((i}Z?L2RR*g!!fk>eG2FvXQh*`q4LJzu(^{OWw84n(ZBcYMj7xnnyLb%r*;jVcKy8X$q{! zQdzJW{m~dbfv>623{WnC^d@ws|KaIOyI*=9EW6yK!aDc#X)ga|{qq>T;lz|fZ=};z*x3ctfajw+pYPw6jrQpngqd#6i#?`u?b>#_BDA>W6|Z>f=Q=j)jRzcN&V82hyQ z{pVzX`^slZ%LF=g8U(i98Q1p+Am zBwzrmC|h{>06>Kb&;U3i1rB6n1CGOjlP_?3#e8x!oJj-V{QcQuyiO`50{0VeU^C&W$_UR~Utqy(mw<=f|a-KacPEQZ5U307xlD z){Al|8*KLH^w+-yOOOS@)~u^XKL4l?-5B zyLX+?tI-vTy)`BjOL{`(U7_;rHMwnl8Gb{C3^?t+3o@hCKitjx!l8_YyABAMjh!b^#J1iqerdx&swebulLIP@ZIrx>;1z=t@lq`t@p3?@d z=IRY=35{l_ZPCimL_!f z{evnGk9IzO+n;w7R~uOlW?%uwHUvfiL6heem_Z;Ol+C*Zcn)e7&E+*ZUcKy`RC?`~Ua&dg?3jwe0;b<4e*!7DoE; zC27avOVW$N_d?6kxlnHm}r#B%*RfFL&z`rY9(Kt4vwtY3M(crp)e~ZFJezxF?k8SSuuGL+h)b&MQpDXQ?Ge;VJNotK6xAcBW&!@ z%eNzAYsJB#-MZi^W)|;n(;_3_)}l>l#eB;fstt8wYOX17&#D&lwbSgWVMmP*nnSU6 z+8_l=D-I9ct2ZPC38XL3P7Bq^hg$jIw{(tL68(}`A&DNcRgmJ5lwz&8XlV2=0#Wn? ztt_L`PYT%##@*-0!)1t`ul_KN& zbpwI6&CK}^6u!+Y=Y^=E8+_(VV3n-!=DYhy?brJLH}L*W5#a4Uy5!OIA5+WSN9Ps& z>gfDoX>Sk}MaNhmjjoXrhS(#wncr1mtL2$ypVbHOSh*Cq9+qvR*F1j|rU%(Q5;I$H&l|pHB!@1iIJ=|azR5SCZE>G*$tHs$lKFU`BViiA6hQ^kg!Cp}e(SCOd4Z*?Y*XUr{`wnAiDC@2DfSt`$F!|}^Y zF|%)uyMCfmx%u z>=Yn;$@WKj{EPn(*)nW6=BWH#0b|>0KP`{FePQ{=&`Vu4-p+8`)eM zqEGtkk9Z>a{2dhitpFLcVQSiz0?bx6U|R}gH}+*(G1(Kzn>>g;rgTS3LF!XUMs~H; zH0_TRgyJVj<3j1Y(lydzZ!24d>5g=3i&o!zedIs&yZ|H$OM&)$r}>e$ zznkffftZkvr#q5!A(RYs4EO>)IhG`8d|oJaicMNB(mmX19=k)35lYfsC`noU==%1o zZnTPW@^4HS^6+&on!*+c6E1Y1J520sz3r{s8tt!8NE@3>Az$0#6GORO3M=OG%zZWW z%`;Av+1Z}TfqF%Avgl9tfv&n*k%ZbctMI0S#-~kPTH}>qH4(r(ZeKjyyc#fxn})OP zf945j8;6n;`D#|34amgkHPbOVugW6@Er#vY*G`ep#M~Ne3-Qsfhk4OyhrjZIXs&MT zW5-3;X%rGvW>7 z?V>?5Xj<|+VCT|AUXR*31ibNLA9VA;7wL2$cErvGNwuF5kB`2FCdD1o`RnaUeOph= ztfYQs@+LEPJiY5i4a1IDQ)%{4lW3iMH9wS&A4y$ElgPSNreoFl;?LlB>ghJUsxtbY zxa$C@0NnyD?*5F3`Og6$9#`S(OY7K&XX9M7;G1q_3(2zesQnXfTCbY&T?$6>plIEE zac`&F_*gocnp|Ptxz88hG;t(%=}zJ_zn|1xHyK03tR9Wo^i! zxLp=MHkdrh3St`UatiOoF6UFoP>5Hy;$3h-*?;adkCf1x#NAHafXl%Zq~;GKtsjw! zf2+Naj6aEAH>Nxv5^7ItO%KvcL7HX!h^Tq(XQxc2pcz-R>1OGd6&tiCT|ZN zH*+hfX0{OF3mHtkXda-=76n8uI~Ig@N7yJ}Dn!^ml^qFlEz^?Hr^tlbWG&|ZYuYr(?4kspKVWyhMY1Zu3U0!62MOj~(x!wfD&hN8`>gb^ zRAsp4WC|rGtE#1~j%y?hpjCt?X2MecIfOIy{|qLe>JRnTUnp7Qj#2Wm`j=cpw*GPL z+v5^h%KMRS9MsD(h3o>41;*!k+b6lmF}gPGFe4s0p?{n7G!F*-Q2-I{tQD|EoSl@Ugj4tt zv;qcY9uEbsz#Zs#IK=?6wd`9|SU;t58a!KTdW4K>x92{`2vV2{h7(t(31Bl_C1zR? zTMT!ueDiIK9It!V>MwPw$s`-zK3d9vP2h8?2*;VIOYE@3%o_?+Q*!ZWiXM+y>Q`3I z@9{E!D#Mp>|I$kOfh66lMtG`IvJctd_}Rtmnd-*W;tjmiie*+{!Ck@lcv~BKWur*> zILLlv5W+7(-Y?ELf7BYg*glJ1BJF$WG1|x2PwCr`_a40=U)sMRKQg-^U!2{LFJOB7 z3Iwrgm29XkgJ7!nvbMZT+(Xz5C#v@nX6jXUvy$vo+5~P4+v3$P$E$amGwD=pna36x zEMrhdM`~|q2PT-e_1P><0}jth8!1!U3Uz!lRy@n!whx^9*V>~))9yb@FN{v(rL_C+ z1c5MEa$$@8AEMwSGp>rigmG`EgrpMgd)LV9PrL6W6F3+nszhZbJ?$p5&Z5Vq z2KfwU#Qc~m9~It3PIXixr>+rI$=el7T$D~W(lPH26|pCm5TOmwgm|hYZ;_FNk07T$ zcb-tx_JGl9$*FrUEbk0gIaUUln#UmwhrHWZD*r%K%|WvUnh`kO9m%l@sK7}y*jm%+ z$I}<$3w2yWudS8++`{`@Ga)j#i`!gv9Ikb?;WW-n4275_q`=8srhMA{{XdWrcg&}r zv^#D^zC)yM3qgErsz?x@8S8EBbHm&FCK1z64POjrzyfqd_yMe>-RY zYooeihtiQr$hlRqL-vR#b|@#}Htz*Z8H`<5tX8Vj=s;}m;S~GNojJj{0+A|+x$d6= z#u5Q>F0y1s!*PK$QQ?)@zSwP^-w#X6z;fQ_Dj}B`8q&HsmYl4()dik5H z#HmwnJH(2xQ#TH=DD((}$R!l)(S3&&o)k<}v-rL$GVUN`7`8c#Zu2ML3ymO(99yXtRwTMJnV z=&M+lqeaTOWB+`M*F=CgNJ^*Lp0SmW`{e=AVZsSbwo# zfroIM#Vub20-!7t!v8758v2w%CZ5t1L{Bibv$k%$&~8i4p)1${UJu2mV^R4K)-cr> zicd9R7bR*9f@hfU3lIFF7JgC8dzSe$@(ok6Qed&f=P?KvAK2ZmvcruHhbGOVVR4!i z56~Fqxh$({(>$}b`dRKskfpB%x~*nTRkIIOZDy9fnXHF3o_KW`E2UXzRp`G#`06>b z7Q`KYXDJcS#*D1V9x)~7Dj09{=6?CE5D4MeU1#vCi9&_nKYZs zvoEsVgPA!Jf8=BO%KO6JOj(P6dApLgy}@dgh2v_ccJR=xwb{j##S-ayk|Y>>Kx*)9 zV$=B()J+t}he^+F1WUWf`Yuw6xlKMBuD1y5vGLn^+019Hw2$DQq^(zJk7e>%Y1?_( z%m*bcHh!~2`ud!3fBp}W6RwZ{j%5B*KVvQRHFiaYQlqZ`Rg@#t(8wrDfb@EOY&=|^Cxbf~5Hd3T)L0ABz zqnbyWm$JY-Bg-qA_gD~*`=(kLu*>72GPNbo8ECeL(rxH|IO~64M{~lnco!vyNy)L4 zOs!LWF}(rWM0*NXtA#s4bid=zXR`$mIX&ZvTFitcM}hJx-*TE|6IkQMqcf1ubIe3R zW+&B*9Z>Q|aw|=eKKHHNN;cjQz~xsqM!$<+(2j^hE6zz3$EJLjh1(W&X~m9i7(Q0? z{M4o94_R~dc!65Fn^(|WJsY^E1kK~$0Q)k#bM)~$2ioY3EizhLX+J-&FnR-ubtv5#xdKdHs#_nU z?}VPI&-@FmNguKPCGrZ#r!|{xN)LaQv(0pMwt1SfWX=VFc3B1bGkQT2L9Jvm zGGgx^WZsy8bKL(L&Vv>ls~>SS=FEVT25^VtU}LABDQuxLl5g&GS}lE2S-kp8ESvpV zHAIis?1V+`%&<$3Qs@=0WJ22F)xst--_ah>B;wUQO6rSOivmGqO@1nalg2M6qifHf z1)&1ZN$gi=@Qqk}g;1ZBt|&Ee$wexbm{~SA-YklQLTB%LM=8d69pr3y z`8L`mDh##Ol$bwMBD^RoP^<8P6V&-2^hT4F40xN&*9v&6*`MvNpFWuMnMXjmD`@Vj zqk1Y|vOU}zul~29*0nGhY%VPV&!DgoT3{~gkIXU`^hfH<+5M4Q%&Yn%x*6(^)S8p~ zBTLP*`y%mr>GGKVcq=$CHR7PalR*Kf1_iH(DKb%mx>wx;UjD5Tt4O5NENFrkp=ZTn zl#c}BOF>BoKay>&*0b{#d4&^SwXh=BUZWH5hl)f(P~plNq-m0?$-ju#W|rhgp%cy02^VX_jZ#DDzSU zb*EaRqzjQ{soMWNE$+gRpslDj22t zb>t++nW~~q()>+kEfNaZUDABnioiy05w&@hH1pWC1bzEqsniXqEbbq|#P-#=|D-I_ z_%&oX2KVDs;Mc{e3Z52l#hKQ9$*;dEVw!!;&{ci1I>+hBvSqm@}}@BrP2aj6I>;~%@dIN+Ppg+tNf zDY{X=O4bF!=SY|9I8UKY5vX&3m?m(2U;Y#6Nxm~#pzr8$COY@|Tg>%Ipi zOH^QXFpXDg5TsaWK^$tPLRsoBa;BgTu$CEm9HK9H8X@A|ROh+&;Yz;e^ z#n+qCXP7Yzq<+zpFt>Kx+#=?J1Xe2d(Z<=gZMP)q*qBcn5p zMf^4^vn5kJQ~DJ>{c6N-&mg`TybH1`)y5bBb|W{)Ab58Kj2G1~m>&+4j++d?NaUX% zssWI-+kA#UMoF?s!MdzO_azr!)ISCjGyO2Om}9@HVa%B=#}2_a6=hpPpqiDTg$nNu zStL}Gf^4*1qnf0~;@d}$#Vl(qYGv((Bi>bd3-iKnjRpO3Dx->Qyk`$jsm=E~c7J7( z?}!dJ--RPsJMPaMq<`&6{K{~+K3b^P9iNsSMS3bsLQ8BflU zEn_UfYn@?^eXx1#Ym4p)$7^aWeI9@hQS3xIwD^+5FnsC`=<%5tQ(0YR&H_ID z6jSF`^owmZMt<1%IFw#LwQ_P98_VU}2kg5hkTI#S~Fgv*OomW8;43 zCGos-Lb>b3a|A^MsQ-yRg)W5b#Gy!po&vBQGG3?dQ28FvLf=xkR^RrBbT}+z3^Ctk zK=AOWNi4>4cqsazI4Ha*gT&_Z&r2VbDr4*F`h48k;a3~b9HGxHe@-P?_Q{rg`;2|$ z085mebIiBi{a7nGL=ZOKV*A`*IpMVE0i{LayszheAg79b=3-CcqRjph*3?^s>K4!j zwTE*LOH=XBl8~*DVUP0P7CtFC{Ne^nuk6e-{^718YY^gEU znbnz(&nS#u<_sp5I>W{i{Ka0>ntmaPRp&*^1IDWfJIMfD46=WoYIpNSn-?c|R}0 zDQU9^8yk{2Og>uMEp{7$pC~eU7n9*URYjlm51RDZ^f{%`Y;fy4DdmvgX>y*qq$rdD z_N0AbuD_*2FMowu_^6&{QY?FHKX!m+Pl|k5L10zQsE>HHhR1yX59mZy`20f|Y4(i* zFC-7MF;um*2!E)`yGnIgT!W<*u}8&laJbs&NS$w#Di`@T5GO?f}FB>0+&fmeRp`o@SmE^zvbIXr_;|`g?ba)w|lGdKW?*fRa@2j>#~v zcgK8Ey_+EosQJQNN&jGq^-ZmE!iHYeSDh6d+7hs1qVr5XE1K`Gydx(voPIH<5RuK% zs#nx{oj2{TT=sCpDf90)a)c}~6MlrMe2T$%tI-j5<${nyMqX(MY267^)7uU*Ek28! z7Ak*52pkFbca*j^ip=AX?NUSPmpyE=0AC`21!!A~%~rpx(5+AOr)uue#PAh+7QIe9hnCTrg~TGv2iI7(C-DdJad^R&4p85H8_Bj4FA7r8rJodsnOCbn>4Gnoo1zIOQu<^-7?No5zhEO)G|Zo#Mq?5 zQOHV7IDS5yW>wk6a5%`EMwe9US$i_Y?t#v1vZpr?^+L3WJ1 z$9#&bMPlP0A4l6_hmJ&#>u~&xFfsJN#(UvD?%1Kj(P15qSYPrSlG*tj5j*rn6cG>_ z?QqP=d^Khj^${E3W4}_XkFvM<`~6Xm<~JTaaSc@R>2oq}aJQhbo|WwM-Ae7g4GTYS36>(m&nUf$*}cG%^2mWTySB13HQ4v! zqM{AKq6MCGTfo=3n6n&fb6RpZZU4G4HBY>J7Qizqu+qE;hbXh$1rtpRz=Oy+!rBre z7`vNG{TP<0nNpkq)QtxORrn0T6tJN!pobNby^Mgzyvlb>5vY^w$CPSXKqrnaIGTc% zEh6!dv|)daJug?_3lteN?c;c+2|1t8+c+pBbOA`hXNnjHeIhl=g>8}4f{-MtF z=nF!*t%|z|<*qy$61%(z*#V4ECm%1y@1eBG=n;Wyy!peAvYZ7kVwz)?U?~#Gz;aCU zi!-t~zB|nMyQQPm6`7sS0UKaglKPS{ik);ElR%wn-z>m$!IRa|YHT`s+}by#gwZT> zy~%X}(=XZx%|UPCHBiH@N_8dS9@%&C6$vR%xY^oYeG zg#q|Zid-Oy-dL;4xx#FR)6Ypm=_l4sa>x<>Jxc1#*W>qDMUkapsfveKkbenefx{Rm zs|@Xqu%55m!Aa2qG2}kh6y`~yUrwm`Zg5H%k_z%H}3FAV%#-| zm$?p}j2!49jPvl{?WBtaY*#_r_>I#BdAPECLIBO(=bU zzUfDL87h^Ed0yCp@tOG%$oK9xdW3yl2_1zKXD+9eL4yNA>l3%yt17idPMX+)xu3I^ zSKwstSeR-^n~DXpNeVOOf`IAu76U|svMF(e>qyQLwynp1DQqZyW69k3jVB5-*Pd=y zyj(aq3dJ&%>g3xL6Xpm-WB19xTE`&DB6Vp=W#t8hhkaZDjz~I@Gsq$*}UZiSOAH!^-Nz=auB;9@fkM75$HFd}CT<5A@;_Rv)33 zdZ$y*U9XpKiGFTen%9v0Eo2-yE0)Z|lP>9s9Ov7@#SI**7z_T|FodQ?5_nPqEWwUP zBv3%$`vmm3TA@-oMk;>0GdY-Xs~TMcTC}FmsR@cp9yqpNe|ZMM#Mn^7z66KyyCH|b z^TJHRi4*;yws-PsayP-E0+k~p7gSzrk5sXAT9Z|8GW$&9&Mo3I-|8u?p1KzCtFEnOrMVR*vIX zaz9n5BlT(%Be_`do`&9uP~s9#qTyv9^w%?JNl{?UAE4=g_M5c}9kJfiqi1_}q{4{w4=iM>X{RL*D;x=#{w=dEF8lqPelRIZHDB{AIFnB6HMvb)umJ>qa%Z zWIY^w+!@e*yw;n}RD0sUYFA~dZC&W4+SDnTDtW6aOEOgs&r~_Ysxp$Vt@*3~io^hf zo>AS%Spv=)!dSvubqCK@j)8%woOq?Ijyn4XvQ+x3Wi3NCx{2mKaF?~uBL>1_&YFpB z-_QL@uB6EXP6>LmE_N@CvCR`lduO}v4tj9Lw5S9DVx{T7a|@Bn9KJeDGk!*(Q`Ew&BQhwx7%@JQOnfv1ch5lu$4zj1wpYv(nK| zC`c(}=K~=Q_v9*n%B`l2yY;MywIhs9{oI`b^Gjz;jxz14F|*vzkK(Nr9ISaw+)UZp z7-kNJfm`yu^ga0urt_Tht8^~a8Cq@I;5Ef@v?8#fw z$(Lva_5PW}Xn9}3v$6#rl7bJ*@>Df3PB_9Q50iMsA>fa^94P#^ItoOdS~`_O=lG2S?mgVJ4s-z>jVB(yK#NlOk7y zjG{Nqlc|!-8__UQHz?$oz>s?UbV zC?0te52=Z_i}qq1sfv^?)f&GqfWT?k_#Bh_)XE9xWo^KTOLNTK@6aSQwIb6nnuZcr z<%l)6a;G|IvxLPdl&G;|xaSJO{num58nr_ox82|TT}o{-&#}d9z@`&Gu)N7f%>s9*(n47aEZ)RuKhY?;(53-*oe&>1pnmBs}#C~I=uP7a9dBciCv+zsSR|IyqPyShAV z>Fn2Y1eo@AAOHeAI#T|?}PbjbO21RTxwV`N9S7JIvHU@_f%cu zyo<}Mkb}*~iV<1vDy^_b9O~{}rN&EJW!*YPhhL*pK519Lezvb$I*5c=NOmo_7g@qfY6&3oM&rupQN75JW zW32E9nTDNxeRP#|D$qw=LQxPH8m~y-?2X`xkUc-ekYCbIke2e0_?v1Fn{W`N@5Klf z45ZGkh%|)?d`F@m%iP$D0G|Qf=hf5?%A#!nSYdq1X%&ynw(vA_7-gQ}{E~Rin#d@Y zUT`eA-_-6+CoU^63t5;}7<&xIGV~Y$?cut~Nr-^AF}k6u$$KI>sj=n?azV#zO>qu1 zv9u^P%y5J$5Nq|v9aE)bx;@pl8m-r67LgY_e~Uhw``8?(2p!?S|MrD?TaR26%-vFX ztr^PdS_BnX`Hl!}d|kfBRX%IHk;nSyv`3%4tEp@0Lf`1;Sy&c*2ObnoO#C~rICCIJ zfZL8x=j2xE+`m8AQ)$O+8M~{eEFHPfT-7R)ls$SXTy}W*&d6YHs~oi&^KLp-v>Nkq zI^|f6K^teGw7Zgl5g2+bOks`!4LAP`YCCL#XY(F{F;DgsCS)LE1!M6G4On}z=wPwl zBT=_`m6g}Ly{~xcQhIQ%&-DcpXVK~UY`K<78Pr%QBwA$uZno+PV?r0${-uB3SUnxgnVWvy$wl=5T zpM6(}Q^-h>(r;>wxB}9sAlrVjuCv2}g}g*hEXaF7C{NFQ6E!Vd3B$e?9nm-C4Em#8 zHbwD+bfhj|e5}WOX&C_Dkw{>FzCAK|f4-ALe8%9kd(T3}pib9~^>qJJr37!$+j{dE zNBAa+<*-&UEXIS|%UbfAgWD_Fq}16Kmc=;si9Pxo@n@2zV|L*{%B@(6#q@F~zD@Ww zeSLrBz~z3C8E3zUEpMr+xF7E)vLsB#ZV}v5hT@5*GKbW$4ehC{PyT?qHU#%nHW)9m zrVhvW8!6^bXo_L{BAjk>vCS*>8>=$<><9RJjyS>a#RLk$Q$A zEnaTTS0~D^cQ*^cZ&*v1-$mxzjGws$i2tx#S*pm=cZ<2=KZ=WCcu7pD7G2&b^2y3p zXllUFW2HXW7z5LA$c7HN5GF@Utn&<gu#o8myFCNO{Fd zvFuUP7m%{qO1Z(xvy_yzR?2jhV$?$bB>vM%oMa^`D~-?fn3Z_4m8fQv&-Fi6Vv!`O z)s{I3(&qN#K@LN|%O(znpZ8u7G-h8WtdiXa+ANQ#oEYqePkgF89@qe&+y+GjRi3BH|SeHKh z(t0H0DHe>(!i1B7h+CfLWHk_L^}oypK|kfO++2WE5sL=x2O-sHgd9!d94+z zfHo2{51G>?Y8ZZ^=Bv9@SeNi7D_kq#7bKi8>IegTqy^w%D<*aizHO7(E9PP7nOFtd z1+h=8m~1OaReL3B7>^P)=ULz%lW?6Cep13Stgvj}n7Ru0O)v0!qvc=s%nqq#hR6b$ zLmgtuLrOju^SCfiBFqip&ej~10f_bPT{0O`SVc>1y5(DUWP?%|$BPuO0bfF<`bhSM z9)W$MT*{g>;Wjdp{Bp`=dXT}7b*%Hx9AI^;(-ZTkJAOl*9*^F_ruG7ONXr*M@UGW6 z9uPD>s8;3;@i<7-*vzq8SjC?ca>kF8C8yeHEG;_l3nArdV=G#cv31lA?EPi*=H^ls zzqg&~wdRaZP|%d~T7fs6e38te=;oqsUku%L>!Z5#ah;JXb%Yfd!;*9G)X;Qa8@>#y z*nyINnjYKRZF;VzW#zO)P|W%~zaw|^X1L|?<_0xV%6EH6hwY6w$QNrbhK-8u6`97i zE9N?I_U2<^&Dg5+chTuYvQd&b>(gH>zr_>6>@(8DAV(67mjdFYS~m~s=mDD2y%0dB zcQ;f?C77ybAamC*D^Ulmt8JN$Pcdq9dB1my`HL%QiUEEN@N0nI#GF3280Ev?o#yzP z(Xf{qW!Q3+9k-$T=%{&gRM=!+=0u<}uqt0NB-~G9YtyT0yXCZZv79vm9Lv*E(AU$m$)5N}8R6$F zMBd4^l;b;O)T^d-%hd|XgXlQvlgs?3DmuixR^G9@!7Wzy%fKxwo#b$se)R<(uULmi zZT$TW+RHG=zI7Mwa8q(PIW2yV0oqZ!oQ7cFFIH2Y6M+ra)24K5yJ+!uJ+2e7$$%84C^M+O18CEa5VU-!6KM! zHOpksjh)uV`Fs>c!BIQ4U4pew?UcOKK_T!WYow*2;Uml(x>zt8kRL(ty~e>?^qNU} zyh^~z*#?1>5^nPZTq$-Z$86pJ`Tr2M%GrDt*G=Y+$d>A`Ob(gKzQW&S0H=l(bGh%t zlkUY>W4skI@RR&tx^e%J`X=H0?pO8$oLx1JRfo8J&$}G~xrIZ|+}HG~*2vY*@QWRS zlC?=$AZvW;scg_U7L^=PJa( z4WgH&-7aEs^Y2B8Sx=HF2>udo&k|n#ElI$hH z&#Szxk#jju{YO5p@zXu)M0NT077i3_v+jbJe&2%%rKvN4(?G`5DgsLhfZ5ejNS#_& zXS)whzwcR<*&4=L(eG!8mRBd#)3JqjFLM=Ay1?P)mUVJC;Y2CS zkV76y`Rf z5331!^mWcX0#JA*_?kD{X0ygifq6*4Ae6{(HCSL43Yg0QL-#>+QMTmY2SB+1DA{pW zaApb|hXp5K!8u>x4Ei#h?+k$R-Zur#K>j0lITr%}u`J=%0HJocXx%>C(abe!1?|7f z`6OV;UCzG;jn`UPfT8Yk{tE%IK_%SlFtV)17RLAk8`{M8P{>Q& z<@^HO6!2&FfmcS3W9k?vPYk5BYg8pdYttl@aO=EQ50kn+lU0mr{-4s?P-$(3)?~-y ze?x20OwYbVYyD$l9Arp-2f@l}oN)gSA$6pje#!Q{d5X0nOLq#5S2?c&$dtnz1rr!B zBx1dCqBuW`g2rmG9?ebnAx1h5AySEid|-R*t+D&`KZyy+Y29C7(J8-daW-dx-pP}N z4WE0i-avOq%N;r|)KNAzjt8|n;iwbX*t2$KFA?acgMnj&fyBgdP#uQZ5y3sI|G>{8 z&D^x9U#Un1L?0|P)1#y>mV~VR^$m7a#rI>p>Y+L#FdR?yMMHI*+7m@ML87UcSN?(_ z7Sm{-z69;~WinZQj_|m&`-pUa-`|Qm1TS1$$#?NB82F(Q)^{7K=*iJV_ zX2@`08xpjq-R^IwAG4Ef8dQ!qqwn&EV`;uUoKuRm_vo=Zdu*~xtMWVL%A$7^?$}@E zW(!6l`nvoW8Hlsmd$ip(BO8D+RW$+3iXeBi#S0y&nZno>#gwu+koEhPh+?ikuwl&5ML%z4?(5v0i87qFC?X z$dp*G7I7v5DeP}cmc^1eTH{}770wY_BdZpHL0aR3Dv+-=-XnotdnAGiDpor4*EnHX zo1M`TF^-ByGU=d;lQQbq0@^!jx#4VMGonRG+(FbpY>-#FSAuC*ih`m{KSK$ZE<^39#ogr5hPbkNvi^p6YGY zTo^jo`rWfewrY`C&yys!0vz2R3e!@<+$~q)aI2)%L1v-WD9%rTXNw2X%|nBiOu+}> z^+o*dQ8l|E{mW-XC^pAQ250n2bNpHux?gT)S<2%v-NZZm#YiJ6!@1PB@^w5 zwIJfJDyk5n{Pjc)9IeOdoHW0+IEB#b2-8LmLZtGorbyEZt=<&U8)>p~So^++#jgxZUg*WC z-$;$l#HG3B&)8lo)TVqsRINb7#Iem=zvGUx|Q>gRpR%$hC$(%xG&fU60u# zml|!Qxtt?%>~Cw07HVtW8oV$sos&9`XJGT?RzXkpp$A zlM7}Gc$dVSP?~1`j`=fy`)Ag8)ix%BSjy@`#;4Ymddt|MrFEk}6@_%R<85By!~`{? zE9G}O8Wq{n1Q|u$7$MC+c|h~s1DiiYHQy;_0qz5PMmMg8>v2NGC=h>6j}qeqkS;P> ztsE3XrWLrPmC2t9@#IAO48p9HNh@K=cjhk{($wP|P&!#QshLgn5E8SgPOcekZw|{( z{;6|j{@5JsLtI)Ho8zG;HGB2Ay2uVocs08?>J|v`%cD-Y`Jkp`l@myE2}Sk4&6laA zep#{2sP<^p)iCdYcOeX12k?hw0M)onXWSwe8@}>8ow3=);C&Hu!rPQy4vB9vhcoyh z3b5F|V#deVY`&gIVRvl$uwuAEYN#=F7|uwqI@>@&r%%3T@r|O)!f%sB zus~CO#{~9pvVt)f7+4B6sjH-okws9_oV-GbODp;|OaJ=0jJTtg{75|^%so1nskR-= z5634kGuF$tTcW6yO^I#zS#>gMa=%M&&XW9&d^(FES?GSZ@@DJimwlM-Gnow7 zs>tY6YCppK?a4~vGhAB_OU*@t&5Kz`Y0wK+oKwMV72FFU=0jD*p^_+Sxa(0qn-8>8 ze{B2%goRpV3?w_MK!E(&@DvrcY~=%GrCRStq0A9hIXS5{Rx;_i(?)CLCLx=@^7QCX zh(c==#tX-N-nJm#!D?3bSwwBha8u(U`Vx1(c`B*zhuVPRPJ02IBG z1*GaRsUmv1=q zaK^f0wL7`ru`4OR3YDF2j`w?Im|xSYIQL}Tfz;%psi#= zZPUwNl3%^a2{dyjd#gHh^R7BJyhSyAxK;LatHCwCPakzR%eVFaKEgeZ06t0k@EqWX}Q-f~HYF-$TBSGL>83 zyE5P97$Q8QoKw;3kp$J)-c=s>F9rMDYrz=YP!4}si3^_Q&Z$s zkeVbfXXz^LvV*{C4IIzaBK&0LtF5xA zds%w5XSZ?tOfRZh03wEw*|BxahXe7`Pu-#t#{v6(vAD?HC5YR z8{i$2@jF* zkd!82`Ljz)5|pK@-Ac(ZOI)M8TWs`>_HiM3wsCc zsX4UeU{2~rdF85Z9?VPen@NifI#MBd<)tEINnxs9^EFmEShe~*&`@pHA?dFnFHg4O{P5NbIL}y~vv|(uxtQk)o*Q{y=Xric2_{7jZ>bV!f+@)N??#T6oPWjc0;(}wG9nn`qUNg=D){-ASYo^p89nOVk5vA68} ztP~l(^1uXU0r8GU9b_NiOFj-LvN!ngaX_nGRtu~A{cc*Wd>owm>>KL1HJ4jHaA7d9 z;5F@Dj`UU_!ZN>3Rj7XM4w75SURPGOFVQ$6!VeJx*1HiHIdnlVkYsK$D zZq^Y~&x^33j(p|&fY`9n?07>EsVuF1?hhNp{b9e#{s`HF@taG_ti}rWwY(l^w)r$J z9n!+@kx863{Gfpqt0$&MV zXP>`_Z{w1Qqvbw|074E-lFgzb*~@U66=7j)lS0Lj&Od!TcL=Qjf9<4_JE< z85UqG=NfZ$cnLEL)*f@}wbplHH#3(!#t@5_l6QAf8l^XV#8Els z?+DY0Jut(a-uN4m6f)G^sk_w+2DSJ&^Zmc~Wq6hhsoA2&g7p;@Ldq;=Bg6>CaU`)B z$=2oRG>~KGKEppu5GTlBn_~-mIu(Ic_F_Q~Cx(c6Ol_n{*LZ5&(^p9wzOoX`VF1LEx$tRl%7_5k+45w`z9P;HbPD*EgORs zt%LWeJZA@tG_Ui5&C4$2{XWkpJO_9ry?}JhJpT_eKdQGrBW8)JSw+#|!OCTO{ttU^ z0v2WU{*BKFC@MI(pt$9zs3@YC8;U&7jDijdN^W6Vf(W7l5{qJ4f@pywNqwzsr!uou z+tDn+6_*sV60;JQY?Ky6MR4Q&+~?fS%s_pA-{1dw|JU_0KB2RL!i>sO7iJ$zP3xH|uhjcg++RUQ7uz-=!$WZQ{O-X);~&{3>e4VR>4iacN| z-7F)fINJ-q*yv-Avue0aCnNOvTe#vBz(2%iww@7N5N6;o&rw@FUME`54ZMN2Xj%QR zaBA>Q953FU3povX3_1mLH?~LN5=a%dnDnzH0n%n-Dq!(SjMz$yp5W0yRAOp}*v*Bt zLY&BIZix`X{Dy{I=aHfU*B<6#tZ{WOAd~*PkJsTi z8j0X=RK^9XoDC;SaCw?-4k|jjtM<#?3Lb+1qu@b(@R`T=k}#t+cH^0?t8bLS*_x89 zjv*HiR=UBm(Sk26Syvy3!B#7wfWjb)9te^qn#Is?D>gcy5mOtcHB@KYyIbvzXXS?| zZG;bZe)y_{yvMHw^9HhjgsoqgY|q*p!swVow4!nCFKiC0>Oo7Z7Pcw`Th$iwipr${ zw(%kk4MkW9tFhBDp=38Zq5HxYt?kE|hq* zW)t^?Og|=s?k<)%)CLF(OKHn}M1flpikprt6m&qjYMbwUZ9~ebUSa$8d;~u9G^)To zwj9bm-B4&7i@wsu_B60O8<2ar0Xl%rh@#PcBP^03|NK1&XCHEJ-bEKN*Y2Y#3C97V zgT4R2%sU-3m_ta)*>gw=bbeTt%0m=<9H0!x0Rk)q=tm&Ek0J>^^;Q@#NS2SD$tWcK zqMs7`U$K9(_5&?xw!@f|$yY|#W6CkZ`K&f}jGjVA10{qG6R8Kk1)()(SSL$dO(fko zgL5*GxX&!?FYFaeiA7+QYMAv>q>C*gKkQ6|^(xmGu2({X<^(Dkt?4EQ%zy5>t@x8>QBPGOph?!%zmdxbYuMHZ{{v&Oa;{d!8@}E- zDtkGOJ~kW(v;Gz8e>pPl`^eyO!+JV82}K6Zc%6vwFPnm}(aAO7f*1$`#;y(3E4Tq9 z_=gpj!`A%qxHqvP@h)AUJU?mk#YQM(nS2(w^+P(b+pChImcGa5&AFnGUs9T&-+pix z@_PWvlwD@pf;HoSIk&-8ykcB8*-(Pbtf3ea??;Ql0;|EW`Xn-ACZE>it@zrP#pT8& zTGK(EOk1Y>Pfj#^{b>;8sbI36j(-L3HmtvZqJ$b2ksYH*@*g{iuo2}k2o?%voTLr2 zH1^?`;$N+0JQ?W-I27OO!-f?UgxlP?xGBxLLF8lX+K8Hv+EN_uMoH8t*=KL%h2d+C zWAk8*;9BH`xmWeDeTALVkY5k|Hepl;%VPm}BjLf<4Y?nS+ILdknImiR8C*(4_e}K4 zer&z&k`)dxFBdE-&Qi&Rh8PTMto~L+8~FvbrwGa~%>t2FLxcAUl?Glke8cKD;VP64`dH8SKX}IMat0L^ zXJsM`tF9najPs$bGQ%^J)GQjD+p$*;x93hfF9CnM)8&-XY zN7S>1J)xLQm=H3Z9j4k}2?mDB->fs<;<$L7dfa9oevZqIKPQ zS&fdOFvsSFq8i2R#$B!}Dy-Xl|U)6A^b0!yb zgQOfK>+-XCt!t3Ep%k_9q-Z+gYDENhN(mR$WyLQj4VNF0z|f!v*OPpxDts+;jBc}$ z60I&b>;Ye-0S!ymitLE$5Z9m?>#L45cy9|KwcqpPnOX~3T_8TLwpd+DCt;X~ zux6C|VVKHqkw3vbB6wf=657CSBI&E|+U9r(V_IdlY?$D_-vuX3_sH9V3Re57hy$B- z(oV|Nmgvv@E2z3)g@GoH`?19SNf%@SgIPNiNT7a#Yw(^ltcR|ry;SAi;JsuF3a|y8 zU$w6Dr9@M)wCaPzVUew_qlbgGQDyqxHsGG34TI8eSfYQG+ab1C=`VY1)|OQzD5LqLC(UZ zVmW?Gl!^;FP1=ZxaX0uMizbhXW;%F(E1JK0|5rt$t)FDsh!5Eory@bNJgR7-j!Zsm zFP|82L>)PWIueFin;4@#2SV~dbtKXSSB%zSq2j0}^JO)`g`fNL$GIDFkBTCq@8<;H zO%E`90a@q)6j6PvOphJ1cpxn+LGfVHfKzA%>wN2)Bl_UdG;9r_1u6A@n4{4Oo_e|{ zi+?HgZ8PARTu|?W!q_J#Kx>h+4Q9bz01(&2vy2sXbza12H1ajGV8FG2-6f2<*j&DiGR$|sP9@ECz z`w-MI4B2*T(*JGJ!nM7JM?_U+2ouUL_%P!=;V${ZHVeFBFL+nmlhTdtHHWv1H{p$T z8O{0UO~PU*ah_-KmArIAazxx&NXPL-LBr}-o1y5i0PSJxjj*Ah7$c(Sh$PtB$|x?a zfE&n~F#Xz9A+i~1{XM+&4klbYy?&!nMJNiACWt$ia_HgiQGBOaZ+LPL zBA#5}I})iWkDwv%wjq}edss8S!vd?h;ft~a>>RP3ZG}n97m?Ko*qk*2JxYG&ccR5~ z6&;FgH`uak0l#Czez0-l0^H#$UPEpPBCPrd=XyudxKn}CUqqbRkVm5-w3}$-xIt$hn?nozA8ZGgCMMwx?YkjbS2EuX- zgmlR=ex)`eT+GKK+GhZLKE120C8pGTM){Z5>5(Qomv&>$V2!M_JqoK!p8+6mD}UhE zOsAwx8E)BX7kumEpzwsvojTjActm5kw1zx~UGSIRcgC1oOoh|r8e-Xh(lRSh^xoo| z=eE~&VWIUUs+SI}-8Zi!61qfrqMZ9 zeio7UnIL9tCzX-+6Uk_fHq#wvRNZY;5n^>tT!&rQnLxulwu|Pw@0)E7~Q?Rb!b)}T{dbrHj!ciii``wA?t1&h< z>ZdK%(b&$Z9a*W7x6l|?|4G%u3LPri`|EfP4H}&3&LX4kq1abK2;CUE*|igz)mB7y zEeXF-5uRJ(g6nPNu!c#!4EL#wQI_00`lSKbmnlMtHkUv&v~wjHn1dmTVIa&+)XO;3SfSH=IgOj=K33-1UTqA7bX zQ1)`^ZQvv3_-LL>9o?<)0ey=W<5nB2&mgC2D9IkQtb(jAJ&b3%_?n(kE$!zFwL7#RaB+MCpp$KpBWP6%8lOo=xEFAAT!c%9d&I`M zsl1oUXv=$s+BK?sgf*MS1;ghwwZ;Oy11)NCQW=jP_+%smZLJU#rkhpz%pd=$Eui^B zz~X%OmTAT`ZHKb?e#k!Z=v_k&0%vY>P`{)m$3g|4sFgzV!%#&@ zyW|eNk{9chCZMapI=QMPzQkf!cbdlZxX);`SyM}He!%V*TW{f}8gA~w%@=SJYW+r0Ng0*V?sqeo~^42CT8TEl4fw3eeSdczZAC|#IYh6Wik&(fz{beHZW&$W+_7M=&# zJsXARF6On7I=kN&-8R?-0LnzP6vO{o(sim{B3|-`X z(ES_|x}QTCQQBEyD9fb&$F3~9)dlgAvTOz34?cyydKN-E(1H&Nzk;V z8R-fPtLfH2rxenm42PzyK#HqZ)3H+wV19TZVxn!ti{59?#{#&DkXo@bC^Fhdar1mP~an@W3ZQrR~;9^ivID1M3)VEmiXQ6Y7k>^etrNxMWmA(^&&FM9^$Q>wm5;3*4?cb=4<<#d_O@q!)%92PO1S#;+ zLDbkXAs706AjX|CTOW9+f+(8gP1K*4p(lVaARb&{iYuTNu88RG`WY-7_^tnlFg5 z7NTw%=h2sY+v=aU_vJ^Q6RVV}`!L7BB1lenM%!M46b@;G1o4kr@nky-$vm?4HRfJx zN|w99Z53u4lSl=C^FS(cYsrnnE^n-g?qbeO5(vr;rbMJGKpA7MKx;bW!pI4^$Cm0y zNSI6r`3jQ0#H`B$v#u7l_Yq>%S=)^7goysI%@4&w3+cnQ$*{r-tW3%=yl6cozujv> zSN+kKCo_{;HRxMHPKs&bZmcOI4PmsC*IckjZ!v`347FZ|O*OvZ zW9eFpdobvRISt$Ai5ZoV{Nl);9mENsB9&2S0LZv$X9Ji8m~ z|4Y7bX&KH>c&(X)<9FCGjDUnU|q5ugTKzd%=-O_ zbLO=b{@AFt{kwsxGW+1lQ`c6M`IOMG`sGsBag!e5G$2gnwT*@zj`SBDBnvIZR{ zqQTQw`=oWCLTQ%Y@y3slm^IR;`Sw+{!=vp{%vhbLPqZBbvVr;S@Urmssb!j|!yu6z z=_9b$Ey1u7I~a09T9;Ek*IdG&lwk0yz^@V;cuovUuoxc4?wniQ*RK{FTY_PFz%=nZ zIA9W}Vf7m6r~Inn;?)Gh{eL7F2JbUGF<981_Fe`zHHJBT8l<1$Pp%<974)_Z)BBtP zqR+SWcAE=+L~qYTknrPY!(B=db4nS)>XS)6R?z@}CD=f0Ci31R!m9H8lKd2S4tq{S zaj3>@nB{@}Wq#OgPS#xUzg{0`xV-Bx~;HHPT!Cxm^p=Sz|g zJK?5`cyCJ{x&I0HFv1IHFWq?_&K2SPiTqtctF*_SHzz6$E$? z&&TmhzjOFqhFzaRne2cqGN~HxTXqLGUfu&`9KRy@NTAnn4`76amw&pu^gm}0!m5NF zHl_I;;qZ~MSx52v1w6`BDSqHPGKDwPR%XMyrJ!Dy5u|tWDg=+}ggm8xkBwwE1FYbp z)?B}I1#8}f!pwV+3eSr^qxYK5XyV`p5BftNVL!yYJ@3tNZFt2P~oI z`oRGUKn<(SsLlJ1|MdrKP`A(PiFexjC316XPM_Y$892k=1H~PWlIvMO-1^hqpCx)W z{i=T7+3yZgTxI)Co$hl+`swpL`C0W5*Y16ns=R+C_gAVse+ES*o{1e|`>xJ8w6`=J zwHp?FP-^?G&MMt^%d2~T3gwnpz!kXNd+y3R-Q8h94*{>i1_f3rY|l}Uw`Ktx-(I%& z+}Z6%5P+wLu=NTE6`l{i=cvQquH`zsuPXZcz2^>Yck>c%qj&GCx4-8?MC|P&?B}={ ziB?6I@4NNTA=vjZa>9ZHopoj3)#!tW^Z4F#m)_}fRYZ8{oq$%vt(tXw-|1QBK))5w zCuUvVcRKpleW&MKq~dEXLYT7$c9n4FnUD=Xr)Tv^A$oq6o51I1b)QW5R`g!{P&Cnd zoKi}qx{}ycbG(-kJ)ZvUuziTOH}LJP*Zfcim3c?(&<>M)e%X*+Zn$r`*(eNfNy z-9CaZnk*Tf-2=I?6j8{123F5!1+V_jGMsYmOwMU?W8W8(h}lJ|4^1vimfV;MOv39T zt!37lP^ed-ts#BB=QM=S;2qK%lamTe)fVACHazFnHP4(2?r^8@xBF7~Bt=JZBamD% z`8SIdQInKE!A^}BAh0>U47sSsA-RszcAg79P z_1K!T7x@qqao3@zs~*72?qn&%y3LU7+y;GSOVjG&GCXH`2G?gAYqRmfboz`=wx{50 zyB>fv)M9T&@ScpX5nLt(N3-^FydG4AcN@G%^rRvL@0AaTcM?c^a?>ico$ zAl-57fyt*?rkZ4I&a>SkM{>h`gk%sJX0v-f_Aus~yW!{eq`3ptS-R0D3fyvyU>Uc zN~n-yM%m?ajy0B%_Wjl5p6_9;uPt@Y=m44Za)T%w+Y|(td;Uh%{o>jS)9QMx?hHFF zwoGqj8zxw{s>YaV59o1I39MVC8>IBqNAT`e8Nq9Y+lIhvo!%b63+I2mkS|*;5|w-Y zjM3HUJ?m?Oh+R0;cMcCHJxwIlQQP)ESO{y@6G)*W)tjBC;(Rac& za~Vp{l>6AqW|?iRVY_lG&x4=2HMh(qKWta1HEb?v8!cjKDfEnxek>sg017{bY>hTA zR6*O5NevCOVSG(P29|G#6~}W#oY+(b`rs1lj0NO8P{i(sznECru46J$)eRNS6+J15 zW=$d|wlF~H8;j__Xkt7G1#R&A)VC6a0IfEa1x=3SN}BH+@uKT>(51Rv+>6c?_e+v! z?~Qy{h~>R>LYdaP3}xYpy+IzqWtjtq9yI829^{Ui#6FWf7Ti~ULkZkZVH&D*LvCBuvzBaU??& zT98>Zc06M!p;ZRjEv+_;t`w`Iv0z;vfsAC?zC`I7{Pr9CD=$<#{9Le#X@PqHZj=u-yXRJV4Z#smJELW3r3*9M8!=jyRyJdXYa>Q$mlC#b z2781~VU^lE*isKm+iz%05bk5aFQUo~_wcIS?P>?^wY%L3gn8v!JX~vcd)Yy=+TBJw z&|6)$>tYa6?^1kOB}8Y$45lxr2^3QE(oD2b1mW&d)lWPa?@HXP3OiK6FhUMPd_A4s zCw!Yo&cz0UPFH%-Eyvgc7dMerOA!z@X9wk4jQZTiOgd=JVZY-nRhTHeM0|xtzBBCl zr=Tn`xJ)F>t7lXU5o?=#OH`GGO2S-x9)7H5qq%oUZdtQ@v+xWx4!J|caPmIG%Q`lO zosAXYSgai_`B71}-%+zj#u4ddJqB&AzQOwt$rKU1)$G27O3ScgXbXC#V#ISxdZzNl zb1QmoCZAi=Gfuygy@yqwTj`P!NbR=={b@sO*w2#Fn#veEHQ~k+fBf4(8ieOI^z4bE z%jwvIUzKDkUqQnQEVZvdojC-t69udLcrnRzK++7Yu1RqapgN<2m z;1?ErIPILg*4Z8&je!oaTS3T?kuu?&uLmer>Bz2}#Cmu$p{JZ9Q(3 z=T_?fKDQ~3d;n{KQTj}XO3h;|E~Pm^QDK;TwcXwt4LD(x#V515kpA!C`=ueajI^5Z z$`3(zV?&qL=z21|$1tl8^?14G>msZN&9ypL?5WOZZ?>A;Bjci6Z&XCM9Jj=kMf5%% z9=B(rwF6qiI4@)!#Jx6D?_$Vk4Xvr(1qWv$T*@MbBASDLG$AX|u0>l^4?JZrJMupPvhMgwh-CExAmuklPmT)a0-)7+8?Tix~( z!FM%iH3F9=JO{(-fLmMGP_zf!TH^@;!OokX#HeXmTtdqgPx5)dmA;;#bJ$xw-`Y$8 zN4Od_OY}vad733W?u7=))<8O_Trmm9Anqq;dOT*K(Q!8YIGblt$jGKU5*lbN6Zvd| z=xq-pZc4o$Qh%M&gM9jNKGzVoy8S{V-)#!kN(bDA@U94S+bwIJ%bQLX^3r*Kap(q^%KSw~61*10HDdCZX-yzkIfBqUlWMdXgD zXqp>Fk${i-06OC#XLu3#atn^rBiIJ-qvQ>(JQ1ZMmTsuG_;MSqHz~H)1p5<;Z75Wq zB@|^0PCVed_Z%Oz)AMD?So2J|y}yM}>B( z&$+*QMdWwzrJbbik+j8iy01@!e|e-~?1}gYYqvf$7mvh8LOLdxbI$3iBMV;A;gVhR zeiyRMtd6k4FLLINc)Y0N6vioh|Ks7gb?#}sYD-&WbVw*%s~_#!7AL10aC*25cA0-- zr5#>+$tAM~wEGzb$@h@_%qtLI?de*QThXwm5RRHy>1sWmD#(02hB|_V}<8x$I zI_DMI)+;jvkGKUqw5<;W2f3xj@P>n#--PE@;`F|VB;zH`KR%aHilhGrGyep;MO7|A z`_Jlu&mxyu3ez*&WXwdvmpgC!_+dG zbOK~R$e4N@f0i5~gfq5Pkn^5A-n%@rC$=Sc5rbX2vvcVHy5=*6o!^GL;QVX52tz2k zBQY=3O15eFHdHp|{EgpdBI<&}jS1IZk_zQA6@>M;)!ml%5J?NyFZ5i>s${ZC|I_G3_vC0WGR_0R!9VP_5V%L$50Pp-rbn|RL6@^Q_aBy6*c zMz?=;rh9SWB+J;70L?%N^a9BAwY`2H4J!yG--5)21zNCU%C?Ry^P~N2zhO&2;ejwC zsv=$FgT(qhh#KvbmoN*aJCuh@Ctkt`>BP|^i;pHY%A!^PVbPkWR~vZ}7ca`t)-+~d zEkzRuM3dB*AyyKCF|82;J%F$cX=I!ISgKCDw5vBbPQ|aKEz*Jyq)NZ~ z{4$wvZ|?(SopXK@jcOG;rdRs^%>P^oR4GW{t@;_-~9Go8;ggXub^o0#rodVy&*)4NPP7RhisGWBITf~ke+156h& zUCMMF(;}uHGTqIzg6UB1+EpE5dbNpkAKr1-SC zOXJhh7cNPe9VpC@QG?{ez2aeL;E-V>*(NaXK^YPjHH}PQ_r~#>E=vqGoiI_1X`~<% z-GraW7#ksk$zqHx3c}!E|DQ6ZSvHxHCj9ayxRSA!J{w~#{yN63WpFz8 zDw#hmz3mt?Ti1ax4S-~fjJ5Fn7|T|o(+y-y3k))WP4KWL?t_}}$27sFCfLGQdmq!A z@S_=P>6z1nAJYWSZ{j|&2~KK)QyII+?CCO^@UxoW6-{t9W37BvH{s_s;je4LFKoi! z(1gFS34XZ=E^6X`Qxm+U34Xr`-qr+v+5{Ij!8;jiT6{a zT+LXkpLLA2`r`h(tnZjK3u9!g@dFuSyNNIs#@!evGRD+Mn7k(ZElv1kP53s(z1hFl zA2K}<#t9S1*vL4VF*F#$WHIi`xTuN$62_263RBKlEC25plR%zKWfS++jJ5Q2_*14& z>wk=lwek;S+(rhcGcnfk6T?_5zih^R*!>p9zKqM8@aq`+GvDv8#^q&UY-D~a<6ew6 zGWKI!#&`hZI>w}`AmdXl!`J#_KgMk(TW4ad^`A}q%cv&q6B%pa=P}mmR}o{ad`cL1 z=J=Y1AI))F8Pfgo*)BYf)dqd|VP}YHWI3 zY?5?E?$R^n18Wv@=Vl}>BnNCUcWy##dWJJz8XuRjBu%qHxJeqAJ2yUd-a;8N!dBfw~9ZPr)fkjXJ=(=GtOH$+0Vv$~Y4uCtVKmMqc0Ri~YGk(-21C#-} z!Jt&X5tMEW!hmy~0y+@XufIbka!L1VwNPQEHqLbN?%B|BF0fjvZXk`R7zh{ z{_F=Exrj-QO^IC)KmR`!YEyUrSf~`Aqg?-1a*on(Nljz{9tj0sE9j9R_W_^}!BSqn3qs=u=<;u>Q!jB@C3=9|;fVwZEdvKPxjH1)K&#N|$&g$v_2tnH|WHcpwh2qT6u z0{xg%yfLvUOH!66FUd?d;{C;^r=m%dN3Sb#9FJmJ$TU`$s#}O2J6V^GzsvBP0ve}V zqD#}o<0%0)^YD}=_({4Uu#-QLecgZZpRQYu5aV=-@F#uWi>E;88Gjdxuv73iNjFrd zjbgec;Y#PUMi`%zZcf6xvE|$_d!#YaZA#|(7vP6mc+%U zr(-^-%6g>bC#DH5UMcy%GmT(*ibg#aNq#ERvTTVjFxA2>;&5NKNPY#=F{u(~F)bV= zu@=5xjKo?v8rAq3wlzuj7RH*pD8>Vu*i(Ihx>D2`sy9@QM5zw=;D`F5^TdzV2UNB# z@S`%PcS4lxh?4w)cvP1(9)(MOiPCz7!X7Ec__$B>X6xWc-ZyrQ+8Ozclq6qQ(b@j zUS<3`D5Yx?eiYB!_>sFW@S}7c#gF(W@gx2$6d=tZX`dXGPuX>T?@Vn>>zFP=4-uf7 zsH1$6c|`Ou<8+fHM8C2G{7EAJ4*ql<<=@$q*U5OkV_L!VC#IE5FEF(+{hjGGrn-6= zjyqEyroK!AnT}y21}*G)wH6El-^ z=FA1UY4NGLDOhk$Ub0jd8XqSDh99i0mXp6U@c>)+k4pqa7z?TYK^S6?MmR_<>@yc+ zrt^dgVP?c9&x@!2hz!DCLI`mw3FqVOXV898|V{K3mbxP<4!3^Yc<80ld- z3|Z?emeI+{qiKYbW|}S&^A`9=KSFdmFeUPS{GBgwB5Y7r=qVS1PD6i0NtXWR1Iciu ze+QP~q$3RJLWW24ABP=+L3|WG&AK#~WUIyJu#@qUONSk~n~3l!-QyARJkUsBDnm1R zc%p=i~2kr~Hr&{TqnUlHAZgquqvFO4k;hMK>C0Ovc}I{L@T4-b#RO z2FmyU87cwe-~RdOw10o+{oTfb8Pb3CZ#8BpP5(7^pM$xL_D`?-@BMQQVU^DDuNmev z|DAuDf8+o4l^{E3RqpCFPdu5Izt+00;HmY6Pe1do4bMLJ{KgkveCg#^UVW|T^*7$! z^w#FLx4iT2d+&en;nr;*ef-I%pMCyC@%9~G?)>WOT_vTv_w3!b|3KNnLx+!ibM#pG z@o!Ij_x%qiD^C4*`lp|NIa7J|-1!T?Ui{5=>2lTYfBgAZ^_7~d*RI#zsH?wu>o&e_ zuhTbkadm6n!o6jy)*ha13|?*9weR5Fv6D~dE?v8Izo&<>XRqGAefswE>p#GMV8Eck zfkW;cI&AoekwK$I2SWy+nqucc0GzNOap9uHNy#ZoQXfuB&&XW5ENl5Av*$ec(A=1R z&wq5qV~?-=pU(gPbo~F@H4E;MZX1WWkDNfD8gr$pU1b=q{CYmc7wfGq!i&;LK7 z{4=pqaW*Cwy-5!XO;Kav*R>r_Dh$$t{&DRD?c62f^z zq-CL)!D;EB@P_>V5+2Pp(h)YLmU8SIL!do||CC;ujZune{zr41MBsR&dp?jBB9)z1 z2LCBM3WH{(DVW7M``1$RpZzaK%;Y}->C!@Ql-hstFC~a+NQLH?Qm@6{RIc}Td1IC+ z1zCqE1R1jH zT;#kn0UVNPQXQw&LE|^&$SYNXcwH9UHI=V6PT_onLp62@-q}3-&H9IQY8C3g6P|O9 zDa}-xjaz}^ols3%3Kt{=c9y?V3N;y^7B|s=V{ZlU>YqIu! zv^X2*OUk_{K66_9cx+`dr!Gu4Po5SoG5gF3say!T#^zQ zpOLtPmW3jIqD38Le@ZegGFuo^{A9K3ExGg+R!8E;{}FXL^D`!FtMOgeKiC5-z?s4HVk zGFCF>j7ernrh>7*gt|(`0~y;G2QaQ?OuBh8b&N+y2tlk&|47E}jDr|^F&@R(hq2U( zp*}EHneWHg#5j<#nQ;)~ag0rjMFWJLh4FaiM=_qjIGS-d;~2&h87DHH#5k4lWX4&H zr!dZD9K|?~@l3{rj5T@kM#ebpB}@@xO|r3t@k-`zW31;6pqOzp#$}9M7*{ZMWvt1; z-5A@L-(2G}Zo$~SvwWZKjC~m6AdxVdJdr;2OvaD-9*lz+dos2#Zo@d5v4L?SWBS-2 znJmU_8Ef+Wc8oRoetX7w?7jozjf}k+Z(&SdCni(O*oSc$dx4Q@jZI^LGcIF1fN=$5f5tY(0~yyb4q)unRpx&%VVC3uF3PCz)c#QyG^rp2@g^@%@Z# zj2~cJ$2gv`dv}@t#f*IzCo%S8oWnSXv7RR&7RIiOD_Bq0f^jtSTQW{$+=_7)V-Lo8 zjJ+6dWZa(d7RDVI7c=h2xQuZZ#ubdaGp=Slkg@I_ng3wMK8zK{evHR44r08RaTH@c zPpD!TyE0B?+=6j7#jP*Rh%VO-xIFE4)#v2*8WW0rOE5^l)Js6iU z?!vf&aWLa*#(HhSXq5SF$=Hi=E5=5~9*hGScVTQ|9LzYHv7RTCiHut^&SKn(aUSC? zj5jh4X1tBD-bil?^FPKdjRNY*8Mk6wsrhGIt@-zr?sYvmzl^;Yw_cO82Rnd&b$Cd&Y&D`#|ZwNORA4o93QziRONobYHHyXI!bdXI!nh z50dV6y=4AdG4^7tA0zojjn6nxE1$I90qay2puSecVa zgbPVYi>U)wx+8f98QNb#hW1gAN#Sr)7^gDE!A`*^DHa)8_md$x7MXOGZ>H-WW=vW{ zTp9rvl1g_ZYaz1`IU+-Q&d8+O^CRO)5?V;ww<2)@r*kRiFA;JYGO;KzGVz?AWKKsc zN`TB__D51KGKuV;_Cb+Z%zK0(9ea z+IocQNdUCB+WLp&G=#$_Fc>MdnPA^9L-i#9T3u~DMfHa8ROwvQp8#;Vx&vo|Z-Hi0 z8mdpkpJvA-1q+8>TNQ?|XvKpW28Rb`tpXyrxkmjE19bqkg zs(+Lo%{|paDnBj%R38ISnp*hjC~e|vGtrbzEb+M@TlHW{n&Q*Smt@HF{vKpmKB68)_#3@TKL8+y$T(nfbkCiE?q^M?|?hFtsmd z_tee;P)oIZNvZO9`+K8yNBPjwoh7@4VjH%r>#*}V{f9eOEak6;-4nFk{&iwQy z;Zc7ti8OQ>y5KNt=z(pb~3VD z!tC`=j@uS{dy?g3wx?T`liA*H$#QbEM_Eqe?CmdI)DxLc=XA*QJMyWeD~d~4rYpkU zZe+Tq+0!Z072%W)s&9^XWx6KV)2*dTdtb7@9qW|NWXvHPEmy`LYOg;s{z!X2pvCX# z4=H{}eUtHzv-fv0en)yq7e}?%jK9wPTROB7ln(99%5+59(<#e8++Gi5JW+POj3<)c zF7?|~&;B_c>H)MGD&v`MuRk&#M|+a-gxd4(9B$+Hnk;&P#_6AemL!b~ccK%fa+qn) z59Ob1w3;E)HOtPI{>Rz-5v`oHah3ejn`rD`>qX=|lgdNuf3@<{`db;_M0-AEeADgq zNV+%M->Y;V;glYV{Q>*FUFlwHZCZPqwY0#~7zF{+@9*<8K)kGJc0~ z5##NQw=v$!xPcy|O&Y7;Acv{*1kt{}N+A#)}zidYhh% zP0Wwe_^e0MGmc`uHV=$p{22378Gpf8(*tUH!aU|{?MKrCy0ZI?%-7~OTNppdd`(a2 z#<-aIT0gDn4K=-D8S~#@_Z5tvW^7}u^~-gPKViQ6P+7k3G1l}P+I-Q6`Bvs@dde1z z{g|)mX*9j1JM)8>|0ZJ#<8K&8Gk%|OBIARMvlu_aIFIoT#v2*$W4wj&r;Ljk|G>D6 zaXI4(#@c+@#<+y}b&O9i*7UF~8M_aY<#(L1596O1`!PPrIEe8f#umn(F^*gbT zIGXv37#Fes9*h&2AIsQ>`E3|yF`vb7_I(Nl=I1g0S;iX~-^bXG!}DUih56dL+nviN zfceGD*G6qW<_}?h8S}Fkr!s#u;|k`lVjRfvw`IJM-K&gk>^`4y9ph=5e|CQ_WA~Br z{Yohr_L*?_eVFgV`~{3lnD5KjkNHxHrqcy6ev0|pJ_3Kn7Um~2j%J+8IFHj8!Z?xn zPcY76ez1m_znZa;`GJh{nEwjnjf|!A4*PHzXEMK-@eIa6on-m6V_e4k=NMNop3d0D z_<6=A_TQdy9rFtqM{#;bFm?};@jcGi!u~rj_F?`^%|G+K8T&E+e#SwJH!`*`UdK3^ z@oS6|8E?_}9DYZ}SQ`5nsG#{5SZ zYx`78jO&=cmT?UG4`S>-T85X+IFb3I82d0khjAXKw;N+W=0C(Zh;a(zLU!Msv4#0_ zHO%o1XB^G^6`KEMGJQtIiOiqJcq4~@592K6$1pZ=_`?|IG5=M@MT{S1T*2Y>V_eMq zg^bG>FVWmHc42H|e2#G)<3h&n!7@LuGxlNpF=Ic*?=lWzJf5+IaVldepQWAA(#Z90 zEY_{gcpjv@&Uij+J+$?9JZq;kypXj!8eYU&6%8+D?TLnyAnkV!KM9gJXG}5=XS@Vb z2xt6o6Fx~xo%tl~bjF&rTZ@0WQ+`S6DW!sOPVzu$BmX9WuZ2K*sQ{#23t#GCG;JN} zWk~)&`=_Oma;bR;fp#-H!jbwKk`72C^)+$!a;CHdi2cBlFZDO`A??;MrGvtA#*TA# zQl3VCo%ysYzA@IuK}{Z<%vvX@4_aWCK1jJuETqKJNIcJ;UWqkrr6w1Ov%hD_r~S>& z{ukQQC;5x)?^)uMCgG>r>!svtS~x8}+EwfvzN4N?_m9}?p_I!|?bqCs9ENsZJL5RK zv&Oh7zp;PXW$x@=o|PmU8K2{QOT57T{v=Mc*BgnGu@~GqycGNUlYCA4sD+>ERGyR{ zdDhbrf2zISO8+DscMdW_xCen?{atHnoh*d%*>l>B7u%9lp!LmcUo`Wi>OB>gwFQ|dq`t$^UM1GF?^6FB0Liu1 zZ%aMe0(*H$z1L!U`;hu2NBTAWk7h$S0KJD+eo{}i*xrvz{i3E-C;d95=+oZC3-`gF*Fup1r&z)^@?0 zS8KZ@`NiZn&k@_4*`;pj@ABpL&=AZPS+HMO?zf322G``d$Xd#eXpT;CD z1Wixk%qP8xGoMOSOOd8maA4A>(v4= zy`C03<(KNKmRiE3_odr~wDeFK$b}rarM}it-tq*OGba7Nwo8Tfb5QOb^+)appuf^c z{js+Dh4zWi`=HbC(#ZV+srK<()*kuSxqO`Mo%1X8`%94uX`~+Bkshfxp#DVmFb*tR zkcKt?5+~ZzBQc%ekw)Tld;cJDhJE~$dVfd!?2SovLleBRF|KREcek?}w23j>=+Wgwr`_aRvRc8-bPI&mU5BrULXN%&*k7^%!!tTD0GVd4lFBfM% zIW;-C$78kKy|k}%;HDW64sdHyBja#CLzwTI@XPJ^#TUpNv}5UhpB;-}>(-ZO!QW}2 zJvq~Djos`1Y=W;e!CrRHd^z+=&En#8O2KD#_uuW`f%N6u@oV>8Y0*1;o*h>=JXkg? zdyE}ZI25K99u-$J{IqH?4w^&Ksm_4LP?JHi+rn4BnoZ%Pez$co(U@nqXAmt~y5Uhl zBZuV>jdFi!HPMoX?>I?3LtKK5&wSCfNqNdyqZxb!q_>-XJH{ZnVRKV{pdWUHFJAv;KMLGo4 zy}08&!i5EozfaWbd-DfGiyn6UkZ9`s{eo`$@X4)&OZ*c1TF}C8&kGtjaNI}4 zHw}A3(3mRS$ArD6&K0yW@hd^20=s@f_LW<*1Pwg#y`V)~@BNhQ%a!$l8gr@y4O})s z)Q^~+ZwhM4zANd5S)Y^p>IXj+blcBuz96jYuu#w#{T@LBH+L%*_AW~W4fHuCsOe(A z?POn=oGob4hZTaNe0Gq1(cC;iQ_If?y6yIeFUh|ALV=)x`4{H#J7S!v) zML~-m8}k*}XMg{+pt|2L3R+Ye{59Ecn^`Dm(R&vJEh!1wCEP!^PSC)BNw`hTT1SWd`1hJT|Q6H!bMg=y?*;d(7^FONu1}p zo7`8vKTy!}Pp1o7{n`pavr{$+y3Ke*(8ABJ3X1o*hx{3z8Y^k}LP4`@3k8kQ7YkZh z{fnT1``q@5d^|o-(DH86BraMmY0Dx(b^;MTLrE3{!a3Ls}nT!_3j5md5j$^ zsOd<8phY28K?9%PDyY|~6M{y$)Cp?r*sYBG72*?%L`$y630k@PNkMhX-xCz|Ptc;G z8bMRrbrk*aw&YQQW`8_KP~F*;f=1Q9A!zEYeS&WL%_eB|SFI0GJlT&A5;Ug&eS#K! zoi1pq@|>X6TZ#oW-u+R~K;@343ws6H1(ku1uZvhD614h!qM)hcvjoj9dQwoYn&$)!4Bjkg^`oB&8ujU3L1QkQkbIBx zf|mH#2uf5 z4O|>1X!+4;g6jO|NxU*uP}4V$3mVhDKv3hD?O#ZGZl9prT)z`Es>eA& zOYW@})TrJSG&R&i%)hpc?ILL9C_h2H28S zZ@&7m*gEA>h<;I@q>^yfg`KcrEQcw zwaq7-oOd>)!_LSaNA%5=UBA}eS9$evNY&LY_q2YbrE;KINoxL^B_a16fM#Zpm(nKX zn?H54y_Kz>xuo`g;!H^Lnac=9(R?Uw*Qq z;=gnALwfgykfa6szX|!gzcTQ>y*mds^H)OSe}1gIQ+>#*BVF!~F?lO7T_4$CJ?^1A z{?3Yi@gHzsNx$>&Ezji0&+18No0ltREmcTnPuuUSQ#EP zzmN4;S0(!oYxga4dngx9^{x~W*O>24s|Q*lQ-2%S(K6d*tBOWLuC2*C}aK=_WPD?&6RgfU+-G?fS2O^ z{fxdh&vjCEUHEX<@3TCWH`T-+2V1)pz?ER2SvNR~~w~XTYV9MR!(uUW`+f4srLLYV*<{#bfxq z&V{}ED>)Bk1?4TdSJ}Gxy@*A#gOzSepZaN0uj-ILx{Mxja^e`JQ$hL)+k9`uCt>ZC z-`5XS5|%Ez{^~O!N^wy^b+BcW(l`3uKim8qtn3)sY}U_vM=1vmRaIWP+C_Qav)`&| zM~5prwhx$=F=do8{fWG1`weTMR8KnJTK9klzsG(`-MT+M?W60X6n$HB)OsBG%WUN_ z$gihz=0KfcmmxhKEpo|M3$N>rwmhEHXF_mk^}Z&Q?v4QqOpEgh|# zf8v*^``d&lTce+-JhEx9@@lJ+Z{JuQtQ?=cyJp*4M&*Ut*}YzqH2ik-KvC zaN%dY^Ax4_xqGg*Z9hVJ(eTEcCwmN1-r7DYJnh+m%DHIek!Rh4lwmHfUG95ju+nno zQ?ss58>LwG&a7KfIzqV?TVv>T*jIUDTFCdtv%{5{HTkhCr;b((FMro?duEXG<4M0& zgFFJ2$}^!G%G+NF**xoVn>epNO52n78nOoURgTqpbvdQ{5whl+NsmvA@2&LwAUiy3 zwwp3P<)iNc2KQ7vu8%tP!QPR|?d*r=t?uNf#D$0A+r7rQSz3)?jJd3q>>YI zsq!t`0A*{>j@A3O4Nwwh|D0jHCs5fi{mb8%7;#hnktu1`Ppd=vpNnnXBUe?n?rgF5 z`7U)K?|yN(Taurm{Hs;3@YZpD%AA33lzO*r2pQk9+izjhhbwE6o7V+i3|2-ZovJ?b zLLVh(+xm~z92udMZMjF6JYks9<3z_-^VE^blLMA}dfh)%sny4Rxcqpqa;|pIOs_XR zlw0mE&r9{gg;fi_%?{czTG{+q|HDH^3{;w3{_BOH)jgGq?#5RR9tl>WPJ9*WrVCQe zE{f^9`)PmWjQ{6-&pa|fIheAlY|2s}r8?2@Ysi!y%K2xPXC8{V5i;Sv>=TbC+zwfC zW>M$bp(B;Wxyn_qF9s{^z2@&o$s46~S+})3aoVkrhx-?FUAS&I(vjRNe#706=XGlH z$D@ZTKCiWF_M6WLZbu`Oi81c0H@8rgp3^s6*fng7(mK%Gruw5GC9dlatH+L0l@S@E z9%>mkTKT2(_L)zN9j0tP^|Ah$LnD-K{~9oQRZu@A@R?3+^4gA3?oLfE_$g$F(&St#4SFHU8&5b<-PkWenSJbsj)Sr$Dwc#Fm(Pg`ReF9CdHK6{B9xuqub3C| zSbwGcyF2UWd}LI9UV1FfID3d<>G$r{i@E)k4VN0O&5uwO!vnSzJH8&T6mOn32Mm{o!48!57u<4SD*mdLiohx4LB9RWp9o9~^!ET{XSSPs^VkeOJ9l z7|=>R{@+#CK2dl6hhOighq@R2q(5{=J)N_CY0QUr)U_+`>0R^89d+2i;_06+yQ6-b zmYMzjoI7gw@67mrN9{8E_L~Fy-BB}2Qr3UwaYx<1D|Aju&26>yOD_2{D{ia59o9dW z{Pk`1>9Y^HeX{AcI(F@?2l`rXtKAMfjQ_XQC&E3CZ-~CFR@d#(uUBDTG-%XkeQ&Fk zB{?oF+T2#H1yB5$d;OMrqxpt>|DSKEk1BcbclO>=%XgG^sDJ;K+PlHW|DI=Wsn(%C zh9o_9OLhD6>+|R0Z>dwd+CI&hd`msrykeqd_x*ZT9+w$;Wrst3P_pxa|93y(;Bn^=d=OQ=Ws@)T?R59(5N|>(vjJop1f+oO-qL z(hs+{S?bk`17BLXe^|ZR|6Fx*tre3U5Pxu5D z=ugzCqZ=@h+Eu5%dv&p4`}=k3%lZ1+k{9aKiQC?*{`HAEb^9|VJNsqSsXkA{;(wjm zp?TJB-IO}@=dVA0^>E7#u8>)5G=ZoKc>4sXiDJv&p?G4p)|BG$im)}r7 zdS}>$pBCIulQZ?Z56!%xj_>qf#__NlYQbIezUpB&)SbVKU+L$2Lv6X@<>ajPH`LUe z)^qUHd9_naO~;s;TJ^xlaf@%9tyN>oa{6vQUaNLWD(#q5TC0Y1d9zjcN408U+O*?i zUawV4%JW7~cpCh~oQ{b(wQ83I`Vp@^T&vdix^L0%v9;=(6NA@GnOdvP@7a1*+1Oh3 zmy)%`Gl$lyrg{2hH+$Er8$KM8_(_LawRy+rm>joS^>#r2Ve@ORtG{`d7tj9fy870+ z-}fx2xUSys^)LUtgV)ukZ%)|1bH{bHL1ZLf!{uLU5#;z z*3Znou8v+DHRg|pudC;#uGsN>+;vsTL$9lMSABW0&xGr0mEoGD<*4iGB)2Uy8wOlg zz3a|hZqfa^x^HjWv%L(!QF{EpuD%rU^St#}uc@Ql2ESeT>oqlE;7?zVtGK47Z0+-8 z@u6$#+4Y-$R=&EX&fYb(=!dP>)QS3~AFO=in!2F;I3+y-gn)+mq@B1VK zTvP9x^-$l>jMvl-!oYugo8zlmz7 z-(OXKd+*Ur&mO$0&gkF!RLoab)egZQwGaO2s@kjUks#leFk=W;i~$Sd%}rc z1>gtev=7R;s{R>v@c8V^tE#2NFINi|URCP`yf*jfgICqkEdN{Gre0OYx37GE`S`2q z$2;$zePQ%f^=9`)f6N(tRo&&~Kkl)SMum8aH z(;C&S^y)L+-lhZruP3=+onCp!zYDDqMG1q>- zqGlAU!GE2*qJA}T(1Pm~SJdt~Z+y`F*cEkFWw+bC_FYl?|1sV)@yjdf`5iS2R(y0t zeeTgNvD@FiqQ2U;va|lxE9%Q{tjnIZ;fi|Nzc6>(+AC^1pPaTnIak!xKXmQ9cG(qm z`TuI~Y~Y(J?>+vU^o8_|rlo}xijA{s+=OK%YJJ&~Qot}lLa9(eMvAEht!)gg1l=?; zbX8F^F79@B>Q3|0<~7s|vTf5{X$#n6d{i|;LzGS+w>7F}>(voT9p)-%X@q}c0?8rjbr<0OtNaHBJeNZyl zzMLHV=|hsKwMe+o^nhe~y`%Th=q|~$Q?poo`c8cQb?TgN-XfX4`-e-5&)O=P{#3dB z!N;zXOvl1UUs}0YGTqth5`TN8WXd=G=O6xcwPZSApZ&t}<&sI0--Lficj=BNzS}67 z=6<2>v9B5=)0^jPJ7(8QCUw5*cC$`0U2ya2^Xs)pFCUW0gBOYBt0nq7296x@9J~A4 z&lnV={8M>;VQS`lmVHH8UnO!U$(969U1>qo|^ z_%SsT-RsJG;SHQq&$AE7pOebr2G6?b{nT^+8ad6J`dcQa*CHR4N9D=o-;dbJkL&P= zDYxd5Ueq^?1(6G}wNOv}4e}$);TQ1-GhCFj)f)^f8n`B!qU+b7+BcpSQ ztr<2qY&uXWcR86@|KxOX8R8_HRW2h08$E$juC=z&wW-PJ+9+XqvQ5r!WRk5_&Mz}5 zzg9V27B&%&iWlN#|2^&aqcqw@hdhV5xEMAidp3Ntz(%pie&J@m2Y^>#9!vlWojievJaE~j zmL*L}6y;n_`9mlZSlK7=KFC59D{N31gnWVJFUH;}<%O)=Uc}0cYF6G-;>^xfo_8KV z+0R$PSM+J(`w-2n6Q6lL)*{LW`O4I6 z0#e+fpS}W|5~{hZ3YKX5}u_0WYSTjWTBMF1Hr(K9Fm(r_lp77cll8h}8zLVnY78 zGG0&P^_Z9Xb_52tZ28&D$}L1PM7~5kyjCB&;QMb?Kpq`2c&r#=Z}6snhSCM(g|tg zK89G?9D-fC%9wjxuWE_0UJ(4O+xz*uZ8!+DI6mJ?&6 z#!$lQj5o46;Tooq;8mtLt@LiiH(efl|I`dNw`VPzYrKNZ69+!$DC%wIK~*xwf7^NkjRETo_+-45NZlhN7U6- z>8!A6tg=7$x8cuQ@JBlCj}z-wrfdKCSK^)w-z2&A7Yg%Op%HT|%`4QGjhft;%*;%q zU2Mge|J1Cyrs!yjyibNswei@B_44(8L@XuE2qZG}o!ou!yi-$0+hJXM2vss{elG@uX6RI!;2 z#RfrJi19fCbpso!Y&8qY&a3OGV)N{?*c`OYtVJ{T7E9`EVJ1IKb3e5Wz$zxp^Etog zWBJ>sA)n2)M}dkJ^a#Aq&P3anp}o-uc59qd>-78EaqqN)v9-B6SEyNqv4T~&%J4|F zC_TejXrpq_mDs^AV+`)Zy>zaeJJ9zaxzE$JZ3FVQK#sR@$h|pV zDhpX9+P-qHJQ`&`XpR|08V~+6S%{~rsViV*!riPeo1amol~~a`N>A4pGyU6lFkR1` znBTv`G_8G1ZM=(l?qTew5Vl#6_gtI!Stu!BCH6d4g8o;6wyv3l?`EHprx}+h-?PQ6 z*p>U9t-$!uu=1BmoM^)g=pcMY{a>*tHBXwjJ_X zt_|lDqrc5zbM{W23u-<|-_v@$LQ^;Gyd$9=H4BQ^yjHJjp7B1_JmGGY(n>s5xPPhn z4C>VSHC3H)kE%|%PgN;tvdZ{X(%6tj>Cb9b+){Yl$H(y>UyM=4-p%<)gM>J0>eNhQEN2?miF*qgj|Sy5`a~WpcBtfjGTj*X z$&<+AsS|A^s8vk8JKs=q7QUYyXn)}}R+OcYR$$0SdsE(2v^)=2qTVIwkB$O1;~Awu z`8!L6^K{?11DvO3^Dx)U!(1~DbM?H2D(j?s(g}0-`tRZY%RtUX0mPzd=92{|-$sHo zMEU;H0FqDC7ZEo(ew#u0`Z8ra;6=o0SH3gPrraUQ{^6dNuA_tQ<8w#j7U;atyo9p_D)yEsM81m`oHqny9yJjSWMUMaVVQ^#rKT*A4Q z)5h7!>E^tTGr;*2=P2jzIaNG<8qQB}8abD6uI1d!>EztW`EQ&t&NSx_Ips#fkCyz^ zy&bOY)(#goFUFq1S9dgb_jX|;ZFaf}ukvs1b-A$NHGiZ3ayvV7g0)rNB^XBb7FpM9 z--@m1TDRQL(X^HJBp{7-=bhL&cFl?{TYD_nSeU(%-_q0B-0sBQc_pfhWgeYmga+P0+&CEk&@eCv{q&Ar!N+u^cevvxSi(gjy^ zIoiAIogMb2?yIpe9(EppBUBl7TLHGUJ>}^`vj08p*y`K4yz83ob!->%=e6#3E$g6N z{yK8y+)k-icH4V9J1lZ>Si*AI(w57Yu2|Ht&`vwpzfK-2u43nQySY-hjJ<)R(o zo5+_5pBsgx6!9q=>oWFS-inU)n>tRiR|!kxP3WhdxPC&cr<`l*vafM&>5|))4HvW? z-z9i2>2Ock3HVX^bbSBcN9dEhYw$jSuwu(*Y`VOirKmm>oF=;+p~r&y=kr!{x7+#o zrEbK-k>Aq3WoyUrc3|vPCB}9oWZ1F3a2<9MmUpjUUpf&{>N%HVdkN&BBkSZI$gUM8 zZnyhzK`Y;60*%9W;PsryODRhB=QV-y>b?P89Da6nG_mXy%R06?51rGgY@d9EtSeh4 z-zqFapKHeE^c`%ctS{SwU98AcIqwH#+vVuQWLn2|qwgS|TkOr<_6{p{GiU!!Wi>Th z85=;PQ(L=_pJz>!$I>qB zOyAu_+tDlAbZkY$9+lf^x>NPk=O&z3+wR)ZzL~t;KxfwU9B-F4%AG1gJ9o4HqEAWz z^ciJ5Jb7;sSW3k@LJiu#AF8C#bt-D(}X zi^;9VyIj4}<9N<+^yA~C2&nnr*_U6wY}G|qV-p#K*R^5gnzhXv<+S>V=bfGHJ@mD) zVI9Ut_qMG_VwrfJ_A%YyKHmttZK8c^4@P0T%f1Oa!?mN^Z@AE)%#B>-@9bH>7 zi*G%ia@DeBa#DIeLEp?IxP4=v*5L;o;|gBfsC(&}m;dsY2H$H7zWyo2MnCDg6Q7i8 zCw`{$r7)-`8b1_Wc``HIPo+5BDUT1uosT@zQ*ZpIxN_=^={|B-KhgrH>k*1gapRQT zLidrn`B4@2Pu@-Fsq4g01@JdU+m7u!PUptK*A!(SsPOi0X6Z2sGvA6GXK;j&{%QXq zzm7gkbKL&EVl;C;_E)BF;^`kp`XBiY`{uG=`o?@4PAcJe{(j)jy!(kVn_Xhj@;*|?bX|#JM%F= zln7|>`y7PU7ZAJf+}#P?33?!I=)|vc{XtNR=kOlbiJyc}nsdPpt~m8Vg!8`z923$ zpg&zDuoYk!LcYbo2Ha}o!)^pc(vhAxxCr-3u!q1mAx7v&z$cCPO9S*YcnCuAc@=z@ z>*L^}^Ax)g?BqJ}+gyJR)Ss{TM(lu4xeic->_Ps-_6zX-3+PVp+(wKY=){{Ka%{mL zb6tHQV@n`%dx7_I-3va(^%3y=ix5ZTvjlWOQqX(ApFt?kF>n^v5l}w0;5vwYqrlq0 ze}Sxkz6V_LS^P>&`GD&`hrSJaCpZX^V9$VB6WRj$x!@{@>>Fs~I`JV0l^X)bxK3;~ zE9u+7d%5lfhapas^&EIclX7i6cr%3Z-wD<&R{D|wj6=Lg|0+1InXwr3TJTDU_6z8H z-~c28djQN^f-wv|A3O+o5cr~L zD0bp25b|>re2?qLz;&NTOyQ3WoOv1cp@FUe7eX$9ZUDDI=-M%enb@OuIDbHPW~pr1hB2R^eFe`SUK0{F9a zjMZ+!cn9BaV_29aAeiXe>k(V%`@su0zz^t`fGBKHCCv>wH09T@M$ z-}R}in;6@>P4WL#u+pvMc?ozJLe~;owlj7F>07~XLCBx|;O<+LIeZWJI)q|!7(DHZ z%C$Q17)0BSb$Q^DFDYq=cXNFg_!8G&2G6`z$zKoN1)+NF0v)$0`4CeOIVRwn5DoG_ z0xs|<`5V9k5NekZ@GykxB7x4^l{|?t2<5X6`~^hzAJpzZ+aXWlMo1rY8+adt=D=Za z?j4xlV6O!qhwOu%0{gL!VH7&?9Y_rNIOzI{Qf?3U9SD^*0{*HGcEolJY`#meUj-hz zTdD71u=*awt_7EKeFf-(P`xIucc66;f4L9WVl4!5{{84%&~@Nj5UTIn;BGHtSHUiV zjyw5dy$sz7UgF2tyBhry{9FLnLN|j~1{M7(@P$Fd8g}9n zLyVO|Pl4Zv;Cko}f-9pa1G*LTLMZ0M-$CU50A3v9^AFey$s)ZQ{B0b*L2pf94GV*FoO}{v1McV-`FHAs>j}eo~oN_k#Tn0~v)*^g`%+gm~7o$P;!mXoX17z2Fm&ap)=Vz;_vL zOwjJ&yAbko9IXBx@3-I{NEOl$ALaIa-~kAw*NxzPbdXDshSp0w2_btLocDdi2X+H^ zIm7|o3hsnZn-bSPkNY3k+raA~bZsZ7{sHFW(uuSVh;$;Yw;`QKYhOqw(pnMHiEFq{ zYePI-M{E8=kI&U%TtcpeTmV@DX@%G!Mo1^b0`Wkc5HrLIu|W)wKFGx5v@3Qw`iK2#f5xBnOMVv61atvIz#Om!YyoG$ z6YvJaz;GZP$ON(hDZql7pe|?#nuFG$E$9q-g5IDQ91f;~nP4_31=*lxP&a58G!I$_ zZG+B1&!Bfu92_1@4`v3lgVG=y(hTW_3`6E2>yT~8Ipi7g4v9mxSOEUXFZ!iKOpYz^DO&afx!4U6I7a5|g`XTwsM zMKlpz#1LVU@+fg!WCQ8}vs_yT>gz_0`%ve8e+acsq242?`6%i>?pFu2sJkAuZw!P` zrxa?X8PuXadeo>9b+VvVZK#(6HFKkGeW+bO>K8%{Q>fzzYB`E}j-jUGsH+;a)uO(7 z)VL9KwxHH+a=qQCc^~TDkJ^V&{}duHf*6b<3S)@FI3l4&EVPJ*9`R^IL@bC&8=~Ss zT-=CEA7ayw=!6iT6e2Vdq7i^O5$!MXmQMRdd>P-UFN>!Gl5gC{{A$0(ul4KvdcVQn z=yxEeKI9TY4kMw_&{$|Zqz-Gt`fy{|5^f7S!tQWixIY{Ur@|xQ(ePM!JgkmrBl<{V z#1d(XI3n&yU!*@0ilibVk=#2~N*obK#W8VQR1ate^aG6p zmVveb$AEjFZ=io5G>{q?85kWH8yFu@`?MHkjXsO74P(rW@zsy0rx5W`M0?z)Mx6DC zvBlr!ccAC>$-ORx9yjVAp)WEW?y=GSX|#G4EzZ!|I<&Mh z0-XPE$GbUVjo2d2h$rHWh>_t)I+BTGBT|G#HBnvE5H&}wQCrj*^+dfElE zREn~gCZ>xSV&<4NW{Wvvo|rc##)f0*SSFT@Nii1J#C35)+#I*YZEOwM%0N0(JWd;o9GlhqE{5fVKFUc#H=WZ%!=P7kc{S|etEVr z%QJ}sb0~A-o=(m~hwIyLtrclfn2E**wU~2kLzIeUnd#=3sWS{^^zre}8iD@-rnwU2 literal 0 HcmV?d00001 diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdC/CMakeCCompilerId.obj b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdC/CMakeCCompilerId.obj new file mode 100644 index 0000000000000000000000000000000000000000..210547ffdfe98678a7d1a0847084e922dca75ca5 GIT binary patch literal 1995 zcmb_dPi)&{6o0O>txPNHA0~|nG%GWrJK&{Fn$ZH1yL2H%Nn5S$HWZ`Gjh)V2YzNzE zyPYaJAwof_1PHiHTqZa%abe@K0Y$)NXq=EHPDpU#!VMv$^4{mqsS|F9Cw=exeeeCg z_x-W`v+;T_)Q7KDe@Xx(AvDGC%xbV|@*$i;upmSu-XH7-#W0te%kq4b`5D1x7f*%$ zGm4=HdPKthCbmisKtc?I`><0%{ZjEdiO*p?%Q+#F@l+)+*8|yhqSze-uOjaxo`GyX zHVNh~DeIl}dNFQt4)MrTNU2JrlXnQ?HuA1=E^UGFs+X_$*$ho%rlNnps~#N0wI%7) z)Y<&xVsW83eP$wwE>~Jeb_QpQQ!|sLv-3%bD*S=%2W;CQ3}6Sj32o6gJzty4X;W^s zX`4>qYm?2o?rUSZ<7y+mX`6oFncBH&tyb5Ay6zZlP+7IEX-iG3-WAG~^eblGs++!+ zRz}B^l$LYt29CXHBd4)!ZZD#^ODiw|&kw*7zylb9Ct;@v?Y?AHJ=b?@fi!&Xq%_+I zEZe$fIm=R1feylLdL}GcSYG$4m&@zf zOn2N5@a(#0R1CAGH|s%pJQbtt)yKESbbQNo{5EqopAV_{*g(Ly2H-G5DtdYn0$fBt z#Lx!%BMfyMr?QF<3age=%tpqQ5R5XAPK{(v^GFK!RcI~9{jH+hTiE>l=ywSj>p`J9 zB};{^KEj9ZCWNg+viSYhSFMEn-!pIjPt~??6Pw>0-FX7{_6W8FWYIs$&>Z@HhE~wi zf(f{eevqLX=pSQ9=o6qXi4cuG*2o@5AmSW=5YY!p>xgjTK<&uW$L<|_u>0`umnv_* zd?Nkhr|P}ryXJ4-e!lWv>-dMC{rbY~TiKy^J}$L>x&QU}px;=$@$kZJoXkw6cTKU3 zon}JV-KC0Z&C~;Y4%>pT!6U0~U9k=5!5LvBE_|e8qCnEuX>rk`nW8|l*J*LVBk>Rg zl01^RJ8xb5YmZAOCnHV|$n!5|N77?Jx4>pWTo8#X3M7eS9}Il-REWsaeUlM~_5^So zn}jW1`B@m|5sCslgH6R2<784$;E|AX8b|a2#5tpIse^MKM;koCPm#&M8*NTpuIUa$ zf!+ZU=`VH%?qY{#>sDLN_u z05hUM@=oU&ygx+S;dY((i~~wc>S+~GPZ8oAOlH5X5dQ=1OLCb2 literal 0 HcmV?d00001 diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.cpp b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.cpp new file mode 100644 index 00000000..c9ba632f --- /dev/null +++ b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.cpp @@ -0,0 +1,857 @@ +/* This source file must have a .cpp extension so that all C++ compilers + recognize the extension without flags. Borland does not know .cxx for + example. */ +#ifndef __cplusplus +# error "A C compiler has been selected for C++." +#endif + +#if !defined(__has_include) +/* If the compiler does not have __has_include, pretend the answer is + always no. */ +# define __has_include(x) 0 +#endif + + +/* Version number components: V=Version, R=Revision, P=Patch + Version date components: YYYY=Year, MM=Month, DD=Day */ + +#if defined(__COMO__) +# define COMPILER_ID "Comeau" + /* __COMO_VERSION__ = VRR */ +# define COMPILER_VERSION_MAJOR DEC(__COMO_VERSION__ / 100) +# define COMPILER_VERSION_MINOR DEC(__COMO_VERSION__ % 100) + +#elif defined(__INTEL_COMPILER) || defined(__ICC) +# define COMPILER_ID "Intel" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# if defined(__GNUC__) +# define SIMULATE_ID "GNU" +# endif + /* __INTEL_COMPILER = VRP prior to 2021, and then VVVV for 2021 and later, + except that a few beta releases use the old format with V=2021. */ +# if __INTEL_COMPILER < 2021 || __INTEL_COMPILER == 202110 || __INTEL_COMPILER == 202111 +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) +# if defined(__INTEL_COMPILER_UPDATE) +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER_UPDATE) +# else +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) +# endif +# else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER_UPDATE) + /* The third version component from --version is an update index, + but no macro is provided for it. */ +# define COMPILER_VERSION_PATCH DEC(0) +# endif +# if defined(__INTEL_COMPILER_BUILD_DATE) + /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ +# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) +# endif +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +# elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +# endif +# if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif (defined(__clang__) && defined(__INTEL_CLANG_COMPILER)) || defined(__INTEL_LLVM_COMPILER) +# define COMPILER_ID "IntelLLVM" +#if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +#endif +#if defined(__GNUC__) +# define SIMULATE_ID "GNU" +#endif +/* __INTEL_LLVM_COMPILER = VVVVRP prior to 2021.2.0, VVVVRRPP for 2021.2.0 and + * later. Look for 6 digit vs. 8 digit version number to decide encoding. + * VVVV is no smaller than the current year when a version is released. + */ +#if __INTEL_LLVM_COMPILER < 1000000L +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 10) +#else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/10000) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 100) +#endif +#if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +#endif +#if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +#elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +#endif +#if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +#endif +#if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +#endif + +#elif defined(__PATHCC__) +# define COMPILER_ID "PathScale" +# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) +# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) +# if defined(__PATHCC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) +# endif + +#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) +# define COMPILER_ID "Embarcadero" +# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH DEC(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) && __WATCOMC__ < 1200 +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__WATCOMC__) +# define COMPILER_ID "OpenWatcom" + /* __WATCOMC__ = VVRP + 1100 */ +# define COMPILER_VERSION_MAJOR DEC((__WATCOMC__ - 1100) / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__SUNPRO_CC) +# define COMPILER_ID "SunPro" +# if __SUNPRO_CC >= 0x5100 + /* __SUNPRO_CC = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# else + /* __SUNPRO_CC = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# endif + +#elif defined(__HP_aCC) +# define COMPILER_ID "HP" + /* __HP_aCC = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_aCC % 100) + +#elif defined(__DECCXX) +# define COMPILER_ID "Compaq" + /* __DECCXX_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER % 10000) + +#elif defined(__IBMCPP__) && defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) + +#elif defined(__open_xl__) && defined(__clang__) +# define COMPILER_ID "IBMClang" +# define COMPILER_VERSION_MAJOR DEC(__open_xl_version__) +# define COMPILER_VERSION_MINOR DEC(__open_xl_release__) +# define COMPILER_VERSION_PATCH DEC(__open_xl_modification__) +# define COMPILER_VERSION_TWEAK DEC(__open_xl_ptf_fix_level__) + + +#elif defined(__ibmxl__) && defined(__clang__) +# define COMPILER_ID "XLClang" +# define COMPILER_VERSION_MAJOR DEC(__ibmxl_version__) +# define COMPILER_VERSION_MINOR DEC(__ibmxl_release__) +# define COMPILER_VERSION_PATCH DEC(__ibmxl_modification__) +# define COMPILER_VERSION_TWEAK DEC(__ibmxl_ptf_fix_level__) + + +#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ >= 800 +# define COMPILER_ID "XL" + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) + +#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ < 800 +# define COMPILER_ID "VisualAge" + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) + +#elif defined(__NVCOMPILER) +# define COMPILER_ID "NVHPC" +# define COMPILER_VERSION_MAJOR DEC(__NVCOMPILER_MAJOR__) +# define COMPILER_VERSION_MINOR DEC(__NVCOMPILER_MINOR__) +# if defined(__NVCOMPILER_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__NVCOMPILER_PATCHLEVEL__) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE_MAJOR) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__CLANG_FUJITSU) +# define COMPILER_ID "FujitsuClang" +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# define COMPILER_VERSION_INTERNAL_STR __clang_version__ + + +#elif defined(__FUJITSU) +# define COMPILER_ID "Fujitsu" +# if defined(__FCC_version__) +# define COMPILER_VERSION __FCC_version__ +# elif defined(__FCC_major__) +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# endif +# if defined(__fcc_version) +# define COMPILER_VERSION_INTERNAL DEC(__fcc_version) +# elif defined(__FCC_VERSION) +# define COMPILER_VERSION_INTERNAL DEC(__FCC_VERSION) +# endif + + +#elif defined(__ghs__) +# define COMPILER_ID "GHS" +/* __GHS_VERSION_NUMBER = VVVVRP */ +# ifdef __GHS_VERSION_NUMBER +# define COMPILER_VERSION_MAJOR DEC(__GHS_VERSION_NUMBER / 100) +# define COMPILER_VERSION_MINOR DEC(__GHS_VERSION_NUMBER / 10 % 10) +# define COMPILER_VERSION_PATCH DEC(__GHS_VERSION_NUMBER % 10) +# endif + +#elif defined(__TASKING__) +# define COMPILER_ID "Tasking" + # define COMPILER_VERSION_MAJOR DEC(__VERSION__/1000) + # define COMPILER_VERSION_MINOR DEC(__VERSION__ % 100) +# define COMPILER_VERSION_INTERNAL DEC(__VERSION__) + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__ARMCC_VERSION) && !defined(__clang__) +# define COMPILER_ID "ARMCC" +#if __ARMCC_VERSION >= 1000000 + /* __ARMCC_VERSION = VRRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#else + /* __ARMCC_VERSION = VRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/100000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 10) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#endif + + +#elif defined(__clang__) && defined(__apple_build_version__) +# define COMPILER_ID "AppleClang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# define COMPILER_VERSION_TWEAK DEC(__apple_build_version__) + +#elif defined(__clang__) && defined(__ARMCOMPILER_VERSION) +# define COMPILER_ID "ARMClang" + # define COMPILER_VERSION_MAJOR DEC(__ARMCOMPILER_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCOMPILER_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCOMPILER_VERSION % 10000) +# define COMPILER_VERSION_INTERNAL DEC(__ARMCOMPILER_VERSION) + +#elif defined(__clang__) +# define COMPILER_ID "Clang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif + +#elif defined(__LCC__) && (defined(__GNUC__) || defined(__GNUG__) || defined(__MCST__)) +# define COMPILER_ID "LCC" +# define COMPILER_VERSION_MAJOR DEC(1) +# if defined(__LCC__) +# define COMPILER_VERSION_MINOR DEC(__LCC__- 100) +# endif +# if defined(__LCC_MINOR__) +# define COMPILER_VERSION_PATCH DEC(__LCC_MINOR__) +# endif +# if defined(__GNUC__) && defined(__GNUC_MINOR__) +# define SIMULATE_ID "GNU" +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +# if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif +# endif + +#elif defined(__GNUC__) || defined(__GNUG__) +# define COMPILER_ID "GNU" +# if defined(__GNUC__) +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# else +# define COMPILER_VERSION_MAJOR DEC(__GNUG__) +# endif +# if defined(__GNUC_MINOR__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +#elif defined(_ADI_COMPILER) +# define COMPILER_ID "ADSP" +#if defined(__VERSIONNUM__) + /* __VERSIONNUM__ = 0xVVRRPPTT */ +# define COMPILER_VERSION_MAJOR DEC(__VERSIONNUM__ >> 24 & 0xFF) +# define COMPILER_VERSION_MINOR DEC(__VERSIONNUM__ >> 16 & 0xFF) +# define COMPILER_VERSION_PATCH DEC(__VERSIONNUM__ >> 8 & 0xFF) +# define COMPILER_VERSION_TWEAK DEC(__VERSIONNUM__ & 0xFF) +#endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" +# if defined(__VER__) && defined(__ICCARM__) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 1000000) +# define COMPILER_VERSION_MINOR DEC(((__VER__) / 1000) % 1000) +# define COMPILER_VERSION_PATCH DEC((__VER__) % 1000) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# elif defined(__VER__) && (defined(__ICCAVR__) || defined(__ICCRX__) || defined(__ICCRH850__) || defined(__ICCRL78__) || defined(__ICC430__) || defined(__ICCRISCV__) || defined(__ICCV850__) || defined(__ICC8051__) || defined(__ICCSTM8__)) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 100) +# define COMPILER_VERSION_MINOR DEC((__VER__) - (((__VER__) / 100)*100)) +# define COMPILER_VERSION_PATCH DEC(__SUBVERSION__) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# endif + + +/* These compilers are either not known or too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; +#ifdef SIMULATE_ID +char const* info_simulate = "INFO" ":" "simulate[" SIMULATE_ID "]"; +#endif + +#ifdef __QNXNTO__ +char const* qnxnto = "INFO" ":" "qnxnto[]"; +#endif + +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) +char const *info_cray = "INFO" ":" "compiler_wrapper[CrayPrgEnv]"; +#endif + +#define STRINGIFY_HELPER(X) #X +#define STRINGIFY(X) STRINGIFY_HELPER(X) + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__MSYS__) +# define PLATFORM_ID "MSYS" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#elif defined(__WATCOMC__) +# if defined(__LINUX__) +# define PLATFORM_ID "Linux" + +# elif defined(__DOS__) +# define PLATFORM_ID "DOS" + +# elif defined(__OS2__) +# define PLATFORM_ID "OS2" + +# elif defined(__WINDOWS__) +# define PLATFORM_ID "Windows3x" + +# elif defined(__VXWORKS__) +# define PLATFORM_ID "VxWorks" + +# else /* unknown platform */ +# define PLATFORM_ID +# endif + +#elif defined(__INTEGRITY) +# if defined(INT_178B) +# define PLATFORM_ID "Integrity178" + +# else /* regular Integrity */ +# define PLATFORM_ID "Integrity" +# endif + +# elif defined(_ADI_COMPILER) +# define PLATFORM_ID "ADSP" + +#else /* unknown platform */ +# define PLATFORM_ID + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_ARM64EC) +# define ARCHITECTURE_ID "ARM64EC" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM64) +# define ARCHITECTURE_ID "ARM64" + +# elif defined(_M_ARM) +# if _M_ARM == 4 +# define ARCHITECTURE_ID "ARMV4I" +# elif _M_ARM == 5 +# define ARCHITECTURE_ID "ARMV5I" +# else +# define ARCHITECTURE_ID "ARMV" STRINGIFY(_M_ARM) +# endif + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__WATCOMC__) +# if defined(_M_I86) +# define ARCHITECTURE_ID "I86" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# if defined(__ICCARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__ICCRX__) +# define ARCHITECTURE_ID "RX" + +# elif defined(__ICCRH850__) +# define ARCHITECTURE_ID "RH850" + +# elif defined(__ICCRL78__) +# define ARCHITECTURE_ID "RL78" + +# elif defined(__ICCRISCV__) +# define ARCHITECTURE_ID "RISCV" + +# elif defined(__ICCAVR__) +# define ARCHITECTURE_ID "AVR" + +# elif defined(__ICC430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__ICCV850__) +# define ARCHITECTURE_ID "V850" + +# elif defined(__ICC8051__) +# define ARCHITECTURE_ID "8051" + +# elif defined(__ICCSTM8__) +# define ARCHITECTURE_ID "STM8" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__ghs__) +# if defined(__PPC64__) +# define ARCHITECTURE_ID "PPC64" + +# elif defined(__ppc__) +# define ARCHITECTURE_ID "PPC" + +# elif defined(__ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__x86_64__) +# define ARCHITECTURE_ID "x64" + +# elif defined(__i386__) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__TI_COMPILER_VERSION__) +# if defined(__TI_ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__MSP430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__TMS320C28XX__) +# define ARCHITECTURE_ID "TMS320C28x" + +# elif defined(__TMS320C6X__) || defined(_TMS320C6X) +# define ARCHITECTURE_ID "TMS320C6x" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +# elif defined(__ADSPSHARC__) +# define ARCHITECTURE_ID "SHARC" + +# elif defined(__ADSPBLACKFIN__) +# define ARCHITECTURE_ID "Blackfin" + +#elif defined(__TASKING__) + +# if defined(__CTC__) || defined(__CPTC__) +# define ARCHITECTURE_ID "TriCore" + +# elif defined(__CMCS__) +# define ARCHITECTURE_ID "MCS" + +# elif defined(__CARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__CARC__) +# define ARCHITECTURE_ID "ARC" + +# elif defined(__C51__) +# define ARCHITECTURE_ID "8051" + +# elif defined(__CPCP__) +# define ARCHITECTURE_ID "PCP" + +# else +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number. */ +#ifdef COMPILER_VERSION +char const* info_version = "INFO" ":" "compiler_version[" COMPILER_VERSION "]"; + +/* Construct a string literal encoding the version number components. */ +#elif defined(COMPILER_VERSION_MAJOR) +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct a string literal encoding the internal version number. */ +#ifdef COMPILER_VERSION_INTERNAL +char const info_version_internal[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','_', + 'i','n','t','e','r','n','a','l','[', + COMPILER_VERSION_INTERNAL,']','\0'}; +#elif defined(COMPILER_VERSION_INTERNAL_STR) +char const* info_version_internal = "INFO" ":" "compiler_version_internal[" COMPILER_VERSION_INTERNAL_STR "]"; +#endif + +/* Construct a string literal encoding the version number components. */ +#ifdef SIMULATE_VERSION_MAJOR +char const info_simulate_version[] = { + 'I', 'N', 'F', 'O', ':', + 's','i','m','u','l','a','t','e','_','v','e','r','s','i','o','n','[', + SIMULATE_VERSION_MAJOR, +# ifdef SIMULATE_VERSION_MINOR + '.', SIMULATE_VERSION_MINOR, +# ifdef SIMULATE_VERSION_PATCH + '.', SIMULATE_VERSION_PATCH, +# ifdef SIMULATE_VERSION_TWEAK + '.', SIMULATE_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +#if defined(__INTEL_COMPILER) && defined(_MSVC_LANG) && _MSVC_LANG < 201403L +# if defined(__INTEL_CXX11_MODE__) +# if defined(__cpp_aggregate_nsdmi) +# define CXX_STD 201402L +# else +# define CXX_STD 201103L +# endif +# else +# define CXX_STD 199711L +# endif +#elif defined(_MSC_VER) && defined(_MSVC_LANG) +# define CXX_STD _MSVC_LANG +#else +# define CXX_STD __cplusplus +#endif + +const char* info_language_standard_default = "INFO" ":" "standard_default[" +#if CXX_STD > 202002L + "23" +#elif CXX_STD > 201703L + "20" +#elif CXX_STD >= 201703L + "17" +#elif CXX_STD >= 201402L + "14" +#elif CXX_STD >= 201103L + "11" +#else + "98" +#endif +"]"; + +const char* info_language_extensions_default = "INFO" ":" "extensions_default[" +#if (defined(__clang__) || defined(__GNUC__) || defined(__xlC__) || \ + defined(__TI_COMPILER_VERSION__)) && \ + !defined(__STRICT_ANSI__) + "ON" +#else + "OFF" +#endif +"]"; + +/*--------------------------------------------------------------------------*/ + +int main(int argc, char* argv[]) +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; + require += info_arch[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif +#ifdef COMPILER_VERSION_INTERNAL + require += info_version_internal[argc]; +#endif +#ifdef SIMULATE_ID + require += info_simulate[argc]; +#endif +#ifdef SIMULATE_VERSION_MAJOR + require += info_simulate_version[argc]; +#endif +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) + require += info_cray[argc]; +#endif + require += info_language_standard_default[argc]; + require += info_language_extensions_default[argc]; + (void)argv; + return require; +} diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.exe b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.exe new file mode 100644 index 0000000000000000000000000000000000000000..46269a789c19f38661cee5cd05f7ffdb7a68f107 GIT binary patch literal 96256 zcmeFaeRx#G+4#SkWRoRiIYC(rBCI5{(OX6~7}XXc*!ZDy-)T5HR;*=#vHsg%vOp11t-tH1yGPamI0jNdrI_VlnF zXRWt~cbqlnj;}BGEM2zbYsd8eNI_`W9f z{^Y*DsrSMAp5T2K>AzF&L+X9)*B9I&uutwuO_j}d`vbYQ#U*zy$b9RyO|T8m8Zpkc ziImM&YSF2@3su-}mzRXIY_?pLH2Ai)@Kun=zieA=eWp;lEU6Zd%uDj6QzU3BS!B1B zs8IUPTDz^zH6-CFyDdnik6&-MoiQZ&1ZTxOfR8DLAr$E1Q$I z)Ia2(-)5UOB>Mjc!GIp0Q(PDi7d!RXdqwRzYFX+Fg2f))aCOhO+qCC%>_$Oxc8Ad# z&neD)+4-ekp8Jw730LzT+m}FF`z2)z*L|P+V$g`#=At(Lbu`$sK?c~II*vbIbJ%P~ zhj}lH$ebnNedeX-*=#CDyvXce-ZZpCmYZ#PbUNYc<7F;jqMK)11s@x4vo*FvuT^n# zA&HGGkpgmN5i~2L3Xg=w%AXG?NM+u03FndkWQTbt$TMFQkUwM+0xOr6dTwspKvg#L zGO}&ozMWQ=m`O5uTliYirs8enH-D^PN@C9};7Z>3EsRS%Qe1cg*~+ZW{)H|#v}g@e zc>6Co>~D$`d(5vAY2IhJZo9*7i}l;JhKVFJc1Evmcg!G(2z_=x*`>PvbzbKAG?otL zuZQN_diGlxW>^`@B*U*K+HCEPVU(5?mvs8q*;QT5+swyFF=x?S-Potc%59M|b&zq8 zTEN38Ro<3Bq9)7RsTM$>&n0L>g?B*0&W3(m5kmDRHhp+D5VvmhHM<>ZA z-MFRLuN#qK9pskHPmjm1+4a_1L-P}Y#x`>@5ZCg4n^J4lxZDhAH6Y85r&8&$dyLTp zac3>mjhsnkf;U5VCLOx3BQna{(yMEKa&<$?qaR#i4e(R;;QZHeQj438e`NFCDFI4a1X z#~ssnNmSlMXY$(WaP!yxl-+8=23l(~7l9mwlcKQEy1x1*n{82r4c?vR|mLko`6~fBkR@Evg6_U&TZYQb1Oc+TaWrO=+C56%0{ z4{5A-4=~fq|+~0UX=2}&e97P!CKB-uRz;Ir8SP4G0_#QB zk8}||TcY6V);4cT+xGP`X_CfHPH(-CkVI*F?AasxpO*4xLT1EDSYCBux20v_xP^z~k*qx!>&& z(2X7PF%d4V(c_I$jWk3h2G&0+wZ>W?>5Wa&&N|3`rbIkgT-zQju5m-vc`J@7<^CBk zV|{i#!55t@emzmut33ing2lR2nvhDV&7X3An;=WQfNAbb=-KA;&@A2fNpTm*((5-1 z&Tehn-s4rIOzlGg;C2B(w=dsdtzc)gOkbc+)w4U&YxbPw z$zj!5D~{>RxVwgt%6vH7u4seoU$S+1bYqNe^w;|qEWfimdPeM?qMYbx<2p~`{_al~ z|2x)~wZdKhsm=c*d&DJ^uT1TYp80L2?7e!tNH^v?&3r3tzH-hl{Q|< zQ`mN-L%At;jSOk5&8hodU)G}l%Uqss%_pl}iE#%4s_curhL#=mAIcWWo?IjV3cW4f zPVE;#qqWL=AZTn0)c4*MS-3b*|Iv(vcSVE79;4UW8AwdGR}Jsbei5iY`1QLMhTx;B zhQ9!Aa*Tc^j}s28AX=lxT)&2QPfhB*GNo0v`_mwZ+GMOptG5 zm*36bGnOw0yuC|rebb{4?-IU}VGkC2CbV7N7QMl6Y$bQB!`bfmD*;)jamQZ>1rE1) zBIU6TPrKtML>Xj0{Fo4uXm(d{;=Z77Uz8aDq~h+W-BDM}bq`e7!7SLG}U7%O$H@m1QT#}`ve{NCbnpI3`DgCXO%o}Ds+M{~lyUhV$> zArBF!L}EWB61(?kp-pS}z7;I6M^582JSCJQl)X7*{3F%X*s^N%eUbXrHnlWL*kH!6 zB88<&xC#lG_xw(gw-Fmj2Zs0vBj8kv@m4s z2pZM4Q1**LDoXfLA^}gz9or1hm&l#k+A~=QWlPT%Acu@Ko}e*2RQg%K_mLL+JxNtY ztMO6*%9p*z2=2AI*hD%J>EiM=LF3k(K=CV#PbFE08`BMVP_tbuG@#GDi(R}kFQqBAMlm5Wm(i=M?Uu!~U z0+?Q3p+@bx*xeAcY9rvez~gv$4D@Uzb1P6LWau(|K}}BMp2$c&dv>{Qgi6fwKw;98 zJoB)vFalK!l<0<2phEiWy5VO(e{{3ZtAotb?^5_lhq&ps+aZG2E@#_WUf*`JdCINu88Sro5vHr3>3zN@&T>!hbgaj-qY zUcfNSDJ}}KehWkGtun=uPHV6it<4n3jJKLIhNB8T4EFo3!|(vUO_7O1q?Nd0LwtJS zvRvJ#J6Ggw5H-_|bFK9uGjSx0*b~C}A=5a_W{;YUOfa>6L?0#-+o2|%+09pJ%Zak8 zCsvtN!)6xEgS@PzM9O_?3zWn4{w%uMD zam~g$$|kO^H+Ora^dI>qGAEykb)(CiVP#{>Z)=~v3dRP8=QSNTF2fZguXm4GEHxq2 zZAQE|cBR~p{9WQ(EpC8aqnvKQ5~#jGa{B>d7O||5g8&fgOGPGu$Nz#uMQnYU57a%P z-O*2i)Cd2QXF6$QTG~@^3|Fp{?N3Dt%uy7HxjwlDWDF1y!6Q5;)jbcGjgMzF4 zj_i|cdgFn}@R08{t?@^ENq)21c<#umkA(3J8M^}OMG=>BM`@ika?$Hk?mS|#z8j(! z1QXNq$OVRfB6@=H9);i0AV-x>xhn_Dof$H=(%zZU-Ul00-Fj?GPRRFO&)3w*xAFCY zph7*ntcLFov?&W0~ObdaJT)0gN-wZq4F1<*Hebmo-v1CsLW;ew) z$nFlLzgdu;7f1mh0RvbCnZoxC0hF%*4T19`fx|QsIE@yZ2Lw*Ps83FWGi3;zKRH_n`%b1@ttpESa)?k-xl#P*rY?;pUGbocebrH@+mtlN51FArJO&9 z@3~Sg19u2WDMr?5S(FVn`LhP=UxzM4RZZ!FOs(k(Rdn6ePNGe^GY#6x-IlIDUA2m? zo0g-X=b#zO*J-4-F2BjiCuuKO@vJ=UkhUcM&@^qyoUC3{LBj~HnPcy5(|x;_kGLjL zkQMTEYWD{ivbC|wo)GN3cK=(5Z+Jjf5M{97rftk;9 zt%pB1`T6tJ+AYV{4C9&Y;m?hJ{(PmD^yWGM@&5kf*89gT*83OT@;S_O%qDH4hC-he-1bq@_co`3KU{A<}9G($b+N;ru!Q79J)qVvk!fc@b-|V(PWn z-o54c8rzn>m2HdGf`QND>mBg*j^OJZ!Ph(g8+^T!#@9P(e7%##*E|3B_Q$@aL;#i&$|y zvkUoiqlZ6VDI@)AKYy;Rk!-b+Z7Cl@e#uuO`D!KK(llGmJe4Y#r`AigX!#?|QJV5b zuQf+=WRC6|z_~P^IVDRS99gloR!m+p!>pLRh&^S+t%;G-#$d@pw#!jf%j*M0Pnzw zB~PsXq*~UTm{;Vh6Z40qy+K429c6(ux>`yYqL1t{zoo)f%hS!iZ~)-Rd?|1}Y}-b^ zdHGmW4`MwMK3{7Pk}ndn?A!G%^gV=B;;JYYrl_SbQ=?8S9zE|6(~WN;TrGFTR^>Rg#*LB^Yc10g-xHZ7$%Y!j zWDE9%Qp`LskX zsxN<7-k$pMr?YsEtS|p7Z{y{teaqJ&Ah!o6eq78FP!LEfb!W z85CiHSt8GN!*M2ZN!#|gYqWanhPx6KDmommyMUC~8&~RkU%=KPiZrIL9(R3GE&6%G z?Is^mUW%b(l?Qd5rL6OWjeCo0m5{P8Y#bDwM(iA0sNK#m!_V)D zBpKe-R^50wk{61fDUA!Ia*CBA&)b5@Fx8%FY1ZocZ;5(3&$375b8?t>4+(T)>uxhyh~0?VbGK&SLJw##+Wz;X`p8vc5_M94<Z|H$bDs8mhxxvDu$!s&p_q`4r`mfKF;UXcQQ!;oo(UvL<8wl>vu)CH;qIYMi?I$d zjS!OVMM%oXN4Ki`grZlQsXaC(OP61~ny=)vU~$3K}0Zc4`gRF%J>} z)Z_LQqs!em_8u{mawtfZ|P!nW3G-kYaS9A zP|>ewk@*5mcvie%yqz>?4o&O%2H3eKk<+J4hk!SB4nQ}Le3nWDV#npJTmq;^&dPg0xU8&?b(o~k2h}~Ao2uzE6DaGq)UJOE|Rj)Z& znvK~XBalUq^aYx8Sx58r*bWMAq2PtlBLt>aMkZ$z@8(&m zNFTk3Orl<06bc>Y8kdYLJ<~(+D?_n%r}-`Ng|k0N{#N=OR0D6`N*WZyI7DA(D>zEa z01(kJC~ZRq#ci_rVPNtoEr@Ag9EF{L#=LNf(vmhvN_*0mo?nX-ijVx2G>w-=W|!drNk zh*6WbM@BBYjA|ChB>a0CQ#;KgwAq>gky$5$@a_p4c`Stp=2Njqn44IZls;P))HZYJ zWI?BSfixauS{>3Fm$PDeZsU*SDdc0%HAJLO!ao_qbf4>fooVPW_sZI{IHSRDw^`qw zIr*DoG9LtEXX_P?iwQ?%9*;3kXIayxVP+>K`1~%``FzQ$04uoY3TLoj?v*wrT%Snz zA=N%BJuFojuD3~-LOoMe)zVhSen|sp1>wJ@!&3jB2&e1+F+@Ps9~!K`S+d3*|3_X{ z|9X~_ZE#!%Y+S-idEeKKPxVq%A^X8&p7Dv^`e8PFjIPZ%#)wBwA2gGmi-k#$_lfJh@EhaH|`)DZx_B5Z91vt(`U8092X5Naal%{0k z(G)!uwbak8oZseU{z!%|;r^MG^j%4MK#lNpr(_?o)p0r?u-{BC+{#PsSY{a-+_j94 zx23*c3`NSvLF|!1CVmn8eqq}AqsG{e`7C<1v~TZ|w2!f`95BdxPc+CE4;tjhWDN3! z8H0Qt%j4%Dh+V5_YgGvormCC0orgEsqa#!9-%Bry z4r6c1{bzze7%sWc#r_|n;3Pe+ioS$#ZzyLUzbMQg^njYhZs zjAT&vX4c_X#&*VzrXo||b1Py;?GaDxXja5+J^-517`wSp?NrHefmrvkB=+ZytYBP$ zNan>{1(jed5fJAhi)S<(&m*c>_8nIH3b%RrAS^8n%lV+|Q4xGlHYmTWgd-t^zKmv_ z!H%V9)^&6kDmL7_jg%AE7K=w37z)n-z3aX(UA7$rszxA3%JcU0OrGUd98WFf$BI7(#WP-rAE* zo2Yw-EaAeCv55(LNdGN`)=?<1-VcOO;%+BC2mxc0T8lU`&@6h_q88(xP<$k`{z0;< z{+6~jW2w!f!@3t4(W%ET76%5yy-;#wK=6H+?qwGP$=?Q?dq(DKu(XBZf#wdu8n`o@ z!tCEzn9NQyN4oH8NIn*aq%1>eeHe?G3`Ow>Bv@X^pYoRz)vLgSW>w&v2kQ$pNo7L^0tKIUN*Rt z`zucf!X4iRxNhW9P(dyXqvXef5>cDH%meaP`*tB;`nE)#QvNSYC=+%q`3pUkN{QiW zm-+h<3f6@dEbtJHv$^G}Kme4bL-;?XNyC6p$ih>Sg2)NRZuZuzue95G7Sa{;0I!DP zGtsEL2Wgn@48^CLkc%R<2SGDT=!FM*Q3Jgw`Pw%aMwmTAJ>#JtoR*C;4cBm>dz4gsyKdkn|t4i1@ z&3vmu|E)q-|AdP*MEH^_p}RD};x!YlyHQD*i!HvdQNp5P&QN9{gyw-y1`;y-%ec47 zWLUPDZ{^FP*=Ale&3X@K{b~#uIbbW9(uJ_yQs3* zB3)%9LEu}Z2H!S}&L1LfA~+V2o-qVVyU4mw7@aQpY`DHcSdUGb&C6y!XQkZ<{UohQ zr9GL>XQj>NWiuaqK5RJZ3XDP`fq#XfXlBKM!$t$(4L4xE6hq3#wtHzI}5#xyftH@3H6V@jSJ4H?5_)dYpZ{sY!)q6)M?K7i4OB;)X)=?2rl@^|IwY znT0c5M^;=#w$3PGfs5EJ``Dmg1kK~$5dAXNIr{jGBdzqt78$3lvtOQ*AH5a9I+W^& zTni?z(XEfsw?fa>rT>LCrjA?x5;^&kQkuHdAo zIcg6EYf!R!!Bua{#xDLS=@t%{B;pey$NyMxA6xSE85&?qB9DXW; zlE!C~(KY`Mf>43yBzBfGgjjs7%swk!&D6xz(^M=mw`5VgNdyUr65Pa$hdLLjud&A; zrx@pT;IpCSU9?L?7;3F9GQX!-co9~ZtwIM*SLcI}8%=gH;B7Ns$>Xh7f2O~F`e4#$ z9s%Wk>i00?C-hXlWO~>gulkpot(zb+=v|mK?Gv}C@#e_Oq=?d`6~OGIqRYKF zgC&<5uH?B?h-;WRg)FS_cQ$s=W#u1ZKE~h}u1?9J%0>(E@ESorG)71HmfQ$CY95=S zbP=Z*uE)qpj`v7_sYxCX&PD?xWZzD9-8dqWWc`l_9(i4e%*3q`Xr`iq10>Kq#*Unk zeO+NdN*BhO*C?p_)E*^Wh^$WDX}HeMG=HUPMdXel&6fiOhAUq+U&^WG&mte~nJUfq zp%6B&Pz9qjzq>>={{mIfHfjDgvjz@@>@I1(m?F@TTa((nUYdFGUV^;+s8sp`KxJ_M z15|8ZjQb_zQn=qpmXmNlQ8m6=atz}B`{biF;!|tkzFrlS#!s{wFZ#Rz+&_$|oa{BS zX<4{;&^im@nE@EG`6!+x;ktk>n>DC~;*Oo?fPH3*ocM8@g~Ee@MmZz}CtyqVWH=J= z0RLgQekS>qAKpC5$6N{G1FV|DmBq5pg=g0jzbh`Jft)YtRg=R}MsD4UO{!5wE`YRl z(AY2F9OhSch&n+$xtCh!I?TVkXr)x+m#q$+{DK5IxUp2!oI0zq5OPCxFu3SP{!94l zzd99xni2lZqCm^HkO0${tC%D=6sgH2PBRD+L75^_2U3f-*xJQ_%vE$VIWV zcso5ZtaCBm?dC`K2^IQDv0w|Q>k=HUQ8~PYl>lmk4yGQd%G!g6$VQAySwJcOu=e7B zZ_X47MUN|Wqi((I3xqG0F4uCNLY*Q|=KxVn;QD^w52cSMTA`Fe0-5ptNt}@82!`h*8FuA1O_>gtEJ0eHSb{ml?W;cRj`hTU&i=5Ku}@6zd-m(5`A}3mC!H zoD*p}rt_3cUs(_k?%x)p;LO2BUK}qs|1BR9;~pkW@SCG2=6RrnD*=n9M-e=W<*|6( zXNJI8FaSrI53xbM`Qec7S7yF@)OU*)4})YBdTcBsWR*Xh{TKr;UketCzRh*B0Ry>g=Ummn&y}2@%b#w`N*2{Jv}2>aB91D|O7%tXNy^H-C;pgAEexbwvJXpKq^rZK7=kR;lXUQwzY zaW`-lbcjPnL$gu^nM}UkI}c2@jd~(FRB<5 zopUncyR6I>Pw{N=7xeUt5x+Z)_$Kf!$SzlgF#=d4x5^-R_XLccY8cG-MoGs_24D>Q z&-c^-$lh%}%O9htr%}P0RiyiRR!kclgURWB7(2`fU(_(x%#M?X;Hzq8TSK6lm7WV_ z-aWEOs3rw5wB4whq{iawCyvEDYb*El1+cT-;_IKs-XOMah?^YgGqRI^4jT(ijPKn&eIQc; zVPlu1g^ZU&;!3i*CRExkn}N|AYCWDE!oHQzmEN)1_nEHE*rnt5E<|clSVXX`%49q> zOH9USf;TzC9Q(j{>}!p_9*$SnSn@pdn$6lQd|iClR+6Ql5{JEF(uqD*CXnL7{o z^ix!wyO1xss*T*R@j)oHWqSG45)8|wU4-@Qxi!_S6==~9D}_jDjWIPTs@Uk8aVUF>c#a^50QKYO)5t>bP8^Da>B$4@A>&o*4wb$RTIgFc+v?lv!X1tZ86(U$ z84x@?suL^F93G9nCk_fbWsopF|D^bFsWP^?cEHCS3%@czbA&##{W%q9Ilx;E>Nzvh z0g@<|bJVxq!)PlxL=ZOKzsz*~8h#q^ZB9a_F`=qKv5)gZ~zWSCIDAsTR`q(Sz#J<5wpNynG28JF1P% zW>xwl_9SN3EN3vW${99R;xD#SYy7DsR$LY>4Hz#c^c+*C3NM5y&s?V}(_~Ie!=Jzd zu>$im3X^w+OE1$ijxB3sHgN)OCHEd^GZW`e(K(oAw42@M1D6>(fuUKfAZ;^m=l#4e zr=-mzY;5hxV)4=1UeVhK{6vAtyO0c*sX|ian6Q0~Gs(&1A+7*qxKAmL6onbO9N}Dr zrop+E2?u33R-BRHTs>$Nsts1qNBXmN>bX(42MtMN=?!-QY=3jRUiuQN@NqrGqFDXpVe|m2pA!DEjKKQpvG4P02~T_%8qk55_Q`uP(%6jx zfA2YlVW?tN0sc_s_ZI82xdw~NVvmdB;8>N>p1j;V=Nz@MkNtuS)$;j!gG)-eB+UuzFW>LhnMb1I#4VyOS~u z?cGVARPW|U1FAnW*U>+SVqIg4oUox+brlyzM>YrSsOUUXFO25;%kR#LjHX|#DOhB4 zoaz;|-t0~J%U3@d!J@Rw{G}WrOU#8HAu6A3Fy3l(gj~6p$RQ)AI0U!ugsADQpRz0t zM$QeDz9bVI4)<4-whTq)De!j55p}B{wON3#7Qh0uwb^E?Tisz3VYd1y^Age-B_knI zSNCo=zQe-T z7cjeo3p*vQn1Gr1^P21U0~e+X7shH_kuMy;TIxs|!r5i`it-b8asewF`C0|GgRSQ` zLl~+PH*xmw0ly16dT()&xe-t(DqxSUYqmAnljp!rt{4AS^9F9TFz-JoVh?;h@sDIx(HWam zdtxgcW2DoD>(q2RZ^g8RzLRXZ#;&siCuC z7^!d+vQiU{Ge$GzD(s>-9A?g-ORDwkJ?UciGtX>d(;JF!rG z(G&L-=<%y^Hsq=d!}X4q2F?MMUET1lpb%+8Lzx{_1OFN=s`X49U0Ay(wyd; ziYB|bZi<--&52S{p6JmnOOCaU30kf7I^ibj->Xx~W5cntVgXu zjCrzxP)yy9U;=G#KvyeQRLP2ZUaD`hr&_4pw{_Vk&B0=L_&LSbv$~ghk{&s*X4lqr zB!~NUE-%;`ELiGEwFZ10D>%!sDXTe))Ap|#({sejXDKwJ3@y!0C`5_nE|_Rs3Lb>d z5z>}8jj_AE*pFg~nkmH@K;3v)P=(JRL;)SzQhHb>*-Hp`%Aut(dI)4h8Xk`hyM-ZPfg^6jNv1_pBV=r=8LB_n5%tK}JTW$AYny^^yRMR0-E{Xu?zGs;ONoFGpC7;Va{ zZ0KyMWpcZPGIIrd{v35Adp$F7LkWF0pA!LHIqPZ-KahE8RF!Q++eu&>8aY{wbm`vz zJNlu{^yo`N*)58?$;@4MA|$N5Fxdf=QD>bh%I}f1$>Loktuc|gjQS(%s;T?Xp+`YEdsVd zR!Cvo)qWZ-5SNo=$xzlBO&elZ%Ke68E)_K@Lo5!9u?=EgQT))4WB`)4TQ_b!fw`DH z=`o8$3IXt)8M#6Py$M#AvxV4>rl0Ey=_lGwa>x<>*A>^9tH&R-io#1nQWXucApZ== z0*5hBS{d3MVLy$D8Pq)1ol310ybpwnOSqPtB8iU2 zB{A-b^D;L>li>psgmE7JvwZ`*GqxT6Zgs>+v@R9P+=KVFV66jF+^qs4hFidPVOXGR zGSdgt59;m~$M-byh88ZD?TJw(b>Yj3aV2EQW7Xi`YP~ zI(@@&YN>NN_3SNr>5k|p#x*(hJx?%=BNxVca`2?g&0&4|5`8 zV;{OAF5j^z-5hrl`d;bgxR3PSLGWX4>qY@UqmOWY7LjCa9 z;=hN3X|Khz*j^K;EJ$tLUVp>AS!1H7qv@cPY;O<`?U|g}6SvAK7`QmcdQfBg0-i)x za*ZrmIgVrT{Zyfj)GH%K&k99*>iZ`%6IXf?^)LFEe|^JN76dl_nmHZNez|FxBi4Uj z^dj$`WNCf>qc1USJoWv*d5wdrW9$3>BJY3I_sd#|ylSxx(d^ipoF$ob`D!to$QpIt zkf<+0yHSOetdE0_`vTg3ZStnl)t)i5+V$ybTb6mLHhFfsO5Uo}Bvq^J(tG_gZ!y zQ4pSV)=YH!{*?P}a#V3MNqSd&0-STF$f96H8HNd_J^Mb9ORtI^))SXrrN_2r5uFnK zSC~;Q*Gmt{8-3W;EPeNJS^&2bBn9QLeDI*f>5`cwy5Y!7%ui+o9tvrD?1c&jB@_$_ z<5Wn&40rTH3Q`K*`ACSvJ=w~ia+hi2ZaphvS%i_P|8|eS{M;Fn6I8ou%q%zb6KHE0 z2WwstH&aX-qs-wDaErf}zV|%KblF%5N!;B8tYXn&{++@`x@!VJ2|tbHP}g1wxf`--TWH+ir~<36&cLk#Ax8< zgvFT^ee#Y}&t6(Vy%&%eEgdL$VWyy43O*{^Q^n+Agd-RkC-=d#i(c~ikda$$yj@-T zPKfJg7@$j0L83(y^~oI<%XY#AoJMw_ZfZ_tMB{K#4yul{61yt|{lO78nV$~mNAP1i zu21=QP(1P)9#WG(5aGf&UJ)r?r8Rs<0D;r6@d+yT>E)MRmeBzxuE{bFyhW4L z(u&MLX&OpgpC#Jd@_p){%}O?>P@>w7;+`uA58r|=YwR9<;%az%&xj{aGjA2>X!WfbYkXjQB9HieNWGTIyH2%lh`KqW@@92rRNn44Vb&B) zQ?y$tP;e0tMYpvu@IjOHgRmyY?c{*SAtGv4nFoNJ z?mv>dB3HA+#-VWO5oL`?ZLOPH#;LWmaCD8`{2vNvPkDpxnZ++Fwb}NbL)a0}pAGm?CfuLEI4QM? zx?bo7>yDznQ)MnkMy4FOEP+&8?C6MPd%3De{4a_@>Lo40G2Yy!FQDTEb?}9M5H2xM z&;i|@xI)ogs}5t_W|@t0kC8!^q4yXKCtu3-D?XoikI?|kn+9RN932AFE0-FUE!4S| zw^l~j&^;A5I`8E&EBIjZ$wF9`dyC8L5y!JaaFiG?ZIylNBpH6APKLzQeda}=DYFqo zZq^e|+Bmr(e$#NktIG7mtS)e5;pSEO5}VbBu5{Ljy05wVm}~LN+GjXzw*369GYazk zO&2Q(nj`7U4l!1Egv>zCz9qWeIu+=nE}2nGFeF~#zOjwqijX-!#gJb;I3X?JA@L{G zAdGNKO5e^18VtD3&WJRH3Vg?-AIRG1hJjCm9`I@!1ZB-_0a#{y#Ay|e&9>}Za};Hs z9R?oooHgN5EVBn@Y^eGxOP))*7!Hj$=qV0@}lMlT%;;-)FEw6;of2WF;q< zYstkt#x%t_(8Q{ODEx_-`3={P*9y zQg7{(i-NgZDyJnqvpSbE3#@$i{il4tQTeR#h94VT(;j`n{>IK#%Y5Umpb^Wz0SyW# zCSL+9&K$@Q;MU{RIkkm44<8QpmD^EU#_sJaNky(S*S82KWsjZ%l^tEWFEX6lD#va> zy_-rFY(RaSN;)>6(8gIPO&kDa_Pr`314ah{40Am91e^|}4K}}$jDoFkM?P`!wukTgLOu!& zGhM>k+LUr1{FY*;;E^K5U)36L1*B0y%zm=3V_`usSgPQ0M8!7P|i} z5%$rUxAx~Uj?hg6%TX<&Sd0gEmo(=#1$UQYq|`AB%Vr$=&>sDz_%lhMHrUhOpM%ie~~1owu-$uc05~ayg=nr zG-*K7$c)s}Gt#2v)_Qd!{CW?t5&V+9g!NruzRCERJAn8XyOpJaECaWgEBd3T7>buf zUkjo%)w8T2kz?mC4^e>9ZjGr%VxR_F0a_mEz)C40WvP|YX{DeeaV@e^p0QHONx8*J`IVL8C*@izgv+1 zlzJ=W4pOeSQY?Mc%%!A+t(04>JgZ2VW~IzjDMlR=fW*mG;uI@UX=!|}5-ag6D^aZ| zpKGj@SRjdNw`C22v_*q>ki*amG2(!Rf4`#8;3VZ{E9FHirI3`_R>~$TWgaOHS}9Li zDYc}mwo-m3Dc0UTKtHX)W53JUEp8xq^KFpX!@u$R#E;})nKt8yT&2K`|HI8&1Ohit zBsl5BzVzPb_9Gcj(O{%ECY%g}-SYfRb_3B?|HG^Yw4Rt*PEh38MATutM0}}vhx%Z2 zkrOlKO;)UoxsjNA)SM|%!|)R|U*506x`ek`;Tj45UBU^YmN3A_SO6ZiVxsrpTbIOM zGLJEziIpK;5c|-IiCIai>XxWsJWkYHY=M7L!nIcTDGAT9!eYEJbrl|1Uf}mO%fIf~ zJyOdY;RVu%Iz*KRmwXB8aUq^WSQ|o}tu-hE5bN(=IhCoPlmz$*iErKUtx8~=BwWB& zdi%Tvv*YNV9!_K0@dKmSR-SX&`v zRAjHn49u>m>%iG-Pl`5Umy+K_rW4Laapvq#f3W-(PY+{fq=`X}BpQ1I;-y- zG$nh%fX?i#uaHVmRnLLvu3KHC4p>*&(uPk_YFB%7?+){)*U=OM{2Jib0KbWa18yw0EJLH3A&l z(<;*@w$~cm^D{{;o{*faL5&jk0Ilb4{$%8%0e>J_ue7aCfQw^TOXO~p!mm(0qZ7GP zH)7rChJ8h3qFY!iR>L_b7OSvDN19WRWk?kW=INiRu+^7=9HWNh$T_NV2vGC0e+-uF zRpD5s+cTuHX@FHe(e*t-Wz6kXes*fx-|I-wOLv*;tu$2eD*78MDqzf?T4CwC`76nu z_)7`lzgdX92eXvp-4w|cGkWD}1?53>iuB24epMA6VcsO~*cxz)mVFkuWv7!I4%4r` z;Nul*_b9{PpO|}T0y(hnLLKJ!j3%c=?@>TIy6&S!6#Rv1$#WvGo~xK`sg^d8;@9AV zO%-LW`n}=HuJd`rFE7wtdu()_)F99N5BgXK<~&vrycO4 z8ag|3NI+RNFg-Kz%>2y6Gb3fihtAL+Fj#7aG9T3pl`Q$z6ou!r zrf51XJx5`-)EqS@g9Sw-2YC(Ux34GG3jwlhJci>Hs%Y#GrP_EFfAoCoCem`u+G{x) zeCcpuOm>+iGU&!W>*EqW3ZdYroidl8?NgSLz3mhNFS18k5*j|jtf8v}qXGF51m7zh zyhW~=qQ@%)tekBSNJ*hKzk@2p?%|ltKS91%$SP;^*<81o-zQtL-BLNEEBgX}mm!oI zQq1MPlh1qrWsUJh$iPqX-I>Ni$8C*5`Q6ta1~^tVj#Y=aeb2ia2DzC-&)nDaves}j zvq+i^N;dV#23hUftTpTxoT+<%57#z;6xDy<>srI__|WqU{!|-JANF8Vle>F&W^Vqy zXOTi2)F3)5<#rL1n}4Sz<~>EGAoxqTA79C;n%JSXs&lQR)d?Z`J586BdaBaPkdShh zTJVHc)4|F{g+Jl{+qo53wJeESURx$2R0zK!RHAQ+qMiYso4z$zYrK#N0y09Mpe|E{ z|LV*o!OyF_Es;w&PkkPrH~Q(Gb)vd-cQXeHx~#h(W;MmESTu-3yW68W} zgA`Jy*45eWPiH>(oXTtsV~xo7^F+$4mDw|3HbBT2*wZnbjRFxH&HxYsO%7quW#?te z?bFh?(QG<;dn>EhTE1vnZUjT?Cvnl-zWOI=s2{Mr)ki;0`-;nD>TH@9q- z!wF|dVTPRPp@hFdc1$@I$gHlh&Tkk8f$0DSr#NWbAzZ^~+{e5&NO|+NG~{c;Mnm!A zq{w*=FCOf_UD_x{V=#V)V0&Hfy{T7`K3dDG@Ms3(P^JeseZp=st)p$dvr^ z5GYpwB{S{{&Oh*pQ>Izqh<_ZI-6(K|eICv?hQRsvR|U>c`XhHauLS^XS;GBYKqw0r zP?wWvZd5zy;9bsV0!!|4{%_z|8;~MwgR~}drq8;|`APvScR61uO;l9`r3!Rmj6blM zoA@5e^ip>@|DA3M_=^U>D<#KCbqwVvhR(J9aj8V+TDyc2?halXMoC=%bk9CF*9OPL_>>`;r1~OP-i(z_jJ9S>9%1urYe$yuIWsi>Jk^(v)NyK06yOAj zq+(w8Q-)YnqXY61bH^`>$?|iA#--fHr33u_R@@{*#@5Xd_)#J>#WbOvarlntcH_&%8oSM)s*Y^Zs z-1U9y^|1ANgqPC4i3pZ4SF#!5{#WIFKX}&S;#Gll_Fxml9qm`-tXO|eBpmC{jhq(i zcSfeg`iDm&em@{z$8Ej?fw))dYrV4UecmuGVnB1p4if2qLIx z>C9i?glQQ&qeY?|6^~@nK^G@w)UgGmcf@kTRkD=i);DsnU?qoD%*&C@wS{Ta8a>FE zzzCgU{gKUo+^#t>+h0Mn}SdlJCrQ@NCY9($s= zj_PeyTo^js`rWezOttW=mq`+)0LRkzglMT@?v*QXxK+~XAgfSo0HGLP3y0Coqr+EL z;sfyNa(?%y3Tw#V_E{E+Ep(E>8NJS&vm|n;BBqic`$y}=`()4_*H~I8Nnyehwz9XzL ztr%lI)=DdC3Wb;03U{{nR(g@@N{&zoFzh#zklG_%>QG(Ul;H+K@hYc)_>*~yfC$qI zy)gMpsnMCZCfocmW>%}_fBi+V@zy?ivTrbV=9|OW;X(9uFvvIv3&n}tWay)e)>hN? zq&;$t(OR6%IU>j5)|P0#w(*VO%W_g#$;(*gg>}fH97Ls<(+G^(ZIXSw6*b>xK;#@b zP^UY&U^b6;QOwCq)68G7euhy0^d7I;#$pgnS#8Mp$huN*H5OW0H||@ARr&(QS9ygK zm#P(AF2B>!pr$QNkWti))1>)l4Qal6X!A#?<~v0#zc$OFJx-_?dE(FMQEZ$5 z(uGH>k%MCJv;vp368Tdmo}93sL5Q_dX(cTF&iolen*0q1l+F?(HLIzPiNtEEm1{=Z zn!;T5ns#lBEkr-WrFF4|9(q#Ut;f|xc4)$@u;Qp&AjB_^I_2hr>Z0{dAju^ZRsS+y zq?WqXg*JWf zUqQh29{9UZV9Mty@7SaRSB-GHzMw-{rP(jZWL^o0Q+^UtB=#F(dd0Z&2uClnvQ@f? zh}`PV|U*M0KvJN=*Lshr$R?ZsHY6TvIk+EdAprGUAS|V~S7i#Enen zGS${kbHnjVSs7cz?3O5K!6?y%pH&B=CilDaXRXX_&!w{{lI2Vs&Q4;skvFEBpABGo zKxHz-RFRgcls&?H;w&Zb8LnA0KsO48n^&=s(x5A?IH!VJ%eWUp)Q75yqeW4~aF>VA z=EH5&ADeV4VVSKm29h07AVB_1xKf2JUHMR6smA+$D1C%gPEKkK-Fu!9D&W*m;{k-Ojcg&pYgsFv?b&=-DYg;pm~WcCfZMrzz00of zmoKqL$^ny%8CwFX5f*@SSq=+%$>~-@G5HjgHB3`RJ*XX%N5Q zS4SDj7NLx_Crfv7zhh^Q{3=vty*a+tFT?zbUctF1>kgzw#{o9UsMFksKd?UJJorGF zy$3&E=GRskUG#QC%B1yS*k@nDDx5wP@!P-CY?ef}5h!B5D1$0|bmpI>v%yW@x)j6l z+`p(!oLU=-3n4TpZ`6Ux9w8N{i&DmI-aQEX8i4ndv$Dk_=ik+h|BAe!$3D%{?k|@8 zBL0(N9TeL<-ZmHULY{kNviAygG88Gn2L%t&`J=%ECDOG}~5DfhLi`uoo1&?xI8?E4^c zChZGo>sU}-dg)&I)tjt96L+$=s6#hzt7F4ERMSUWWzVx3T>XsVhT{aubBLNiJ^6}<&cP>t=qrD5Ou%iZBPW6C^LV=8`AGTii1i>1&ag^grS zgf|J`Bz_CSAGd}-lNBIZ>_wIJSdZMvt{c`I*Nya?H_~eWK10?7^GD5n(J;w66Hnf= zF6UjpSr($j&}ZZgOP9-UT;kFiHeRV_DV{Iukw1GPqs*S?aFr1W9#@&aJ)7c2dX@lE zDk?zPAe5~-vcDSo`=S(e1e-=CaT7dXg*TeTR70nJdog(+5#Z~bUS2fKUlfs$0rYFj%QJXnKTFA~|>$#B> z(c8nMIc9fDzacfSW?V;NynthZhubrjuiP5U@!*WpA_@2QHfJTrYwI}O`EItht|iu; zr>zT*@@T*1!nf8YLV;#GM|}R79M8gNRn{>Aov}AKjc#V|eY0Usjwd>vRaVvPtU~#) zU+6VXxq|XPl+%RR=6UqJCD~)PSBU!RgnvWib@`o`#N?Svw!Jl=eV@x*T1z(t_-(xhcad&9 z^nj__zLVdZ)Ry<#1-!lIX$5+Df0jhEdVZCTW=k}?=fBd?9Es-iJeZEEra6-LklLRs zpK_B+B%CMVyyOB2J0aQVsj<^8JH1<7+H?2>d> za;$_$NO(k2ld$~RuO+j!8#?(VSFQ2iw1?5TfuA&5vIzw7`#*x9=w!BwkkKK_1+jvI zYx8^Nvw8(zUBU7j5LbUk$)TV9eAb@pFj_;UtS+Q#eyeolhG1zYyot8*?rFwD-zj-S zM&82Sr}pGR+VW{u@-}&8t8RXpljJv(mVfF&vBmo@7rvnc_#2&$a6W*RXo@7+{W`N&s#k2 z@_fY8$CKj8WvaS(M)8d0nZR={PZ`f;JXi2k$-}8(h7x8ZZ5#6|m0{Y+{J-IB)BMss z>ySZOlC{FGz9VYDgR)&CLwd{?eIN&exU#|+nzRX`_*?(#+_ew}> zMWivu(o#63izR=#oCrG$AD^JH3k0C#2D^~Ng`}0p2aoyWcB^C2+7nzzM*fu*g1O_c z{7~;TgRUk0Pm>+3M72{qNv^wC(7-GBo+MNxB$UG`lJFD> z#L7e)_oU>P77%{iO81y6KSNQ&%|pud1mNl8*7B3yOD%8A<#=PhCCYx0v&?UzHuZM$ zBTJ98X6|i|Rqpr~;IxK&+(K|Vdqm|S{W#G_RkZK5O7Y*LL~DH?dNXt7lMJzVDR~d{ zNTc+|_c&KTL7-NgV!rd|feg=)A-O=rSg^jfOr|oM z*=eE#<2aJ&jKp+#9u4H!xzF&A62u8IFmvp{rc)MJZ!Z+|aAJt4$JE9Mca5jU{R5RG zvlH`JmGWvhzjs{6?kcl;cwQ+itEg~L9G`6e7+ zeVUw7T0I^uT08G^crFSUDPETan^s@R`yHMSd5-W%dLHSTdHJtpeN^pwR@4#|^9rJ) zgXODV7q_CS_H^U0>nErDf9$;pSXI^cH-4{xqM{cElpOM^s3@YC6N;R}RnUuqk|U-i z$RH>nF({TPh!%JysjO_KGP6;e(Ja9c%@nf%vl55wDrSg^;Kch`d#`ivh5G*b{h#;$ zJiqsO-uLKbozGr-Txain_Fil6eXI|k78DbJk(H{Be`DaZ7i_ZaOgrzAAOUm~ zYgWT2S((BQSW7p{gelDqA}kjA*wU;TZq>;IJ^vQ3*ah$p>6t92#1e!V*vxapT93zx z)^h_-pdDIPKTMn&JQBu>r{_do{U3u(0o{%DQTPN>#VIEJtVw{hn3x1C9*Gf4iO~~0 z>W4~9?GUTEFqa6!$RBY#VKKD{hZcupw;^bU@Pawb3QW7OX*{?f6g$Ru1sf(D1S*(l z#9+}d;gC;hX6q1`?wX7F!CpSA*kK=ob_PAZlZ2@XtQc4=5om~QXxMcYIVy1OVJgNP zSN8%6>9hM-9k!#93JzOkoG{DT@KaE4?01@rijMB8!!nnG$DqI{cu*g3YQ(Cbz#bU-7fHcWG< zd#vwnwKblVA41xQAO5`YRS9{HUmC^@_+#XDg zxFq8kjspK@n*dA#(~ky0o;hW_)CKlVQ4-n@%0 zVxBETRT8!XLyW8|HV5zHZEC7QAVn;)j7auWq_2PngK0AF(f`VnaFBS?c!y%k;< zq{~OoWE7fy(NBr>uUJ1>`+=4<>o*vb$wx-VV@mlr?$ySM(G%!sV1&?NAoc4p5Snv_ zbT-G-gwu&L*e4T?^UOm2!dk(UXhc@2hFC6yJ6Xf>Lr#TR&WGhEZw)QI4Pg!|nP^#k zIwD)In`jBH_C}a5upVff>yVu=GX#NiQWKS!fzd-L&d z%en)Ui%`qeNd?YVg8k?EDH*NlBnOOt?z$|$OrxkLD-qEo%j!SKXP6~qYnac0v6wkm zE8`7c?;NFDhOLhc2SO}=1^ZkKkNF`ypxm&Ywoby3fjwTw!+gr7AZ~PW4LBeM%7C$J zgY^nd015bU`Nfbmf3EN#Rydxe6O89SS-r3j3Pq-n1up%NPptN;q@-o&@p*HvC=`~A zCg`^x+y&_!fH7s4iI!l^IAF?ca2Agk$4xerU@>bj-ifo&VlcsKFs%Lwg)vb`OY&BH z?aSBe6J77NgxQ9dGl~mnq`A1$Jn)DHG#FI*xU_C)F|1f zZ{>yHYmQ^{;Ed>6Hf*cM1;zvn41J=7{!x})f5H8YM~ z=8*_WeWS+U9`byRp&fn99ir_l^nZOxA}@Od(NoLWs{_#n%yiU9gGYoteneXJ4v&xV zogT8m7Dsf1dFXv_1jgLU;!<-McT$rd1Ws7YcbF{V^kI|bg6x0Wl>dSh@Nx)sg8I1H zY;i7~gx5rvC8OLMuc`bN`4e2j0`{dZrUmRlq-^BYqRZbz>m+T@6R=Zg^P$8LQz;KP;JI~ z84&?qa%Pr_`PeNX6(@9>v=9~V-GF}-O&*D6I(UB*&0oF$E7554Cn+28A=~0qWXPIF z6;0HUBZqAAi2_H|5gg*PI|QRP@s4iu{?AfA;Z8VWv(15$?zJ@QL3q1f4)wjxY+abjRZCMG#gFyp!p%tw2s%wrA0i|hJ8bT9N>isZA zqZvH)bP;BsQtI1gATasXVJnUqLq*cf2V2Uv)2k5+vzmR4?#_xT6*{yQe}`(K-XH?d zr%kC9LhRPN6$UY1gjdI7an=a* zDEXP+ix$&WbST!{V9Txr{7wz~!N!RT@Q0Y?&qu<9f1>m5n&o$?E$;|Q@}2Pi&H zLOQY-v2QeCz1Dz`)K3mJ+Q84E^8dYD_9I# z^;KiHx3I9b>Yc`J57)!(Ss@vWu%JmsZ8P`8Xas{9jP&qz7R)PLkx$GtTH^bP&=A7a z@?ZtM2+Q#zq(heRE43QoBNn%4pT6|@^sd&H7*g{d<=a4Hg7LDP;8VVe8&PRUV8Sh-u7h#eY<@myt z{U>Q_{6z09zIkqaZ5Jk5U!r>H(AvEMy655rg1tpCxA4kciF>ofjrP`LHmjPB1y*uF z);*Zh#YkSg^~08^aOy$T6EY(U zkrBK^Az^wu=gQ9_@;Vcwj1@&P@qQv3&CzDMV~?t9mzWpR9`82Rr;9YPSB*u-G$^1=5t?UuU+nG1vf&NiQ$Fo`jNK06#jxA(d2pG3 zZe>tad+Rh?aI5!gA|;ws%lrkQBc#sWXw7*R8!5->18K0c6c%H2_wv-C*?n}`sNGmZ ziUBAN>cj{N{#hx9tOjA-i&!H8jULBEV!>>|d1)65w3j&OOQ*|0^+f}laE!G1PVxC2 zNKUr16VX@W-Pov~Hd{txIj6Q|rAD4YV_1##rYJ3JsA%u6<38AbaHcD(jJgM7T?rv{ zW9VkrPH0wJ5#F^V^hQN!Ziy3)x0SCo4C*~8gD@u0)6b+C{Q{IJA_q+eryGqj=ibpT znUGToy9N%|)IrvkQoKhZO0BKT4d2F_T9;s8QQUx<7vH;#1{lRUjNjs0qa%Sj(04kBagHLL@3k0hw{=XH{ z@)MTuj-XdG74HQqUM@Wie8e0d&2y@wvlTv|Z_(nt)fV$J=&2e?vd6gUGHYnw-c%R$ zc`I0^I{Zxs722>nv|$g_P*6twlO}4v&;d{#4m6Q5GjjyB#V*!;uTY`IYv;w6@huC8 zXeH_b)@H+memXR#jvSU&kkf^SaZd+d(>w_HO`mwl1_st!h!@GwF!; z9JY>&cxiKwSU5M8*HRg+c~4WjMs*LfWYc@W@Hq{wF+uN0lUnRl#;qGZ83{#OD@29i zW|cnk=YMJoX#P;JIG()~G-H~!LfJG6x{o}1*3g5%o69dHxEGcrZ*$BmMc{-CJr`!7 z@8#-XRtnA!K@}zIk}K>=?rc|@fUW}btL5-thPs$`?kK!T!d~)AT79 z-G%!oaP5lGBJcoPV511!#k4kDXA6B74?zoEDU`t%uEEK{4iyHIlu;};BRkY!DCOB$ zb_o73v~)Cup!eCzu>iip zq*d$`L`Lg~oS@sDh}<7zQ{PO@)Hg&k^)=n;JVGmkv|eSbV9S-T_MIpdwTF5HTN&(| zou`pv0G7TZbP71B6XXgGNcdZ*uo31y8qfzhTKNvhIf7*ds%khsW{vP7r1j`Qyb`OW z9r0uaG*q^Y&n>NUUc?ARSQs0~ZZ*Yjw=#ia**9T5Lxn58mJwf-6)w zqopxQ8dxxW(cXJ#>#5Y%v(Pz3$$ck{@?yk`mA(^&#pyNH@EwplshHYX_ixnR<<#d_ zO@qx(>XOJD1TFBALDbkXp%=O|5bvEbYaaxtf-0KyP1K*4p(lVc0(BX*xs-ck7FI1LBePpv3*fgQl0GpS4E1w=jGG ztw5Cvx@T&PG+q$0=KU&Iv3d}boBHQ%efg2+#4M%ievEN236cYz(bm_Xg+m^pLHx5; z+*waUGmm0@jj`97l4UONTZPfaBr*YDKah&jnsZ~Y${VwyyBKqm27-!%AraXMAY+Ub zXikSx7-_(PjNEHYwPz$mW`sid%TQv}<%UsL3+wxcvFfyS#`i)+|BclfVxfujH`d8; z!VIiT>M`7DJ~b0vwme^S$d?d;Tpxv{%io0+bXTFh#u6@U!I*^sG!*HWQ-&Ktke%;L z1q=0NL&(iw%XPR^;~PHauC+J^gHD)pGao2H@7!=9JI>i;>0nwrr#X%#zB{Iw;fc@TEok(w zhE*R>*wGb++)cPKd|nkAa5)&O91Y`+U@4!-9#Sb8lPy0?g5#G?tEH9AEyr4y({5Xz z1EEkI{1g`OZF)CK-ZbDuya|(d!{>X5HaF#WxZs3R7p&4j;DHUu-4X?B7If`&=_4or zbPL98qzvk%Lj@#VgAN1Hfa$9}(>jt+n&o%8@pCvvjr3{0eO2voYkd?WR>$EJEeC;a zAhta{kh!8|22qDWDm$`AV69ucVI@{DaV}4Md-B>+Nz1{=#}=(>?n6W2Zx%N|AC(8RF`59U%>?=mmf&*gz~M z@)#%Ls%jGeie=uPJ|~hmRAVyCcEkEIZ>%=*E(POJ#EwZ^_gwe`Z8J&G*+179W*@>2 ztF62%YYdUyrwRASHeDqjZo*F)@%l*~`Tqs@8^q)Lj40FWHj^a1jPhMT;_l8k+s&iX zlOnN>yWz9XZ0X}XLhi-y2mFxgVu&0qF>((U6FaOYMgR}92r$sR3JR(Bux-FNf+)qV9RegDeAef?nHKR^wuP0fIg{q-kYP`A&b_QH=xSMq;rZl5=a z)(;-;gVN%RN6Br|M!25r?m_f)`c?hD)88MYw959KIN4_&{31-B&Jc{ z%S6x4b`kjO?CwVh--_IeA4C(m$04UA)dpf$&GooJ^jP|L!}eV`>G1%8dhgx)j@|TK zK_$3}${2+Ak*FV;gHVz-Kduz@(SU4NA81bgZ4!{pH(slsx6eB~3SBaar&c8O1U1xl2cHV;~S*FwP0o z8#>BI5gxO{U>c|G(BVzi;1SdZ^*rBY2tv_d$?)tR=#8a{LasBgwDAzU`a4Up%lRJi zPJb<^dp|d-0)z0na7&q`CK%>bSZm0h-{$yECKly2r-F|9{kYY zakLkuCJu@qO*l_9qo{0R9Tbf{o9<7MWh;6M>#>j**$m2Ai)(ygYzMa{M zKamw-`9bAYFVYYR)!&E|n%-s7hh2A}5hIphp~sA{>F4ZoEF-P^tI0jv!%|;c>YC9J zI_qTyAsp)zM3{T_M%65F>_yP(dd%(&JI=REZ)F`OShuRi1l1nU20sUnz@Y~X zdh7=|V>Dwb07oNb+=$M-FT#5@AJaU<5L}ArIs<}q1VJ6o-Rq8X1#+=osleqrUihZ% zT`5mO6v#IEOf+BWlXEX6;Bi?E-d9YPV;HNMXurz<6SnPVCI;sRRTySrh}SNnJj8-w zW4oZT13@Kdf4T!fV&iX>FD4Q})NrvSLkP{^BpN&J@hYKN2HGvnHjK`cs=cvbULS^n zWLdw2bPeA74c>CiS^`ZZ8eI#7_t5qjK1A=ly#$?B zPn%o3VUSBJ6g|Bo-MQj!Nvj&O!M6;529)xAh2I^N6Ipw3mny}C%1L4}V@?b?#+5IwM9feL;gjp*1hf zL>omEu1;0`#EtQ;#LcR(LKO@n^f1KN)9!u3w~6GOZ!qX|rRQC8j6HC06FIdM5n*w* zf3Df6&wVUN2g^C!cbujQ6M=__uh7V6hE@L*l{pHBiG;ZKjEEv)X`643s4|lzOvPsr z#$qy>dY9yuHOn`Nz%b*GKU55d;4|DUW24yHSP_re(#f135n;WAnngN}aCgg5SabCa z9*0P$i0CaQ*DWM1!;YaX=$^!g`<8T1^2L2Cx^E`$Thl#uzmvP0MW0*gl;KD1w+H=c zOKsTOoYR_QjFp=3(*}Qh+Cm$I`?hr72BOR9-S8S-=_D1T8Ij+d#A%8!=QpRm-k*B@ zAUfCv%CWshZJrOinLfj#z;qXhEcLWbT|)yBYU)+3tsw$XFM$wHYq6cq8VE(7r6TvN zY7M758aC37ZmAFQ6gwYS=R}9KSlg1PYV=tCmTztx%Q?6*yP=_XS?^n|kD}fCWe0xs zF;d9rQ(GF8fpuVHr|r4lWeYi?i}uSsz03Lw97#!6_2=Snp$@F?%LjI-)g5KIs-f8rr^*86cJoq*4Nr{W+hGCk^pESR*xQr)fd{={JBk&o7`!B&c;PMn)DA@xp z7Th5sxIN2v;;m_3R6^600t$J+S&wI-v%6c`^w~@ihdCQHNAyK)Ts21o3|@yEHjoV} zR}8}O68DxPJ$`4Q(Qz@oxtOORWE9gC8TGT2iE=)Pjyt!VANfTdjMQEe>`y4Qp)h@xP?AyD@qq8%b9%5&$MOoT1WGT; zo*q7GjZfuK;`a0&La>Gdxed*he~r$564GBI>noE&giPc3trOUkX_S$CvwfK4znOEO6H1m+^ORfmqYwM z$3k=KT+@2hmbS>~7+<(nKiat+c1|~7_i!29GB0DM9a?(9DYFNx`xyqwca!|gD^OqU z=~|Lo(Xgiwo|&*Fy9BA1yB2l2+2sUJ3c4rPC7 zFh1&nj>1%W2Hm$IxHZ22a1aKaU4EE{D-lYfFnok*aei!P5{jcrA48iD*#JEtCY*YC*R5s@Pjo)V?>Vn-* z5RShj6Us#rgyopU)tdGYX$$0g6kNd>dN9WyLvp#f3h&pTX%x(xnEB!8f5QE;AA@l$ z$t=FEhYkn^J4=Xnj(<#gawSgK#C>j-r*q~c;hJSMx;%3aor?=ES;n3OXaq{27eJ<$ z_4WJdwSrjk&B$DcpBXErtn0`zKhoR!2bKgB9tbg_D$+qd$gKB+sL>8(2{B{1LuI&d z{3X009XNUv@zI1vMbrck5~&4xwNW5(@S==uO=AY;QZ#@-GD(dYVkRLN(;6|*0|?i^ zMy}b9rRuapyLyA;Abu@x;bwdwRfc_Md*c$V*+^pG;+})?)UmjBL0o*q$#}6zOyPdgeL$1s^|ESYxaIhFM-|DS1Bv99$%KDfA0X?T)a;AAq zH!v+?`YF@BOn+or$@CJ_I;L%txV%girjbn3n66@4$n*`SpEBLc^hc&=m{v1&X8Ctv zI*@4))5%P=8X3_^Cv+(qE%{dB$|h9vpWROKHLTHMcj=zW{7*MZ?3E&E*%KUYH&ZPj z)8Nt4y>h3d*1UiA*Cd!Obn^Hqqhl5)rzR%FrOgSSHgnvA!b>Cfc}wEb(i0b_%<&V~ zkWqu=&3)o#sNaxbBiY5zPX?FqrY1#a#4k=uo-->kC3f-BbnryH(P=RWbFxMZ({Or5 zbV_V=TI{^oxcKPIq>MR3h-J@5TvkS03S}VO=3~nE@xluxRgEsc?v2Y`a#3QK3B^^; znBG)$RW#vS8Dk-axat^ViG;XZt7Lo{_GIkFd~e2D_+gA`luuVs6MjS!9K~46Ujkz- zy)4G9Wpui{CgGoE%xv8T#`FTFtBA1{{}#rwb?J2PGo}d+UE7-APn-BJZo=Q$1eY|y zWsJ4w@qH711!FBgzck@jHo@nb__sE}mzv;e#!j+$x@%4NbxrW?CRle#K7UO<&WyGE zx;EjvHQ~EA;dgAp_iTc@HNnOv;k}w*?V%u8cP_*81ZujN384gt69tHtjFVoA|df*5Y@)BJ-ow zFC$}3K0%CeDuuY3#^1DmYuZ0GZQo7%gQopc)A}F9>63zvu6gt3N2kZdYT^TqiH(a% z0!@uhkBLr_zQ|vCMl7)AFmGN)LLzx!hk5hjqti1S@shZhjKyi13*t@Ez`S{J(eo2! z%!qesVtmHDA)Jtg@3Z05jIk{R1jlI+)@s% zGseax#bw0l0^y)2I^CEtIO7TTfpF-FA02m0*W%Q;v}oj)?tS5p>W>d9rLQl3bdMjk zNnd4vZZIg-Z$zaVgE-(_r+^Lw_3p3JtpI%pzvcM#MWE%l834Wye!ln(#;*^43IdHm z1buP89KVO~i&_$&5j{UCt{=kE6}4o36HF5Ar;Ey1mKwJ>Uh}4lN?Htt5hJCH#2Dj( zOpKNUFN%bgvcT?d2|aH}1@4*jfS3~{AD0vt0yOmz#X~P+3`SWT)EJYrI6X5>H1C_DeZ%u-a$;?!mTNoZ8=%-9TLT3kkET8el~Q6Xks)YR$I zLq#sYm(SH!D;nc}j+pAdR321OTUpnP5w>f?C2Z*W0$2wC!=X3rX*%0qUU%d zF3pI!hf%8+8b3NCE-Nv^q53=eLOiz0^Is-s^Y`D%9F_ZTa{QP08@2b0gtWNmSchhB z>(7k6dKnSD9mg!57rit(k-Jfw`NdtDMW?4b zaE;L!M)~S+||X!{w~FR3TTXOu`W#)hr4*V z%*S1t;3w&Zz)k){@pb>$c@I^?fFOUG$teW;rJ0YF&@V_0+hl`W%p@JXMs}u zv+<*}lklT>lJPU*mx^CE{L=8Fyk_G^dCg(E3Y6S)@uPTF<4569Pj&tAdzJC)pp>so z_)$7<<469!z>o5E1V7^cgdg!|Lx41fq;+~EA9JmIj*(0gm}W6uh#taMH&I9Br0WsU z!;I5SmJt2QV(=%4^4s~-byR-GE23VeGo5K9)45Een8q?qV4B1HBjavNy_otj4Pt6$ z8p$-4X#&$!rrAt4GS#z?w=mwuw3ulL(=w*zOe>gHGOcE+yCuuv&eWS}5Yq^z2~1O& zW--lXn#Z(|=|-kSOt&!I#Tb*YY4N)=HZt{N8pJe`X#&%1rW=^P z%ybLW5~fzBx;rwxX_+ay@Wm;*>3Gl0!rX3pLZ)tfTB2@RbcPPR*yv?2r!SV29tr%V zlOz15$UFI<5H3wH01;klu*@WHXMgO`bT3CH3ZXbi_?q?={9$XRDL zk4{b=O>a1PP19v!+=B4vM~F@brcBPl-&lbY-~w5pr%VK$hW?1MEW^bD$#`XWJC^aJ zBM#|9#z*5HyBnfGdK5p6x-_5Us-4G)^I;D!^OFfV$O<9zm4RF&^SD&TEAzS7<|gyo)R&h3#_lHMlCB7* zbRUj?gCWyc{9Wcy9&(|71M#*bKlIONb0MG7w^?V_jYb}m@i!g+v?~rz#aA~2^8J4o zNx=BGf8ILn-`{zEcQIpx^k4m3jS))Ie~ta;U~HrP)9e0w{~TjjrL+HQhB3{5=bskd z_?7831|6#pwvFg&Fm;b81Qgij%_1YVC^*3+bzH_%hr*G!u z?9#l2Ys*%x-P*J@xVLNHp`%Bq&Yt&l>DsOPy*-RQd-e9})3={@{{cP&eFqKp8*<;! zVZ%p^^dB`k06G9QD0)5=!0`(b5*IE?N={jv`fyr$M&^>GS<4=oGxx!V=0*Ky{-etu zTe0$gy8Qps>Hlxb-!yhyaLD)x=Fo|g!onv{iMW62wCUJaA36I0DgXb>|35F?k4Jv0iTw{r$YMaiI=)<@GB{abj_HzhfQBW=-r4g&hO@JSe_#2^nzi02WJmqamw z)AB*_4f+2iJ{oJJBW}tqmDn)_KU)g_mR}l;QI2W+M`N4>;5g(v7D$VcWT(mC-{PY< zXhfQVQJiCVEl2+zei>4x@bSo(7K2@C{}x_q5YvzgEiC0;OTDRH@9+A?EFlG1hbRV_ zvOR`>PM^}#v{$~!dj`g#>5yX@(w1q&qBKkYAy5AtABBxU93;sMjOct38_7kEJLAD2 zohH?BnjJKLQueY^C5Y2y!CzDTYU3QnA|9%-i}B3n<8RhKkG+|jQ*(?kTqF*K3bZM%O&+* zl%6RqZakJ!nNkzeO_QgE3cZ>;+(``{ofbDuSnHspr9P9~{}Ux$8}Xf(hK&o4PPK)0 za3`~1++?WEr!Nj07a9!jLhoiuO^sNblo+!NT4h#-PLE4VPE3LQaB)g_Tt>oTnih)m zi5ff0{*rWDba^wT^y%_r47HcI{27z}kS;T0ZJj{`V=4z-k&H=qNLLhN(#_G8z?gJ* zbfq#TT^n6lj7ire`#*S!o~ z8EflI+!-4+?~HpgHZtzT*xgCS*PF37^Sv1RG48|IpE23Y=?Y@pPePrUG3i+8ieOAS zQ@SD<`$(vZVmy#>0%Kpssf@`sPgfS>5fbXM8INR~$Jn27A>&buH!_x1G1LdfD)YB6 z4r08Gv59dp<8h2j7>fo7w=%}#nP1L$0^xdZTL+>EiAu@mD+#?Fj2J-7?w1m-u_ z_>5aH&SUJ#cq3zMWRe%_6X{dabZue28{=ZeZ5Wp^Zp*lWv4OFbF@5Zit~$o;7;F0d z_KY?Ceh0>`U8MXwGWKNb!PuKIeI1!Df5x7S&5ZA19Lcx~;{?WC8D}x>#yF30cg7nT z-^+Ll;~tEQ85r*<*D>~D?Ale9zYk+i#(f!kGw#ROpK*W2X2t^; zM>6(doWOV><1EI$j0+hLW?aPhKE~S^4`W=ycsS#7#v>S4G9JmenlXJKR$Sd=c}FpJ zXFQs*k#PWHKgJ51&~M`7@r%*vxn)<4DG{7$-1(fN>V%IL3L5 z7ct(*IEnEV#yO0O8S8lfQpVVsv1Yex!MK9?Eg4%Gw_;q!*p0F4y;2_Tj6E55VC>Df zBV&KYofw-LcVQgKxI5!i#se8=GY(+9k+H&f3*&K&iy1FsT+Ud}1FA~K&Wx)Wx6lS$ zJ!JV>GInR&im{Qg8)HAlT^I*34qzO~coE|S#`JM7y0REMGtOh&g7HSiEg5fN+=_8A zV>iZSjJq(dU>v~M%2>|>ygJ6tj9rbg{4E%JGH%J(n{g|~{*2ui;}C-&8tZpq9LYF< zaVldy4;Zr;Tacd;Tacc;Tdn!!h6f`C0cmK8w{{2<9M(fEwZH5@GYl^QlnT&>|SiFLiX{1FnnGj?7sv5|2X#(s>Y zl}-mezyjfsbXjz1>l@5$If{YSfiBXwNh(L0H2bED>`anMf23EVD;_g*x)R_+QnF&| zz?J?;-$56xub_+8QP7pb@uo0NWsHrbf=^m3x@g`{7wNI+N@x9My6$1dWJScG74RXc z^hdfDx)M<$x@gTAUFo*+$aIo~6_VDiNF2}kT*Bo`fS!h~Xh@8%IL=Qp=OY>tplcC_ zBP|zQ2^^l*LD98{{V(G9(xE+~E0g2R;Pg{Do&}r^Xg)rHvXic~**b>no}e1z(W zFRZuP{D<^3ghMDYTvBT@!M0w8>WeR|y4rk->J8zk(z~cXzTj%rovhWquzAj4KGi4U zPqSgtf`!7aEibB1zOX85;Zsl|!rJ_c>X$DpvKpW28I@N{pX!@0kmjH29bqkfs(+Lp z%|F#clAl(7s*k>qrWSuXq)mKH9#lVlky~!wNS)%FZi|oVE9Fm%kLoRzkGluthSHni z@VrRgW5v2Fk^t3fN>7t7>5%F9Y3Wlv_eJis`6ktO!kYZ3-cx#7`KbQ;!nZb`rFKB& z<7E)={fNL1;BOdEhOK`fx!LOtwG&^2(#l2td{K)`{M@pHT1z)}Q&J-1dA?J9o?{wRcBM?Y=RleA~lQKj4Uy#q+oGsef?fr#Fd@`U`q4 znw(-pIVYe-$&30C$LA>f6MKxZU!fW|TLh5(3&kjF*uy9fNoFiR8L8E_Ki6dOoF?1k zEc>HS-Tj=t>`%to+KXK0V9y`fHz>78_AmRBv9{+z?;Yx|wDu)BYFydCR}V>nH2A;>#jQZ6C3`X}Gp zW?Orbax&TSE#+jg^;=R-_Vy^{G|txk(nURy<#f!4%)h;yTD~G!!ZKfBwss@)HO-b! znXfR1d{BL}rz`U{!Ip0=U)uAM{q0zXd?sTIVQ;xI{a{=Dk?Duq`T;F{dw)pj+v}T5 zf1Itqlj+;@OSU+wy(av1?BCL1m7siRPgdq5+?G!%|4>^!l<7p+_%fYve!A3eQ$73V zbf^c=YN$+Sx~=}mbnNX(rW0%{zhk_OpKG${2^!~r3R;r9WV{m{Fv(%2tvpnIa?xsr z%-3uiUxpuN>qj&>YwuMGPfwz8c&!(a<4lr=*8ghq)B0PP-b7nDWqQ+X^+@_R*`BNP zALfuBO8o)bx?SmCYi(M4o8-VJ{}Hx+L@ST>o|o~P9Li79nPF=el0UXd_=qO_`)&0@ z#;^67BoB&jn(e(UvDu*>5P!16d!KNGt^D?JzI@YDy>PrVy|LEsYkK;D>?wPQoOj64 zw%rb;#Zk!o3U(-B{4?Wij6Y&r!nlZWIpg(=D;XbUT+R3g#=84te!gSu&iEb1M#kG2 z`!U|jIEe8tj3XFpc7iCzhnb(sco*Yr#$}8XSkK>|aUt_xV!Va%BF5hAzbE4o=ErD! zwj=2omor}*2UaqEjQQ1!zhLag;Way<>rg2#t^Jsp@67%^nXip+ycs{i{0R2%!q}hr zT0b4he9f+CX8s%OKa%lNj1w4Z{c;xLPne&__&vtntmoIpiyN75VSW^+--7WL=4*DE z1m?RkznJ-NGA?8ME#nHt?=!YCKFGL^@zac5he`SFVC>0wA7gLEpECAm{3ByC<8sE4 zjJ5G{0^<_qXE8p`IF-xSl5rmMk1^iJ_*ceT82`k$nDHUTWsE;#T*0`Mv6b=njO!S0 zV(dCx%HspZo{Ud1_GbJHV}Hh#jLnR{WE{!(2;&6Crx|B4KFK(b@o$VbGCsk03*-Ha z{kc8%W?antIL4{W*X*KY%uix|1oQhcu3&xw<1BtYtr%OGAI7+l!w+Iy$NY50*&Mz# zW7iQ5YFn=Lq zBlCMOwlY7O@kWlnE#o@ovl`B}PQk!@*OBu1Jj>XV@%@aqaD48Jy_v7gyYo0dzRdS$ zzV_DM!u%o3H#0wrv1W%J%{Y?zs~B(VEc4fnu_yPhD&qw9pU*gp@iZ+wr+**gJmyO+ z8P=IFzYp^_GJgT%Am)29-okvTMZ=+;jGttF7W?;MT*mxl#ubcn8M|_M0vTJG|2X41 zjxRvN%wNs8i1~huUHzrJUSaIXSZeRE4u^3j^ZgmmU|if;hHuZ<%>3sVM>3wyIDzr= zjJ0(v9T;aZzkqSMr;L9D<2>fCU|h!OcVxVg`7^cf%=cith554>7c<_-xQy{S#ubcT zV{B!-MdNe)ofy|KKa#Pw&ZaYC*HN-OQmX(*ex75P@5%f%jJ+AZ&Dfvu62@l6FEg&@ z{M^GhlKFEO`+3UrdofO6ek|iG#$PkeW4wp)M#kG1Z(&@_xR~)bjLR4wU|h%f>%zE# z`Ewa-di}GEt<2xeST|bc|0~9xj5llH8NZr{dmXEA>*<4O+i&p40y*^I5sAH{eh^K%%xGQS(+EzEz2aWUf*#_sIDJL59u&(kpT zhcm8V{&Fq6la!~Cv6cDr8GCa0dl}a;KZz_L>-*oGCB9Vm+|rOZ!bMwA~t}d{BIj*uKwB>eJ}2 zBcE2qH^$m~P}2t|v(-u3gBIAd4^nRv4K1;}B%W`}uf&?QQqv2?*q*cG)B0w|@QJqk zN&Z6HbCx)zN&Km{dMWvu6;4Z!RuwzOZ?EUl|0A|~DD^T_`!)ZhhoRNijyML-tTArN zZycUhnLGNIdnL(5re}ZN5-+elKZz4;^+w`ktOa+BFU9u!Bww>WYVoH!$dk$=_j=mX zPqo!s8J@J`j`3$U;V*HBkM!+v4w&p86pOs1Ju1;APia5Ytj=V2CVQP`ZIyOtlDAer zrJmJZe>AN1LlVH=348wr5$a7 zOoGK}wRjC{EmGR6wN(;w zorK&aAd ztrMZ=LA&AQCD#k2+TP!?_Q=1E@^N%`EU&chFF`KkCGGI`{7Ab2^(V52v18eSG^~Y} zIKh@5iD?Iqyd+My^$!wf*xsMg?r%?@gE6UYXo6QZ#&u2jt~PdqHZW!vy^SMps`*`he`BRT{hxB2g*%>PaO%gK>XUV|L` z&cc(Pj5PLtH;J;g{R+i!ac1(1xY|eKstjty^EBMSrJ-cIQa9 zHTJK&+yq}~g57O_`Ecl!n#1|&l!MP~{=eV91NqCh;n(iJ(xP|hd>gKAc(80(_81$c zcqmRSJ`z_m{IqH?4wgf*sm_4KP}4!N-@;eFnnUrVe!q1Q(Wqy)XAmt~vf)ud!-wS% zjc|QwHPMoX?><2^yYL|k(dva?K1sAFx7AZbD?j_}X`&HR=4~LVo7UnvqUC!&c%EqW zrl~IwHP&B$k!b3k=U*mT)ZG77qTBZU_?p1_9XP!T{!TyfI?5etKK5&zJ1bWqCvSG-zHkJ@fShMZ@!7sses>I_zuzXcl_QZihKyFdvV8m zgbNE+yie5q2h#^ciyn6VkZ5Y`enGc=_{3JiCEi}*`B(Q{Eok9)X9e{eIPN3j2Mv2e zP#m58F=6+q^8~F-_)5?SzpkHP~D~Tf)Z>F{v)!H(R2R5QQ1>ZTL955MD<%I$o}&fLE}t)G;X;d`?tgqDsNeWsB+hf*P5vw2 zA1G+~r_%+ker>s+*(sX@-DW&2XyNBq1;umRL*a~1j+L}LQPAw#LP4YS#e!B=|0by4 zK9{|s94iJ2THbA%#6`;_ZCNC!E^@!1e(Np?8nn5+=%4&H`wJSe?jb=7BUcL=)cRdP zQ#T!z*z3BWI6-Fz54+nkFjF~4LTez zXi=a=P`~H53hI91xS$bEb%Gi@bt|KAh4{oH(UR*if>!Q+LQvhZ_XI`#6SSzPM$pvu zokV}UEqRon*&ojpRCju%pb_gVhusC)is zL4#Z-3R;;pThQvW34*4M&k{7d=m|mHYn~I-FJQBv)sKEAXvC*`1&um)T=Lz{3R>b* zBdGg0{V_^EXsWxUvwH~Y7dc2!-IPE<%LBs%E%KTpXsXjfK_h<75;W+YCj^a3eNND# z!EXtQ{z1@^=XMLaO*tlL^>3#HOaH6psNbRx zLCcR!6IACjU*eUif(CuNLeQuV1%eu z+dO+DFfn<4K=9S$fwPnmZ(V%gT43v4r^h6W+Z)*Kt^HBo=GO}xuo;+E=se|?>|^N@q@s&ej}^ZC2f^Gwaq8| zH2-v9$DQFl4(ppMyMC{|zw+wEz^bcV?rr@@OXWbblGOY+O9Jmd0L#oEccpF0x0iKu zJd~}UIi>c0{8V7_nd)gkDFZIv`1I}g?#f@cZ}?Y! zTpO5Iow|Seg{y&|92(uSWty|{_JjTYSfAQm`D6RTOJ6g`?qTosv*PRg8z z7P{AtY_3e2zx+yvh3%Em^Rth9th7{q+u(8Gm|sU_NYQ$C_eI?mQ_YUnFF(;q@!7fg zA-!uuVA6v9-v)l(Um5t`-kk%R`6$70zdlypxjyjKkxsLsf;^O{u8(Z69CK4vytBMt z+y|YNcjmpeFFdcEvZ}m!)ZTWTl{(iUV+OSxtc*VGb9P_H&dQ5Z((k(O4 zuiXiJwsrV{mj?G%I=C$}Thr?UQzG}f?mT?2vT@?n`U@UCmEs{oW(Rd1tPJ;$?PEFG zRmuL-(tXRk9?H2B{p-UgdMO*+dQ})VwpUu7b$-U&dVq2_Zu5IH!+R((_m8-z(5X7s&z zrn9o^+=sg^&2FQ-sV4k9*wS4YIdb%!UyHpI=k^~)uUOSVdChPk^wD(#mF}~j_o%+u zUm5Y}kd)2u-lNQDp4M_^+8|~6`;Xin+O4lL^^NGGKJon(zkYk$m5uXMMtt)?>n~%C z$|ilk)o#ODDL*b-|JjTuyC^Td^3cmYeJ=zqytA^+`50B{7<2!Lwl57*+=kD;r?7W_ zCFh|m|GdTbDO)$c7q)OtfYNQrlfNwNRUP7vwz(?N;z<-s`A3sF3S6D`mLIFWVo_p z`+#W~Q${J%AJ2QX->?=+^`x_{bq~1lbL^+ot^4!SKDs_i(RVdREXPp3%vNrLyn8B# z-kv@5YHAy$PtcYQD^~g__vZK9lj1j2iO96j>r;vC{?oeQ+Y}{Z!z4{sW*yxOYdyEj$`D97gPuG#jMQF)>YOq4F*3 z0A*{>PSyLj4N&6e{F-68*H76n{mV;>jW{X)@RT&mr`3V|&qTNGk*g|OcedF3e3!bw zcfa_iTave;JkzRIXzLhnW$wT?Ndzd*GBF%Gqa^Wgd#U5jf%g?BgrqZwD?uweX(W zp(B+=xyn`dF9s_e++%m79u&Ub`{M-Koh1zXT3ZI$!9O|HEa} z(_a^@etLF)#nNxkyzrB4l)b~754iHymB3+R2If_d9j`nzq4w>UE)P)7MNQ0IIjOHQ zXxX?kW83ss^iw*x8vBJQbB_MlX;9Wg#T@_hvbhn#O3!b@FMj_{n6mSS#uZp zcW3?FkBrK%OOD1E=L}KI{ocKLKDWQJ;X=c;*f3QwJYZeEBb-rF=U z_pOCK%CSEU%k~@|r&LY}+8b8VUwNe>^N~sEK}!2hm3y6oyDA&ZM|Q{U3sSxve!|r8 zz))p!z@iyXZ%PXs_lwD?z#Ikl>^5%J>K=}FePrumb-Jd^-;P{?9yuY z5tH)TOF^yXeQQ!uBl?!j91)^K_3|EGv0|k1-S!~Aot?dun!lDk`qz^|%7t0Kj9Ppk zT$y#v?d@lr!xXp8ZyotDccQXm*1k5WKl&-HzOuGYIvA{Y^gjE{Jp%)kv8(2F=y{-* z@>s^t;mV8s6rZyOx@6o{Gk(_}96jrUU&A#-|wi0x)=STKXgYunX`RK)Q5M}wJY!KUGwxEb=bh->7OsXqkf#0nf?CU zJ8JjuP56ID?K0=~n*;mZQ8P+X)_>-9N8P_Gcy3C~ZMF4FPWdw{ZmWNMqkk^>>)Yy6 zryp|pWYcYR?Aluo^tIepyB&BK|8J|0hqgJkA@a6bUAITSUWI$npi!Uoy{%T3>JbpR%`YrWF^9}hvzurT>z(vuEONsZ+XIKh2qZOFh!OVxoEEE%iipQqTzFEp=0`6GJAoyrp_?eSPQSH8<6V z3OB5@p1i4cP2JZeb>B_(PSq2GyKlXzejT$g>aUGA)!C>0?mxfkrkdQPe6v3JraEzL zLJQNZn`)O$Cj7st7IoYA#DIY})n=0(sXyKc?nQe1zp0)W>i+DLm+DpDghicS`>9@S z_WHTW$9C7NKfBMk==EW}D)nRaYD3ABZ3eHYSJR5!>dvLss~;{s+xpA7^=jpXA8&6n z*Q@6TzO-`xuzI!snexzE#(MRKLVf!wZR*vVo-eO2t*=wnkyjp>dcICQ?&(*cKVGMf zZooilSDpIq)kTKw@7Jj>=j&@rUZ_(iZhNo#_s8qh?N68N?3YoedOjYF|8;7|=2^RS zQ|i=TzyA1<=f~8kMVnUXCk_B#Ty<)5&!}vD^Ex$XrT&h~wHs>pDG%J==gbZDn#*IR zdF40M<@!NSfAaMWbVkdzu3-o*$wrhcZQw& zWx)+KIa9y;(99d^_|6Yz91FRj7Th)Ms~&bk-TB-2mEK-A)RxO%PR{CZLru+TJr`fS zS35`5bc(8}RS%3Dx9G;{S~a>Xr|;%twQA?2(oRXGwQ69OH(P~%RI3)IO*=N`^;)&0 zJa6=br@&9h>6DODt9DtSAMx75wQ7B@`xjn{u2tWh7_er_)LJ#RXY1KzV{6slO4b(7 z99pXe&DSrz*}GQV@ZpezPde7B%{xU#<+#+Uw|)B$i>RV?n?O9N9 zU7h9rj8ERd>*`ZCC+y$3&@qni@9nm#@cFTvJoF_IaZC&^7h+ z`pv&8UtLq@?3!Bi%{!Hy7AZ4@XPP{^&5RneWJ$?ed2wusrS!* zsPAXSYidVvA$(lhW2;)ge}`?V{?=8s#n!jRuD^U$ZT8N&odeHYRrhDU`_zdauBv~$ z_vof)4_;Mg^zVHl>Z_}2$AFJI1blQ=?N#=Ozt>w=)tHr@1G~L&RejPm{&=qf@cnZ- z_~%?zFNYjFHYf9{YHsn{)q=#UYTba><{f$Ps#==mbF16btLpd;mG3Vbe^vc>=d3yB zMqgELc3=4C+`(7XUG6>;&-T8meqMDUa;fK4weXmq{+>2h)u?8B2bAfrstJQ%|7vw@ zjXLSr-Pb2q)u_*BPb%qCS)&$3`088z06H)y{MNx5wcW@Eo~-}6Mh)`Of8hLSjp|Z* z_33W!)TpnvKiA*%YK{7qbMc(CXKK_A+w$6P%d1htkG$@G^RXIrc)y(Di1Zq@pZ>?? zpCr_%SDwyJ?e$QN`tj1IR=hH;Mt#~V6#r|~IZ>UT|3axzPwjj$Yv!;TbwGRU_#RNB zX5H$U^F@yu_52KT_A-we^+d@6w+XFl)b>U_{@19fJxU*QzHvnjD_%L~+NCRMMzI?3 z*O@EoR}%*|fopt6AqNT^DoRVc7-phZhlYPxBAy*%pDln<@k4W z?%bI(GiOHEzLPuGf9=sx<3qR2Uh}=dQR5>O*&5-IQR6P}cK6DiqsG_I^n7FJo>60^ zc1wkJ%cybNYqd4^Y#KFg-ni)Q!p>2n&~VgzpKa7wc6!gXCtW>i{NbyA_I+=~sPX)l zil<-OI%=GIapQf{jibhj^jY{fYK*3Sd-uKbMvV&%*ZkUY%Bb_$+;-kv@`)-cuj@@VD4#4}^7a#w@$n-IoL@{x#zBp}c*}rfw0E%N*y73~>e6>V>$H0*zo?~}i|0RQBlz%GE zFN`gm&$;j7>&3pQkP>asZe zdC$9nbNo43{~P)9v2t<-%hZ1AxqnTZCeGlk6Vq#vkIJob=d#nHZRN*xc*K-jb4f4i z8^VIf1=w1sd+ts0Bg>(e@CP$ol(Wq>7?=c`2mPbiuu>+AzM{`~eSP)Cl z4V%Gm-MZ_s-Wi{%yag?7?Q<p=*;9(^)8CYNAcfFFnE5F6Wn-kYBr;E(@Cu zi@O%!W&ho4kQv21pnRTtOV@_(n`lifjRhJL+~XP%F_i1I1 zkj@q7v*N}=R@_kN5M+PgQ)2-uuPi~UCT)9l+EcbVRLN7Y)*R-n}hzD(@-PL%2uZYywvWPZ`xv}wUsh0eAB`=ZCjO8 znMIy*9ZHa{n3X$G2fUbWCd!z(qugA?V<6XO&maOd=P>pf+UjbsVqE^YG73zTase*r1Z5la#s4aheE@SoQF*ce@U#Vi1&JwdwBCwLRxL)=fKDWbX z#D%tj`z_mHL!GtAGiRU5r*qH8dsiVjpQcLFgY$QcOC!fC{La^P73sd2(yhq!O7Vs*Q#r5V|ZOnIuzQEEkh%3}j#vAqoEF-F>skBu5b39E0snbiyH zm_~wEnc|cZ-HLCzJotX*G&Z~Y3O2j(ayDCNV>!Rws+`}Y@Vf-@1^=BIN6jfs8}k6w#JR_yWg6A*_`Q;0gEuFgtFg;isg{jt3V zf8K>Z(lLJ=Shq4&|IfJ+_hk4c$@RZbn8ykmF~`!pLb2Sa$&Ja(^c4EVT8#Nm&8oYL zSvAH@HQK$pts<+~pxdUwR>-OwV3Wtz@p+=+KdR>C$Y&#%xdA%wojqyw254OWO zc3!B%T-H4c^XW|VUA5A;LM5xtQkzh0ASN(R)nJ~gfj>13h=J)UHoc+PAZQCQKBu8> zU_+I)W`5aO_1#sh);5F9Lf_0-IDL1qq|O#*@>4YTQ{MorV%$8R^Lq}KzkLz%D^g{hLhF-pkaD zcQg0BjJ*zVuNBzI$Zz~-p`?J7*z#Bj;;#gKT{8pU&0Zx>6VfW*v&F2~nfsot!1&Ox z@|Q~-=)=n=^x?{n_aWWrDRYSi{Zfv8LI3Q=cTGLUdG~BqZ>zz0*J8X^F?lverweM_ zhpnA@>@0s6?EpEDYqQ#7#84@#Z7gB64YQ=`?2JrBs{Gj4Z5?E6Gvv)&AI>U9yv<^> zc2ArOYQ8|<(|T4X=%$=^B-Ep3ei5r}_o!+c?^o3d_o$R!;%$Zdmzpo3PVEn=>Kk{e z>V^ANm69f_j9(><6={@sRLLN^a?;}C2 zV(K0FhMLpx{cK173n#InERD1RLq7VO@}{EYdAJhwE+7OEEUevedAWJ zR?TWL*VJOJsl{Af+fZema8EjJ?!M{=`2RAH(@_AiXo~q{KFYU}APrHz|1^N)6U8Fh zO>V!npnQFqG9K_EVzo2hk!Mxz5M}>x&r8?QLHF^wBe@=WHuq^F>E51>dpJeTy_^}& zw>dxLtfD+|=s7Rov~b?S*~{tYTz!*r-ShnOE1ZWp-{UO3K}k24^8!vAr-So$&h4BJ zar!y;a;7;CasGz$1J2SLmGX3)3pf{XS~zdu{2FI3=T1(Ma}Vc!&eu4zoJTmvIHzym z_2s;f)57WG+{)?U^m6Xu+|T(M=Ubc-XFhNL*_`t@O`Ml<%AJcJZTTyDI-Hx$9Zqas zj6H*|>uBlf>BL6b>|_;Q<=@idbYjD6{zm_$Hg@VbYrDKlFpTV7vaZ~`5nIu}cDG_RH4f~Zhw@OItDC(+wq;#iH}*I#>giPW zmhJ4u)_Clfbmj_WAMVRLH?8kPiFf8L-MF}8ZO`@BcR0=1tQ}6WRKew)_BEZh4IQ>6 zuIsQd9(EppBUBl7M*+6AJ>ls?vj5#{u+_I?Y3I7GRct%*=e6!~UerOm{B`8Y*+Qw8 zciDP2bhOIFVF}A6OWH17vTR|)0vqjM{}y?yxQe|OZlLtbu$}!9r;~PsZzf;HeQpw# zP>WC6SeLQ=dCNN1+|uzedzG+Q-h_Vgi5thY^`vvnowk*Z^__CxvY~?ZW4i?JCLQi6 zI{`mNpN{SS`zU>qcMaYn5SFc9i%pj|vn17r8mHN2L(^kH{R?@^y4KkE`6Vv2hdsY- z&H9ZU$NGV>HkC(5Ct-Q_3ikEmElQDdDYlnD9y+p4j6in1Fn+t;M+(~c zCKKo!z5}oOcwS0TvOjMMlvmeH2yyt?+0o3hlPs&)iaZ3TL)kw0a#>flOuk*X2r<`! z&FMSXHd()DJ$A7oPvyKHlx>$Hh{?2y?Lh3HJ=fb>x@;Y0>}JmXoyux%F*DXr-d(n2 zIq5IR9+)ZM*avybdp2~hU$nU!8*8juZ|i8WuW_=9!kpUPb?iKAraYE(VrTlUPTG!M z*`{M7TI?~opQbuBmtt<*i7VDP*RNSi-d;^-R(2oj7YpT16``HG*?-a}r2zVjG9H<@ zw+JlByk*CC3&H27=^EZ2)S`@CCAa2EY?6*YoXL7yhtkeL;i3&a8|_ryqMmi@I-H26 zBDx;a!p5!*9hdcVJ9@gQwF<^rG5CvAQmRhOpQ)RlRbl_&VM0$G$2XN9e`B+5-MsZ=ZX9@1QThW4w|qBC zj}e&ZcDKwR(m&%n(ibG#Lq+{0}y5Ug6$J76G_!roFoPFtb#WT%E$~;c!-J=kni~T`}8>~IZ(cI5Ayke_WguIHR;IXN3QKCKIq7O zp4?wOeYrEA^PjQ>H28fALeKY!op|moL3e;|$QX3ux4FIx)Z#ffyIPR>1?ZG!9@xQk zJNPJs^4S9#@wZ~qP2e!ZglmaMxt(}wwUR~;uI0K7ybVJ6e+|^n#P9jI7LRP$9T2kn z!Qaoq`v71c1HYnG?5n{iAlqP1g70yiXsA)_#DjP~;z1gEPV!%Mj76az0ju!Lobo5m z=ehxW7?MUBKe(6Mli&+n&w>)^NW)G--t~;-uNIgHCLwC*DexIcDfA3@5Tb!zs$=X- zh}@pw60Wy`(@us@NTUWXgHReX=;V4gIKcHF_$GwfJPR&31<$>3tpV(Tklh77&Gi&m zI9IW&!7CxuZmYqkAXIJ!%tB1iC9ng(<1K^k22&6-r2#*LSfI0cSbG4mLN|b$Aa>|3 z@Wc67?g4!}zMw8KVBHP$Wnc(GzD2{aIEQ`&T!|O1QQx(L`iqrv zi9dl*y)xjgR>e+y6+(UvgCBDJD7fk?_`4|bw1U$wVeBw;4Y&ZZ54r)|1fgqF;Phom zxf;*_Av@8`b>aaC`H%(+mMi|(fjcfmdgQqiJjsmqxmI90@QgOp0s1`fv6YAu=zG9t zuV8F7^cTUOuR?vGXTgswn7g6VOVXdb3T4Vbr~OJMjW@)dIkcyg!W zvk9Ev1s!$+c!2|Lz6R|8Zh{QM?gEn#iW%awH>2HG)Q+;N*S zhwlX6f>4_r22c8$a;*+L3hBeO#Km7%(h%?A`gZVTuD=4Fdb^Up9=sbu_1X^F?@;m~ zCLwa0fbT#&$o~j9->u|t01rT@U((=V2-QU*+ZH8HViZF8>;ZoXk^Kj?TX7C~60d=% z@m${u-VdQUa0r}zC*l=p>cG8_Mx;rCeOO0eflhoMG7tI~==`QqZa4US2$huvf87f^ z@)-eJ?pExp!9({b^*sz$->cZQ;8LzH1Dz16mmAEjKOp|)eq4)kiE|#noB&-1z6+uH zz6b8`;AsTxB52=C|`3@G|) z@Wlc2lMQnP_{1P%+mR*-ekY84q3;5hN01lNm_ZMO+MM`%h#U{#`B6UqfISc^(!0Rl z#o!zC_Bhs%Kq$Q*T#-=blXmdY-N+Yd_JCi04D%aw6ZkU-#qD9Reh=;kVAp{@2(`Z- zEPNb(!mb8?z8Ci~(6ivZ`xw6D5%b_LAT&2-!J`oJf%xQ8%DlP{oc}cL*^u4<9)wUE zz5;&h`$}0uV08+8gfv?4WeEB93izjI5YMo)XE81y7U)C|guX|Jr#*)}VK;$hhy%I@ zd;;Qvo&*m(kGexogC9W1&oQw22Rv@Uog_#@e2m-ofCnIyUYExE;2`^uhSn=Q1tEJ1 ztok=81aPNcOuq!Vc!4(UW%??O6})}D|~ zq_rBP6IXJb)@Hc5&i>BFh4mPhkn17mKo&#VAvQ=OWCNrX;)XaNCWskgg%}{cknzV! zr(#|^4e<>=04aw4doT;JLjDXn0ND#ML4FDmAufmx^7iRySI9xgbCAa&{gC@0n;{*L znP;GlAp+!GJ!8Lu9E3at2|*r&IKP0{gj@r;4AKl)2ssrp3sMC6{6Gt2F*Y$YbzZ&K z;BE4nyk@W6>+rg~Zf~#G;|+R;yh-nnFXhYlvOdYj{2IT`Z}6M^X1~?%@VosUzvv(G zr~Daz)-U;4Koigf3;|QX9Iyr)0e8R?5CcPjR3H<`2BZKR&J9tN`ow@3F?A|pebk$ zT7!JvnblBiP}wHijfMo_ac)J=`rX;D8tYS@H2wxX66)YFcd zx=`0%)V2@x4Wh56kr)L(SEwyB4+Aqy9~3fmXDE1+8F5JGjsiy=aR*v_=r^ zkwlB6(I&%al@YYd7+OY+w$Y+>^k|uh-}C_4!0!&^P2u`bLnG8oB6^Lu<$qvWHxu-cVmC7)plHq2bU-Xe^`-Ys30* zQ@Ay33ERW2aBsLT91JJJ>F{uPBs>;YN3;=rq$$!Gu|(_{Mb$BFOdo5CwZ<$ld(0Ku54JCC;@VlC|-_K*0-{OW)UxT2KoZQKr)aH3y)5sOSGFcJN4 zLa$rV<8Jh}h@MtPfb>s0-c#XBI2)G2ETW0%B8G@5VvblNj)*(riHMP*NGg(vWFu09 zMKw`f)DSgA%~5OA5p_pBQ879cO+_=&Y*dP}m?ox+8DgfGIcAMHV(yqHCdP(hsaPhK zjY%;U*Ti*kL);WM$E|Tk+#UDC#rRM>70<-8aVgFcnuIQ4NSG4lgf-zvxD%d)m>5c= z5}8CcAtjhWkk|DyQ6uU^gJ=@XqE&Q=ZqXx(;*gjUGh$YhMAonA*Yz9vP5tJ6YrmtP znen>>VwmP5tvuVLRj>1gPSYUHX#zL|g)abiJ?^dUNw`HJSKsb;9j^AlzC K`SFk10{;VK0U*Z! literal 0 HcmV?d00001 diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.obj b/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.obj new file mode 100644 index 0000000000000000000000000000000000000000..6167f7c578fea2c0723f8b77f87d155662335022 GIT binary patch literal 2062 zcmb_dO>7%g5T2KH+E6t#KU4}8l~!g2K|)*GaUB{!Y;4CQ$WB6jXe4iCHr{o*t-Wh` z*KWwEYEUK1R6?Q`E>&;zR(dKNQhtKOpNfD32Tltxr61Bj8{yzkBX zX7;_=^=JM10941{DDOo8qTrdVH;l5|GW;c+L9oKQD}O#V1hQ_dH#el^?#>>9?H-;2 zLupypT&+vOc^pT%51@mCtRII0%ExjeOx(fvj?W<;8S)g_H8x$TW_b~S4-jk#013PU z*%2HfEWR#phU*Pr-10fZBU2!z$n`MqIL2GZyXJGTw8tx7yzHcsl*Ua#f4iq19K&ae z;+gDBZhAGplAlXYMbTvnE75RpA)lR}F3c=PMXK;Cj_+~Y1U8HlWTwb4y)sn$@0Arl4Ima%R|_V@}Vkt*Lw;!C_Rk*Dt{oyfDnx0ltM%I1Rf^IG77&xnVnY#TCbw zo)H)7u30m;Olw2zR-nuL3!~OEpci$5qapOv5C&uDM>!JFpX6w6KDRJc_HR%uEWehy zfN_9Oywt)znzmJpaf_dmJ&@pFA{I|h`jJ@7OZW2Y*wl*Ms1?tfmTq5haF5s38s&?{ z%~Z0xc3kYd*3e73QPG-Jw-~1_Zk&(bs$tOqoCC%}E_VTD+bhygJ1-5)ZvQg&c|@X& z*>YAAXFG$0PdtdQ&T&cjqVq{RBK`NB8~;i_QjAF@v^-FWxj&oAD;n;L!Ry+Zq^ho7Dsaq6qLems94lesDM zdB~=|+w5oi`&3b>7@CW3{B6dr`jM7hm1{cmVTKn8Dj*4(z(@nb7F0jV6xd#vfUBVL zk%|b6w2$;IY;cLK9KVB6W*TJxg!wNb;=eP-H`0gpV#hsi2OYtat;Jl{wVGUjKqe{ zI`VKd@JjrH0Ox=l(0txUreH?czQW~8rd6>^T`f|SiN%?8b|&q|eF?(wtLbbv*cF&w nG6_s?4Ap(3scjggUbXp0;@@TGkSaB;D=2CbZ??q${XF~^BZz@8 literal 0 HcmV?d00001 diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log b/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log new file mode 100644 index 00000000..057c5d42 --- /dev/null +++ b/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log @@ -0,0 +1,65 @@ +The system is: Windows - 10.0.19044 - AMD64 +Compiling the C compiler identification source file "CMakeCCompilerId.c" succeeded. +Compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe +Build flags: +Id flags: + +The output was: +0 +Microsoft (R) C/C++ Optimizing Compiler Versão 19.29.30147 para x64 +Copyright (C) Microsoft Corporation. Todos os direitos reservados. + +CMakeCCompilerId.c +Microsoft (R) Incremental Linker Version 14.29.30147.0 +Copyright (C) Microsoft Corporation. All rights reserved. + +/out:CMakeCCompilerId.exe +CMakeCCompilerId.obj + + +Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "CMakeCCompilerId.exe" + +Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "CMakeCCompilerId.obj" + +The C compiler identification is MSVC, found in "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdC/CMakeCCompilerId.exe" + +Compiling the CXX compiler identification source file "CMakeCXXCompilerId.cpp" succeeded. +Compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe +Build flags: +Id flags: + +The output was: +0 +Microsoft (R) C/C++ Optimizing Compiler Versão 19.29.30147 para x64 +Copyright (C) Microsoft Corporation. Todos os direitos reservados. + +CMakeCXXCompilerId.cpp +Microsoft (R) Incremental Linker Version 14.29.30147.0 +Copyright (C) Microsoft Corporation. All rights reserved. + +/out:CMakeCXXCompilerId.exe +CMakeCXXCompilerId.obj + + +Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "CMakeCXXCompilerId.exe" + +Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "CMakeCXXCompilerId.obj" + +The CXX compiler identification is MSVC, found in "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/3.25.0/CompilerIdCXX/CMakeCXXCompilerId.exe" + +Detecting C compiler ABI info compiled with the following output: +Change Dir: C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeScratch/TryCompile-coepre + +Run Build Command(s):C:/PROGRA~2/MIB055~1/2019/COMMUN~1/Common7/IDE/COMMON~1/MICROS~1/CMake/Ninja/ninja.exe cmTC_ab3b6 && [1/2] Building C object CMakeFiles\cmTC_ab3b6.dir\CMakeCCompilerABI.c.obj +[2/2] Linking C executable cmTC_ab3b6.exe + + + +Detecting CXX compiler ABI info compiled with the following output: +Change Dir: C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeScratch/TryCompile-me7b8n + +Run Build Command(s):C:/PROGRA~2/MIB055~1/2019/COMMUN~1/Common7/IDE/COMMON~1/MICROS~1/CMake/Ninja/ninja.exe cmTC_0894c && [1/2] Building CXX object CMakeFiles\cmTC_0894c.dir\CMakeCXXCompilerABI.cpp.obj +[2/2] Linking CXX executable cmTC_0894c.exe + + + diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/foo.h b/flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/foo.h new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/foo.h @@ -0,0 +1 @@ + diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/main.c b/flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/main.c new file mode 100644 index 00000000..cd3cbc1f --- /dev/null +++ b/flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/main.c @@ -0,0 +1,2 @@ +#include "foo.h" +int main(){} diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/main.obj b/flatland_viz/build/flatland_viz/CMakeFiles/ShowIncludes/main.obj new file mode 100644 index 0000000000000000000000000000000000000000..d1652e5e9e840ea5c018a138b22286b30f843dea GIT binary patch literal 682 zcmYdkV`0eAs7yY`$iTqE00DX_MXAXpWvNgugFcY41;&BVTmlSwDXB@N=_6N5bl&Iw9!K9dhjMYHmlUPO1bN1!}jIvQv0P^ zr+wHvJMLbx_i>fa8!w#t0`d|FI3xoV>t&`KE@ff-|DS<@nZY3~HL*m`z+eI+V*^yA zEI&stH-&)-s2(DM9N(beW@ZG%I4VGnYmiQ6Mo?^{0_1oG#W6D@hy;ON`CacJEKo>5 YSRmIa09}C`>!7G&W>kPFfYTsx0N10BKL7v# literal 0 HcmV?d00001 diff --git a/flatland_viz/build/flatland_viz/CMakeFiles/cmake.check_cache b/flatland_viz/build/flatland_viz/CMakeFiles/cmake.check_cache new file mode 100644 index 00000000..3dccd731 --- /dev/null +++ b/flatland_viz/build/flatland_viz/CMakeFiles/cmake.check_cache @@ -0,0 +1 @@ +# This file is generated by cmake for dependency checking of the CMakeCache.txt file diff --git a/flatland_viz/build/flatland_viz/cmake_args.last b/flatland_viz/build/flatland_viz/cmake_args.last new file mode 100644 index 00000000..ad693462 --- /dev/null +++ b/flatland_viz/build/flatland_viz/cmake_args.last @@ -0,0 +1 @@ +['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja', '-DCATKIN_INSTALL_INTO_PREFIX_ROOT=0'] \ No newline at end of file diff --git a/flatland_viz/build/flatland_viz/colcon_build.rc b/flatland_viz/build/flatland_viz/colcon_build.rc new file mode 100644 index 00000000..d00491fd --- /dev/null +++ b/flatland_viz/build/flatland_viz/colcon_build.rc @@ -0,0 +1 @@ +1 diff --git a/flatland_viz/build/flatland_viz/colcon_command_prefix_build.bat b/flatland_viz/build/flatland_viz/colcon_command_prefix_build.bat new file mode 100644 index 00000000..84bcfb68 --- /dev/null +++ b/flatland_viz/build/flatland_viz/colcon_command_prefix_build.bat @@ -0,0 +1,2 @@ +:: generated from colcon_core/shell/template/command_prefix.bat.em +@echo off diff --git a/flatland_viz/build/flatland_viz/colcon_command_prefix_build.bat.env b/flatland_viz/build/flatland_viz/colcon_command_prefix_build.bat.env new file mode 100644 index 00000000..b2048369 --- /dev/null +++ b/flatland_viz/build/flatland_viz/colcon_command_prefix_build.bat.env @@ -0,0 +1,50 @@ +ALLUSERSPROFILE=C:\ProgramData +APPDATA=C:\Users\PC\AppData\Roaming +CARBON_MEM_DISABLE=1 +CLion=C:\Program Files\JetBrains\CLion 2021.3.3\bin; +COLCON=1 +COMPUTERNAME=DESKTOP-S1BRI82 +ChocolateyInstall=C:\ProgramData\chocolatey +ChocolateyLastPathUpdate=132961487583511216 +ComSpec=C:\Windows\system32\cmd.exe +CommonProgramFiles=C:\Program Files\Common Files +CommonProgramFiles(x86)=C:\Program Files (x86)\Common Files +CommonProgramW6432=C:\Program Files\Common Files +DriverData=C:\Windows\System32\Drivers\DriverData +HOMEDRIVE=C: +HOMEPATH=\Users\PC +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; +LOCALAPPDATA=C:\Users\PC\AppData\Local +LOGONSERVER=\\DESKTOP-S1BRI82 +NUMBER_OF_PROCESSORS=12 +OPENSSL_CONF=C:\Program Files\OpenSSL-Win64\bin\openssl.cfg +OS=Windows_NT +OneDrive=C:\Users\PC\OneDrive +OpenCV_DIR=C:\opencv +PATHEXT=.COM;.EXE;.BAT;.CMD;.VBS;.VBE;.JS;.JSE;.WSF;.WSH;.MSC;.PY;.PYW +PROCESSOR_ARCHITECTURE=AMD64 +PROCESSOR_IDENTIFIER=Intel64 Family 6 Model 158 Stepping 10, GenuineIntel +PROCESSOR_LEVEL=6 +PROCESSOR_REVISION=9e0a +PROMPT=$P$G +PSModulePath=C:\Program Files\WindowsPowerShell\Modules;C:\Windows\system32\WindowsPowerShell\v1.0\Modules +PUBLIC=C:\Users\Public +PYTHONPATH=C:\Python38\ +Path=C:\Python38\Scripts;C:\Python38\;C:\ProgramData\QFS\QF-Test\qftestpath;C:\Program Files\Common Files\Oracle\Java\javapath;C:\Program Files (x86)\Common Files\Oracle\Java\javapath;C:\Windows\system32;C:\Windows;C:\Windows\System32\Wbem;C:\Windows\System32\WindowsPowerShell\v1.0\;C:\Windows\System32\OpenSSH\;C:\Program Files\Intel\WiFi\bin\;C:\Program Files\Common Files\Intel\WirelessCommon\;C:\Program Files\Microsoft VS Code\bin;C:\Program Files\NVIDIA Corporation\NVIDIA NvDLISR;C:\Program Files (x86)\NVIDIA Corporation\PhysX\Common;C:\Program Files\Git\cmd;C:\Program Files\Microchip\xc8\v2.31\bin;C:\Program Files\nodejs\;C:\ProgramData\chocolatey\bin;C:\Users\PC\Documents\Aulas\4ano\1semestre\SETR\avrdude-v7.0-rc1-windows-x64;C:\Users\PC\Documents\Aulas\4ano\1semestre\SETR\arduino-cli_0.21.1_Windows_64bit;C:\Program Files\CMake\bin;C:\Program Files\rti_connext_dds-5.3.1\bin;C:\Program Files\Graphviz\bin;C:\Program Files\OpenSSL-Win64\bin;C:\opencv\x64\vc15\bin;C:\Ninja;C:\Users\PC\AppData\Local\Microsoft\WindowsApps;C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin;C:\Program Files\JetBrains\CLion 2021.3.3\bin;C:\Users\PC\AppData\Roaming\npm;C:\Program Files\heroku\bin;C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +ProgramData=C:\ProgramData +ProgramFiles=C:\Program Files +ProgramFiles(x86)=C:\Program Files (x86) +ProgramW6432=C:\Program Files +QT_QPA_PLATFORM_PLUGIN_PATH=C:\Qt\Qt5.12.12\5.12.12\msvc2017_64\plugins\platforms +Qt5_DIR=C:\Qt\Qt5.12.12\5.12.12\msvc2017_64 +SystemDrive=C: +SystemRoot=C:\Windows +TEMP=C:\Users\PC\AppData\Local\Temp +TMP=C:\Users\PC\AppData\Local\Temp +USERDOMAIN=DESKTOP-S1BRI82 +USERDOMAIN_ROAMINGPROFILE=DESKTOP-S1BRI82 +USERNAME=PC +USERPROFILE=C:\Users\PC +ZES_ENABLE_SYSMAN=1 +windir=C:\Windows diff --git a/flatland_viz/build/flatland_viz/colcon_command_prefix_build.ps1 b/flatland_viz/build/flatland_viz/colcon_command_prefix_build.ps1 new file mode 100644 index 00000000..827cdc74 --- /dev/null +++ b/flatland_viz/build/flatland_viz/colcon_command_prefix_build.ps1 @@ -0,0 +1 @@ +# generated from colcon_powershell/shell/template/command_prefix.ps1.em diff --git a/flatland_viz/build/flatland_viz/colcon_command_prefix_build.ps1.env b/flatland_viz/build/flatland_viz/colcon_command_prefix_build.ps1.env new file mode 100644 index 00000000..79f84d3d --- /dev/null +++ b/flatland_viz/build/flatland_viz/colcon_command_prefix_build.ps1.env @@ -0,0 +1,56 @@ +ALLUSERSPROFILE=C:\ProgramData +AMENT_PREFIX_PATH=C:\dev\ros2_humble; +APPDATA=C:\Users\PC\AppData\Roaming +CARBON_MEM_DISABLE=1 +CLion=C:\Program Files\JetBrains\CLion 2021.3.3\bin; +CMAKE_PREFIX_PATH=C:\dev\ros2_humble; +COLCON=1 +COMPUTERNAME=DESKTOP-S1BRI82 +ChocolateyInstall=C:\ProgramData\chocolatey +ChocolateyLastPathUpdate=132961487583511216 +ComSpec=C:\Windows\system32\cmd.exe +CommonProgramFiles=C:\Program Files\Common Files +CommonProgramFiles(x86)=C:\Program Files (x86)\Common Files +CommonProgramW6432=C:\Program Files\Common Files +DriverData=C:\Windows\System32\Drivers\DriverData +HOMEDRIVE=C: +HOMEPATH=\Users\PC +LOCALAPPDATA=C:\Users\PC\AppData\Local +LOGONSERVER=\\DESKTOP-S1BRI82 +NUMBER_OF_PROCESSORS=12 +OPENSSL_CONF=C:\Program Files\OpenSSL-Win64\bin\openssl.cfg +OS=Windows_NT +OneDrive=C:\Users\PC\OneDrive +OpenCV_DIR=C:\opencv +PATHEXT=.COM;.EXE;.BAT;.CMD;.VBS;.VBE;.JS;.JSE;.WSF;.WSH;.MSC;.PY;.PYW;.CPL +PKG_CONFIG_PATH=C:\dev\ros2_humble\lib\pkgconfig; +PROCESSOR_ARCHITECTURE=AMD64 +PROCESSOR_IDENTIFIER=Intel64 Family 6 Model 158 Stepping 10, GenuineIntel +PROCESSOR_LEVEL=6 +PROCESSOR_REVISION=9e0a +PSModulePath=C:\Users\PC\Documents\WindowsPowerShell\Modules;C:\Program Files\WindowsPowerShell\Modules;C:\Windows\system32\WindowsPowerShell\v1.0\Modules +PUBLIC=C:\Users\Public +PYTHONPATH=C:\ci\ws\install\lib\python3.8\dist-packages;C:\dev\ros2_humble\Lib\site-packages;C:\Python38\ +Path=C:\dev\ros2_humble\opt\yaml_cpp_vendor\bin;C:\dev\ros2_humble\opt\rviz_ogre_vendor\bin;C:\dev\ros2_humble\opt\rviz_assimp_vendor\bin;C:\dev\ros2_humble\opt\libcurl_vendor\bin;C:\dev\ros2_humble\Scripts;C:\dev\ros2_humble\bin;C:\Python38\Scripts;C:\Python38\;C:\ProgramData\QFS\QF-Test\qftestpath;C:\Program Files\Common Files\Oracle\Java\javapath;C:\Program Files (x86)\Common Files\Oracle\Java\javapath;C:\Windows\system32;C:\Windows;C:\Windows\System32\Wbem;C:\Windows\System32\WindowsPowerShell\v1.0\;C:\Windows\System32\OpenSSH\;C:\Program Files\Intel\WiFi\bin\;C:\Program Files\Common Files\Intel\WirelessCommon\;C:\Program Files\Microsoft VS Code\bin;C:\Program Files\NVIDIA Corporation\NVIDIA NvDLISR;C:\Program Files (x86)\NVIDIA Corporation\PhysX\Common;C:\Program Files\Git\cmd;C:\Program Files\Microchip\xc8\v2.31\bin;C:\Program Files\nodejs\;C:\ProgramData\chocolatey\bin;C:\Users\PC\Documents\Aulas\4ano\1semestre\SETR\avrdude-v7.0-rc1-windows-x64;C:\Users\PC\Documents\Aulas\4ano\1semestre\SETR\arduino-cli_0.21.1_Windows_64bit;C:\Program Files\CMake\bin;C:\Program Files\rti_connext_dds-5.3.1\bin;C:\Program Files\Graphviz\bin;C:\Program Files\OpenSSL-Win64\bin;C:\opencv\x64\vc15\bin;C:\Users\PC\AppData\Local\Microsoft\WindowsApps;C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin;C:\Program Files\JetBrains\CLion 2021.3.3\bin;C:\Users\PC\AppData\Roaming\npm;C:\Program Files\heroku\bin;C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +ProgramData=C:\ProgramData +ProgramFiles=C:\Program Files +ProgramFiles(x86)=C:\Program Files (x86) +ProgramW6432=C:\Program Files +QT_QPA_PLATFORM_PLUGIN_PATH=C:\Qt\Qt5.12.12\5.12.12\msvc2017_64\plugins\platforms +Qt5_DIR=C:\Qt\Qt5.12.12\5.12.12\msvc2017_64 +ROS_DISTRO=humble +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +ROS_LOCALHOST_ONLY=0 +ROS_PYTHON_VERSION=3 +ROS_VERSION=2 +SystemDrive=C: +SystemRoot=C:\Windows +TEMP=C:\Users\PC\AppData\Local\Temp +TMP=C:\Users\PC\AppData\Local\Temp +USERDOMAIN=DESKTOP-S1BRI82 +USERDOMAIN_ROAMINGPROFILE=DESKTOP-S1BRI82 +USERNAME=PC +USERPROFILE=C:\Users\PC +ZES_ENABLE_SYSMAN=1 +windir=C:\Windows diff --git a/flatland_viz/install/.colcon_install_layout b/flatland_viz/install/.colcon_install_layout new file mode 100644 index 00000000..3aad5336 --- /dev/null +++ b/flatland_viz/install/.colcon_install_layout @@ -0,0 +1 @@ +isolated diff --git a/flatland_viz/install/COLCON_IGNORE b/flatland_viz/install/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/flatland_viz/install/_local_setup_util_bat.py b/flatland_viz/install/_local_setup_util_bat.py new file mode 100644 index 00000000..0d499269 --- /dev/null +++ b/flatland_viz/install/_local_setup_util_bat.py @@ -0,0 +1,404 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = ':: {comment}' +FORMAT_STR_SET_ENV_VAR = 'set "{name}={value}"' +FORMAT_STR_USE_ENV_VAR = '%{name}%' +FORMAT_STR_INVOKE_SCRIPT = 'call:_colcon_prefix_bat_call_script "{script_path}"' +FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'call:_colcon_prefix_bat_strip_leading_semicolon "{name}"' +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'call:_colcon_prefix_bat_strip_trailing_semicolon "{name}"' + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/flatland_viz/install/_local_setup_util_ps1.py b/flatland_viz/install/_local_setup_util_ps1.py new file mode 100644 index 00000000..98348eeb --- /dev/null +++ b/flatland_viz/install/_local_setup_util_ps1.py @@ -0,0 +1,404 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' +FORMAT_STR_USE_ENV_VAR = '$env:{name}' +FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' +FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/flatland_viz/install/flatland_viz/.catkin b/flatland_viz/install/flatland_viz/.catkin new file mode 100644 index 00000000..e69de29b diff --git a/flatland_viz/install/flatland_viz/share/colcon-core/packages/flatland_viz b/flatland_viz/install/flatland_viz/share/colcon-core/packages/flatland_viz new file mode 100644 index 00000000..7d2c1877 --- /dev/null +++ b/flatland_viz/install/flatland_viz/share/colcon-core/packages/flatland_viz @@ -0,0 +1 @@ +flatland_msgs;flatland_server;libogre-dev;libqt5-core;libqt5-gui;libqt5-widgets;qtbase5-dev;roscpp;rviz \ No newline at end of file diff --git a/flatland_viz/install/flatland_viz/share/flatland_viz/package.bat b/flatland_viz/install/flatland_viz/share/flatland_viz/package.bat new file mode 100644 index 00000000..f21f4fef --- /dev/null +++ b/flatland_viz/install/flatland_viz/share/flatland_viz/package.bat @@ -0,0 +1,16 @@ +:: generated from colcon_core/shell/template/package.bat.em +@echo off + +goto :eof + + +:: call the specified batch file and output the name when tracing is requested +:: first argument: the batch file +:call_file + if exist "%~1" ( + if "%COLCON_TRACE%" NEQ "" echo call "%~1" + call "%~1%" + ) else ( + echo not found: "%~1" 1>&2 + ) +goto:eof diff --git a/flatland_viz/install/flatland_viz/share/flatland_viz/package.dsv b/flatland_viz/install/flatland_viz/share/flatland_viz/package.dsv new file mode 100644 index 00000000..e69de29b diff --git a/flatland_viz/install/flatland_viz/share/flatland_viz/package.ps1 b/flatland_viz/install/flatland_viz/share/flatland_viz/package.ps1 new file mode 100644 index 00000000..4198e42e --- /dev/null +++ b/flatland_viz/install/flatland_viz/share/flatland_viz/package.ps1 @@ -0,0 +1,108 @@ +# generated from colcon_powershell/shell/template/package.ps1.em + +# function to append a value to a variable +# which uses colons as separators +# duplicates as well as leading separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +function colcon_append_unique_value { + param ( + $_listname, + $_value + ) + + # get values from variable + if (Test-Path Env:$_listname) { + $_values=(Get-Item env:$_listname).Value + } else { + $_values="" + } + $_duplicate="" + # start with no values + $_all_values="" + # iterate over existing values in the variable + if ($_values) { + $_values.Split(";") | ForEach { + # not an empty string + if ($_) { + # not a duplicate of _value + if ($_ -eq $_value) { + $_duplicate="1" + } + if ($_all_values) { + $_all_values="${_all_values};$_" + } else { + $_all_values="$_" + } + } + } + } + # append only non-duplicates + if (!$_duplicate) { + # avoid leading separator + if ($_all_values) { + $_all_values="${_all_values};${_value}" + } else { + $_all_values="${_value}" + } + } + + # export the updated variable + Set-Item env:\$_listname -Value "$_all_values" +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +function colcon_prepend_unique_value { + param ( + $_listname, + $_value + ) + + # get values from variable + if (Test-Path Env:$_listname) { + $_values=(Get-Item env:$_listname).Value + } else { + $_values="" + } + # start with the new value + $_all_values="$_value" + # iterate over existing values in the variable + if ($_values) { + $_values.Split(";") | ForEach { + # not an empty string + if ($_) { + # not a duplicate of _value + if ($_ -ne $_value) { + # keep non-duplicate values + $_all_values="${_all_values};$_" + } + } + } + } + # export the updated variable + Set-Item env:\$_listname -Value "$_all_values" +} + +# function to source another script with conditional trace output +# first argument: the path of the script +# additional arguments: arguments to the script +function colcon_package_source_powershell_script { + param ( + $_colcon_package_source_powershell_script + ) + # source script with conditional trace output + if (Test-Path $_colcon_package_source_powershell_script) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_package_source_powershell_script'" + } + . "$_colcon_package_source_powershell_script" + } else { + Write-Error "not found: '$_colcon_package_source_powershell_script'" + } +} + + diff --git a/flatland_viz/install/local_setup.bat b/flatland_viz/install/local_setup.bat new file mode 100644 index 00000000..e6f53a02 --- /dev/null +++ b/flatland_viz/install/local_setup.bat @@ -0,0 +1,134 @@ +:: generated from colcon_core/shell/template/prefix.bat.em +@echo off + +:: This script extends the environment with all packages contained in this +:: prefix path. + +:: add this prefix to the COLCON_PREFIX_PATH +call:_colcon_prefix_bat_prepend_unique_value COLCON_PREFIX_PATH "%%~dp0" + +:: get and run all commands in topological order +call:_colcon_run_ordered_commands "%~dp0" + +goto:eof + + +:: function to prepend a value to a variable +:: which uses semicolons as separators +:: duplicates as well as trailing separators are avoided +:: first argument: the name of the result variable +:: second argument: the value to be prepended +:_colcon_prefix_bat_prepend_unique_value + setlocal enabledelayedexpansion + :: arguments + set "listname=%~1" + set "value=%~2" + + :: get values from variable + set "values=!%listname%!" + :: start with the new value + set "all_values=%value%" + :: skip loop if values is empty + if "%values%" NEQ "" ( + :: iterate over existing values in the variable + for %%v in ("%values:;=";"%") do ( + :: ignore empty strings + if "%%~v" NEQ "" ( + :: ignore duplicates of value + if "%%~v" NEQ "%value%" ( + :: keep non-duplicate values + set "all_values=!all_values!;%%~v" + ) + ) + ) + ) + :: set result variable in parent scope + endlocal & ( + set "%~1=%all_values%" + ) +goto:eof + + +:: Run the commands in topological order +:: first argument: the base path to look for packages +:_colcon_run_ordered_commands + setlocal enabledelayedexpansion + :: check environment variable for custom Python executable + if "%COLCON_PYTHON_EXECUTABLE%" NEQ "" ( + if not exist "%COLCON_PYTHON_EXECUTABLE%" ( + echo error: COLCON_PYTHON_EXECUTABLE '%COLCON_PYTHON_EXECUTABLE%' doesn't exist + exit /b 1 + ) + set "_colcon_python_executable=%COLCON_PYTHON_EXECUTABLE%" + ) else ( + :: use the Python executable known at configure time + set "_colcon_python_executable=c:\python38\python.exe" + :: if it doesn't exist try a fall back + if not exist "!_colcon_python_executable!" ( + python --version > NUL 2> NUL + if errorlevel 1 ( + echo error: unable to find python executable + exit /b 1 + ) + set "_colcon_python_executable=python" + ) + ) + + endlocal & ( + set "_colcon_python_executable=%_colcon_python_executable%" + ) + + :: escape potential closing parenthesis which would break the for loop + set "_colcon_python_executable=%_colcon_python_executable:)=^)%" + for /f "delims=" %%c in ('""%_colcon_python_executable%" "%~1_local_setup_util_bat.py" bat"') do ( + if "%COLCON_TRACE%" NEQ "" ( + echo %%c + ) + :: only invoke non-comment lines + echo %%c | findstr /r "^::" >nul 2>&1 + if errorlevel 1 ( + call %%c + ) + ) + set _colcon_python_executable= +goto:eof + + +:: call the specified batch file and output the name when tracing is requested +:: first argument: the batch file +:_colcon_prefix_bat_call_script + if exist "%~1" ( + if "%COLCON_TRACE%" NEQ "" echo call "%~1" + call "%~1%" + ) else ( + echo not found: "%~1" 1>&2 + ) +goto:eof + + +:: strip a leading semicolon from an environment variable if applicable +:: first argument: the environment variable name +:_colcon_prefix_bat_strip_leading_semicolon + setlocal enabledelayedexpansion + set "name=%~1" + set "value=!%name%!" + if "%value:~0,1%"==";" set "value=%value:~1%" + :: set result variable in parent scope + endlocal & ( + set "%~1=%value%" + ) +goto:eof + + +:: strip a trailing semicolon from an environment variable if applicable +:: first argument: the environment variable name +:_colcon_prefix_bat_strip_trailing_semicolon + setlocal enabledelayedexpansion + set "name=%~1" + set "value=!%name%!" + if "%value:~-1%"==";" set "value=%value:~0,-1%" + :: set result variable in parent scope + endlocal & ( + set "%~1=%value%" + ) +goto:eof diff --git a/flatland_viz/install/local_setup.ps1 b/flatland_viz/install/local_setup.ps1 new file mode 100644 index 00000000..30fb098f --- /dev/null +++ b/flatland_viz/install/local_setup.ps1 @@ -0,0 +1,55 @@ +# generated from colcon_powershell/shell/template/prefix.ps1.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# check environment variable for custom Python executable +if ($env:COLCON_PYTHON_EXECUTABLE) { + if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { + echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" + exit 1 + } + $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" +} else { + # use the Python executable known at configure time + $_colcon_python_executable="c:\python38\python.exe" + # if it doesn't exist try a fall back + if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { + if (!(Get-Command "python" -ErrorAction SilentlyContinue)) { + echo "error: unable to find python executable" + exit 1 + } + $_colcon_python_executable="python" + } +} + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_powershell_source_script { + param ( + $_colcon_prefix_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_powershell_source_script_param'" + } + . "$_colcon_prefix_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" + } +} + +# get all commands in topological order +$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 + +# execute all commands in topological order +if ($env:COLCON_TRACE) { + echo "Execute generated script:" + echo "<<<" + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output + echo ">>>" +} +if ($_colcon_ordered_commands) { + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression +} diff --git a/flatland_viz/install/setup.bat b/flatland_viz/install/setup.bat new file mode 100644 index 00000000..7110f881 --- /dev/null +++ b/flatland_viz/install/setup.bat @@ -0,0 +1,23 @@ +:: generated from colcon_core/shell/template/prefix_chain.bat.em +@echo off + +:: This script extends the environment with the environment of other prefix +:: paths which were sourced when this file was generated as well as all +:: packages contained in this prefix path. + +:: source this prefix +call:_colcon_prefix_chain_bat_call_script "%%~dp0local_setup.bat" + +goto:eof + + +:: function to source another script with conditional trace output +:: first argument: the path of the script +:_colcon_prefix_chain_bat_call_script + if exist "%~1" ( + if "%COLCON_TRACE%" NEQ "" echo call "%~1" + call "%~1%" + ) else ( + echo not found: "%~1" 1>&2 + ) +goto:eof diff --git a/flatland_viz/install/setup.ps1 b/flatland_viz/install/setup.ps1 new file mode 100644 index 00000000..fe4e8385 --- /dev/null +++ b/flatland_viz/install/setup.ps1 @@ -0,0 +1,26 @@ +# generated from colcon_powershell/shell/template/prefix_chain.ps1.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_chain_powershell_source_script { + param ( + $_colcon_prefix_chain_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_chain_powershell_source_script_param'" + } + . "$_colcon_prefix_chain_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" + } +} + +# source this prefix +$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) +_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/flatland_viz/log/COLCON_IGNORE b/flatland_viz/log/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/flatland_viz/log/build_2022-11-19_18-56-57/events.log b/flatland_viz/log/build_2022-11-19_18-56-57/events.log new file mode 100644 index 00000000..428f08f4 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_18-56-57/events.log @@ -0,0 +1,30 @@ +[0.000000] (-) TimerEvent: {} +[0.000000] (flatland_viz) JobQueued: {'identifier': 'flatland_viz', 'dependencies': OrderedDict()} +[0.000000] (flatland_viz) JobStarted: {'identifier': 'flatland_viz'} +[0.125000] (-) TimerEvent: {} +[0.234000] (-) TimerEvent: {} +[0.343000] (-) TimerEvent: {} +[0.453000] (-) TimerEvent: {} +[0.562000] (-) TimerEvent: {} +[0.672000] (-) TimerEvent: {} +[0.781000] (-) TimerEvent: {} +[0.890000] (-) TimerEvent: {} +[0.937000] (flatland_viz) JobProgress: {'identifier': 'flatland_viz', 'progress': 'cmake'} +[0.937000] (flatland_viz) Command: {'cmd': ['C:\\Program Files\\CMake\\bin\\cmake.EXE', 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz', '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja', '-DCATKIN_INSTALL_INTO_PREFIX_ROOT=0', '-DCMAKE_INSTALL_PREFIX=C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\install\\flatland_viz'], 'cwd': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\build\\flatland_viz', 'env': OrderedDict([('SystemDrive', 'C:'), ('ProgramFiles(x86)', 'C:\\Program Files (x86)'), ('Path', 'C:\\dev\\ros2_humble\\opt\\yaml_cpp_vendor\\bin;C:\\dev\\ros2_humble\\opt\\rviz_ogre_vendor\\bin;C:\\dev\\ros2_humble\\opt\\rviz_assimp_vendor\\bin;C:\\dev\\ros2_humble\\opt\\libcurl_vendor\\bin;C:\\dev\\ros2_humble\\Scripts;C:\\dev\\ros2_humble\\bin;C:\\Python38\\Scripts;C:\\Python38\\;C:\\ProgramData\\QFS\\QF-Test\\qftestpath;C:\\Program Files\\Common Files\\Oracle\\Java\\javapath;C:\\Program Files (x86)\\Common Files\\Oracle\\Java\\javapath;C:\\Windows\\system32;C:\\Windows;C:\\Windows\\System32\\Wbem;C:\\Windows\\System32\\WindowsPowerShell\\v1.0\\;C:\\Windows\\System32\\OpenSSH\\;C:\\Program Files\\Intel\\WiFi\\bin\\;C:\\Program Files\\Common Files\\Intel\\WirelessCommon\\;C:\\Program Files\\Microsoft VS Code\\bin;C:\\Program Files\\NVIDIA Corporation\\NVIDIA NvDLISR;C:\\Program Files (x86)\\NVIDIA Corporation\\PhysX\\Common;C:\\Program Files\\Git\\cmd;C:\\Program Files\\Microchip\\xc8\\v2.31\\bin;C:\\Program Files\\nodejs\\;C:\\ProgramData\\chocolatey\\bin;C:\\Users\\PC\\Documents\\Aulas\\4ano\\1semestre\\SETR\\avrdude-v7.0-rc1-windows-x64;C:\\Users\\PC\\Documents\\Aulas\\4ano\\1semestre\\SETR\\arduino-cli_0.21.1_Windows_64bit;C:\\Program Files\\CMake\\bin;C:\\Program Files\\rti_connext_dds-5.3.1\\bin;C:\\Program Files\\Graphviz\\bin;C:\\Program Files\\OpenSSL-Win64\\bin;C:\\opencv\\x64\\vc15\\bin;C:\\Users\\PC\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Program Files\\JetBrains\\JetBrains Rider 2021.3.3\\bin;C:\\Program Files\\JetBrains\\CLion 2021.3.3\\bin;C:\\Users\\PC\\AppData\\Roaming\\npm;C:\\Program Files\\heroku\\bin;C:\\Program Files\\JetBrains\\IntelliJ IDEA Community Edition 2022.2.2\\bin;'), ('ProgramW6432', 'C:\\Program Files'), ('windir', 'C:\\Windows'), ('ChocolateyInstall', 'C:\\ProgramData\\chocolatey'), ('PROCESSOR_IDENTIFIER', 'Intel64 Family 6 Model 158 Stepping 10, GenuineIntel'), ('CARBON_MEM_DISABLE', '1'), ('TMP', 'C:\\Users\\PC\\AppData\\Local\\Temp'), ('PROCESSOR_ARCHITECTURE', 'AMD64'), ('PATHEXT', '.COM;.EXE;.BAT;.CMD;.VBS;.VBE;.JS;.JSE;.WSF;.WSH;.MSC;.PY;.PYW;.CPL'), ('OPENSSL_CONF', 'C:\\Program Files\\OpenSSL-Win64\\bin\\openssl.cfg'), ('USERDOMAIN_ROAMINGPROFILE', 'DESKTOP-S1BRI82'), ('PROCESSOR_REVISION', '9e0a'), ('ChocolateyLastPathUpdate', '132961487583511216'), ('USERPROFILE', 'C:\\Users\\PC'), ('PKG_CONFIG_PATH', 'C:\\dev\\ros2_humble\\lib\\pkgconfig;'), ('LOGONSERVER', '\\\\DESKTOP-S1BRI82'), ('TEMP', 'C:\\Users\\PC\\AppData\\Local\\Temp'), ('AMENT_PREFIX_PATH', 'C:\\dev\\ros2_humble;'), ('ROS_PYTHON_VERSION', '3'), ('ROS_VERSION', '2'), ('OneDrive', 'C:\\Users\\PC\\OneDrive'), ('LOCALAPPDATA', 'C:\\Users\\PC\\AppData\\Local'), ('CommonProgramFiles', 'C:\\Program Files\\Common Files'), ('ZES_ENABLE_SYSMAN', '1'), ('ProgramData', 'C:\\ProgramData'), ('HOMEPATH', '\\Users\\PC'), ('COLCON', '1'), ('COMPUTERNAME', 'DESKTOP-S1BRI82'), ('ALLUSERSPROFILE', 'C:\\ProgramData'), ('CommonProgramW6432', 'C:\\Program Files\\Common Files'), ('USERNAME', 'PC'), ('ROS_LOCALHOST_ONLY', '0'), ('ROS_DISTRO', 'humble\nJetBrains Rider=C:\\Program Files\\JetBrains\\JetBrains Rider 2021.3.3\\bin;\nIntelliJ IDEA Community Edition=C:\\Program Files\\JetBrains\\IntelliJ IDEA Community Edition 2022.2.2\\bin;'), ('CMAKE_PREFIX_PATH', 'C:\\dev\\ros2_humble;'), ('PYTHONPATH', 'C:\\ci\\ws\\install\\lib\\python3.8\\dist-packages;C:\\dev\\ros2_humble\\Lib\\site-packages;C:\\Python38\\'), ('DriverData', 'C:\\Windows\\System32\\Drivers\\DriverData'), ('HOMEDRIVE', 'C:'), ('SystemRoot', 'C:\\Windows'), ('NUMBER_OF_PROCESSORS', '12'), ('OS', 'Windows_NT'), ('ProgramFiles', 'C:\\Program Files'), ('ComSpec', 'C:\\Windows\\system32\\cmd.exe'), ('QT_QPA_PLATFORM_PLUGIN_PATH', 'C:\\Qt\\Qt5.12.12\\5.12.12\\msvc2017_64\\plugins\\platforms'), ('Qt5_DIR', 'C:\\Qt\\Qt5.12.12\\5.12.12\\msvc2017_64'), ('PSModulePath', 'C:\\Users\\PC\\Documents\\WindowsPowerShell\\Modules;C:\\Program Files\\WindowsPowerShell\\Modules;C:\\Windows\\system32\\WindowsPowerShell\\v1.0\\Modules'), ('APPDATA', 'C:\\Users\\PC\\AppData\\Roaming'), ('USERDOMAIN', 'DESKTOP-S1BRI82'), ('PROCESSOR_LEVEL', '6'), ('OpenCV_DIR', 'C:\\opencv'), ('CLion', 'C:\\Program Files\\JetBrains\\CLion 2021.3.3\\bin;'), ('CommonProgramFiles(x86)', 'C:\\Program Files (x86)\\Common Files'), ('PUBLIC', 'C:\\Users\\Public')]), 'shell': False} +[0.968000] (flatland_viz) StderrLine: {'line': b'CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):\n'} +[0.968000] (flatland_viz) StderrLine: {'line': b' Compatibility with CMake < 2.8.12 will be removed from a future version of\n'} +[0.968000] (flatland_viz) StderrLine: {'line': b' CMake.\n'} +[0.968000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.968000] (flatland_viz) StderrLine: {'line': b' Update the VERSION argument value or use a ... suffix to tell\n'} +[0.968000] (flatland_viz) StderrLine: {'line': b' CMake that the project does not need compatibility with older versions.\n'} +[0.968000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.968000] (flatland_viz) StderrLine: {'line': b'\n'} +[1.015000] (-) TimerEvent: {} +[1.031000] (flatland_viz) StderrLine: {'line': b'CMake Error: CMake was unable to find a build program corresponding to "Ninja". CMAKE_MAKE_PROGRAM is not set. You probably need to select a different build tool.\n'} +[1.031000] (flatland_viz) StderrLine: {'line': b'CMake Error: CMAKE_C_COMPILER not set, after EnableLanguage\n'} +[1.031000] (flatland_viz) StderrLine: {'line': b'CMake Error: CMAKE_CXX_COMPILER not set, after EnableLanguage\n'} +[1.031000] (flatland_viz) StdoutLine: {'line': b'-- Configuring incomplete, errors occurred!\n'} +[1.031000] (flatland_viz) StdoutLine: {'line': b'See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log".\n'} +[1.031000] (flatland_viz) CommandEnded: {'returncode': 1} +[1.109000] (flatland_viz) JobEnded: {'identifier': 'flatland_viz', 'rc': 1} +[1.125000] (-) EventReactorShutdown: {} diff --git a/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/command.log b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/command.log new file mode 100644 index 00000000..16974ef9 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/command.log @@ -0,0 +1,6 @@ +Invoking command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz': ROS_DISTRO=%ROS_DISTRO% +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +Invoked command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz' returned '1': ROS_DISTRO=%ROS_DISTRO% +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz diff --git a/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stderr.log b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stderr.log new file mode 100644 index 00000000..56e0e6e1 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stderr.log @@ -0,0 +1,11 @@ +CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): + Compatibility with CMake < 2.8.12 will be removed from a future version of + CMake. + + Update the VERSION argument value or use a ... suffix to tell + CMake that the project does not need compatibility with older versions. + + +CMake Error: CMake was unable to find a build program corresponding to "Ninja". CMAKE_MAKE_PROGRAM is not set. You probably need to select a different build tool. +CMake Error: CMAKE_C_COMPILER not set, after EnableLanguage +CMake Error: CMAKE_CXX_COMPILER not set, after EnableLanguage diff --git a/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stdout.log b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stdout.log new file mode 100644 index 00000000..2cfe1293 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stdout.log @@ -0,0 +1,2 @@ +-- Configuring incomplete, errors occurred! +See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log". diff --git a/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stdout_stderr.log b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stdout_stderr.log new file mode 100644 index 00000000..5737a61e --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/stdout_stderr.log @@ -0,0 +1,13 @@ +CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): + Compatibility with CMake < 2.8.12 will be removed from a future version of + CMake. + + Update the VERSION argument value or use a ... suffix to tell + CMake that the project does not need compatibility with older versions. + + +CMake Error: CMake was unable to find a build program corresponding to "Ninja". CMAKE_MAKE_PROGRAM is not set. You probably need to select a different build tool. +CMake Error: CMAKE_C_COMPILER not set, after EnableLanguage +CMake Error: CMAKE_CXX_COMPILER not set, after EnableLanguage +-- Configuring incomplete, errors occurred! +See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log". diff --git a/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/streams.log b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/streams.log new file mode 100644 index 00000000..849703d4 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_18-56-57/flatland_viz/streams.log @@ -0,0 +1,19 @@ +[0.937s] Invoking command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz': ROS_DISTRO=%ROS_DISTRO% +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +[0.968s] CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): +[0.968s] Compatibility with CMake < 2.8.12 will be removed from a future version of +[0.968s] CMake. +[0.968s] +[0.968s] Update the VERSION argument value or use a ... suffix to tell +[0.968s] CMake that the project does not need compatibility with older versions. +[0.968s] +[0.968s] +[1.031s] CMake Error: CMake was unable to find a build program corresponding to "Ninja". CMAKE_MAKE_PROGRAM is not set. You probably need to select a different build tool. +[1.031s] CMake Error: CMAKE_C_COMPILER not set, after EnableLanguage +[1.031s] CMake Error: CMAKE_CXX_COMPILER not set, after EnableLanguage +[1.031s] -- Configuring incomplete, errors occurred! +[1.031s] See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log". +[1.031s] Invoked command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz' returned '1': ROS_DISTRO=%ROS_DISTRO% +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz diff --git a/flatland_viz/log/build_2022-11-19_18-56-57/logger_all.log b/flatland_viz/log/build_2022-11-19_18-56-57/logger_all.log new file mode 100644 index 00000000..c11171dd --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_18-56-57/logger_all.log @@ -0,0 +1,70 @@ +[2.153s] colcon DEBUG Command line arguments: ['C:\\Python38\\Scripts\\colcon', 'build', '--cmake-args', '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja'] +[2.154s] colcon DEBUG Parsed command line arguments: Namespace(ament_cmake_args=None, base_paths=['.'], build_base='build', catkin_cmake_args=None, catkin_skip_building_tests=False, cmake_args=['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja'], cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, cmake_target=None, cmake_target_skip_unavailable=False, continue_on_error=False, event_handlers=None, executor='parallel', ignore_user_meta=False, install_base='install', log_base=None, log_level=None, main=>, merge_install=False, metas=['./colcon.meta'], packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_end=None, packages_ignore=None, packages_ignore_regex=None, packages_select=None, packages_select_build_failed=False, packages_select_by_dep=None, packages_select_regex=None, packages_select_test_failures=False, packages_skip=None, packages_skip_build_finished=False, packages_skip_by_dep=None, packages_skip_regex=None, packages_skip_test_passed=False, packages_skip_up_to=None, packages_start=None, packages_up_to=None, packages_up_to_regex=None, parallel_workers=12, paths=None, symlink_install=False, test_result_base=None, verb_extension=, verb_name='build', verb_parser=) +[2.223s] colcon.colcon_core.package_discovery Level 1 discover_packages(colcon_meta) check parameters +[2.224s] colcon.colcon_core.package_discovery Level 1 discover_packages(recursive) check parameters +[2.224s] colcon.colcon_core.package_discovery Level 1 discover_packages(ignore) check parameters +[2.224s] colcon.colcon_core.package_discovery Level 1 discover_packages(path) check parameters +[2.224s] colcon.colcon_core.package_discovery Level 1 discover_packages(colcon_meta) discover +[2.224s] colcon.colcon_core.package_discovery Level 1 discover_packages(recursive) discover +[2.224s] colcon.colcon_core.package_discovery INFO Crawling recursively for packages in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz' +[2.224s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['ignore', 'ignore_ament_install'] +[2.224s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'ignore' +[2.225s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'ignore_ament_install' +[2.225s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['colcon_pkg'] +[2.225s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'colcon_pkg' +[2.225s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['colcon_meta'] +[2.225s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'colcon_meta' +[2.225s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['ros'] +[2.225s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'ros' +[2.281s] colcon.colcon_core.package_identification DEBUG Package '.' with type 'ros.catkin' and name 'flatland_viz' +[2.281s] colcon.colcon_core.package_discovery Level 1 discover_packages(recursive) using defaults +[2.281s] colcon.colcon_core.package_discovery Level 1 discover_packages(ignore) discover +[2.281s] colcon.colcon_core.package_discovery Level 1 discover_packages(ignore) using defaults +[2.281s] colcon.colcon_core.package_discovery Level 1 discover_packages(path) discover +[2.281s] colcon.colcon_core.package_discovery Level 1 discover_packages(path) using defaults +[2.345s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_args' from command line to '['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja']' +[2.345s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_target' from command line to 'None' +[2.345s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_target_skip_unavailable' from command line to 'False' +[2.345s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_clean_cache' from command line to 'False' +[2.345s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_clean_first' from command line to 'False' +[2.345s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_force_configure' from command line to 'False' +[2.345s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'ament_cmake_args' from command line to 'None' +[2.345s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'catkin_cmake_args' from command line to 'None' +[2.345s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'catkin_skip_building_tests' from command line to 'False' +[2.345s] colcon.colcon_core.verb DEBUG Building package 'flatland_viz' with the following arguments: {'ament_cmake_args': None, 'build_base': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\build\\flatland_viz', 'catkin_cmake_args': None, 'catkin_skip_building_tests': False, 'cmake_args': ['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja'], 'cmake_clean_cache': False, 'cmake_clean_first': False, 'cmake_force_configure': False, 'cmake_target': None, 'cmake_target_skip_unavailable': False, 'install_base': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\install\\flatland_viz', 'merge_install': False, 'path': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz', 'symlink_install': False, 'test_result_base': None} +[2.348s] colcon.colcon_core.executor INFO Executing jobs using 'parallel' executor +[2.369s] colcon.colcon_parallel_executor.executor.parallel DEBUG run_until_complete +[2.369s] colcon.colcon_ros.task.catkin.build INFO Building ROS package in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz' with build type 'catkin' +[2.369s] colcon.colcon_cmake.task.cmake.build INFO Building CMake package in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz' +[2.378s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_core.shell.sh': Not used on Windows systems +[3.311s] colcon.colcon_core.event_handler.log_command DEBUG Invoking command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz': ROS_DISTRO=%ROS_DISTRO% +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +[3.409s] colcon.colcon_core.event_handler.log_command DEBUG Invoked command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz' returned '1': ROS_DISTRO=%ROS_DISTRO% +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +[3.434s] colcon.colcon_core.environment Level 1 create_environment_scripts_only(flatland_viz) +[3.454s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz' for CMake module files +[3.455s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz' for CMake config files +[3.455s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\bin' +[3.455s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\bin' +[3.456s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\lib\pkgconfig\flatland_viz.pc' +[3.457s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\Lib\site-packages' +[3.457s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\Scripts' +[3.462s] colcon.colcon_core.shell INFO Creating package script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\flatland_viz\package.ps1' +[3.470s] colcon.colcon_core.shell INFO Creating package script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\flatland_viz\package.bat' +[3.477s] colcon.colcon_core.shell INFO Creating package descriptor 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\flatland_viz\package.dsv' +[3.481s] colcon.colcon_core.environment Level 1 create_file_with_runtime_dependencies(C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\colcon-core\packages\flatland_viz) +[3.506s] colcon.colcon_parallel_executor.executor.parallel DEBUG run_until_complete finished with '1' +[3.506s] colcon.colcon_core.event_reactor DEBUG joining thread +[3.518s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_notification.desktop_notification.notify2': 'notify2' not found +[3.518s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_notification.desktop_notification.notify_send': Not used on non-Linux systems +[3.519s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[3.519s] colcon.colcon_notification.desktop_notification INFO Sending desktop notification using 'win32' +[8.598s] colcon.colcon_core.event_reactor DEBUG joined thread +[8.606s] colcon.colcon_core.shell INFO Creating prefix script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\local_setup.ps1' +[8.614s] colcon.colcon_core.shell INFO Creating prefix util module 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\_local_setup_util_ps1.py' +[8.621s] colcon.colcon_core.shell INFO Creating prefix chain script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\setup.ps1' +[8.634s] colcon.colcon_core.shell INFO Creating prefix script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\local_setup.bat' +[8.645s] colcon.colcon_core.shell INFO Creating prefix util module 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\_local_setup_util_bat.py' +[8.646s] colcon.colcon_core.shell INFO Creating prefix chain script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\setup.bat' diff --git a/flatland_viz/log/build_2022-11-19_19-03-07/events.log b/flatland_viz/log/build_2022-11-19_19-03-07/events.log new file mode 100644 index 00000000..c040f2b9 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-03-07/events.log @@ -0,0 +1,65 @@ +[0.000000] (-) TimerEvent: {} +[0.000000] (flatland_viz) JobQueued: {'identifier': 'flatland_viz', 'dependencies': OrderedDict()} +[0.016000] (flatland_viz) JobStarted: {'identifier': 'flatland_viz'} +[0.109000] (-) TimerEvent: {} +[0.141000] (flatland_viz) JobProgress: {'identifier': 'flatland_viz', 'progress': 'cmake'} +[0.141000] (flatland_viz) Command: {'cmd': ['C:\\Program Files\\CMake\\bin\\cmake.EXE', 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz', '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja', '-DCATKIN_INSTALL_INTO_PREFIX_ROOT=0', '-DCMAKE_INSTALL_PREFIX=C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\install\\flatland_viz'], 'cwd': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\build\\flatland_viz', 'env': OrderedDict([('ALLUSERSPROFILE', 'C:\\ProgramData'), ('AMENT_PREFIX_PATH', 'C:\\dev\\ros2_humble'), ('APPDATA', 'C:\\Users\\PC\\AppData\\Roaming'), ('CARBON_MEM_DISABLE', '1'), ('ChocolateyInstall', 'C:\\ProgramData\\chocolatey'), ('ChocolateyLastPathUpdate', '132961487583511216'), ('CLion', 'C:\\Program Files\\JetBrains\\CLion 2021.3.3\\bin;'), ('CMAKE_PREFIX_PATH', 'C:\\dev\\ros2_humble'), ('COLCON', '1'), ('COLCON_PREFIX_PATH', 'C:\\Windows\\System32\\install\\;C:\\dev\\ros2_humble\\'), ('CommandPromptType', 'Cross'), ('CommonProgramFiles', 'C:\\Program Files\\Common Files'), ('CommonProgramFiles(x86)', 'C:\\Program Files (x86)\\Common Files'), ('CommonProgramW6432', 'C:\\Program Files\\Common Files'), ('COMPUTERNAME', 'DESKTOP-S1BRI82'), ('ComSpec', 'C:\\Windows\\system32\\cmd.exe'), ('DevEnvDir', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\IDE\\'), ('DriverData', 'C:\\Windows\\System32\\Drivers\\DriverData'), ('ExtensionSdkDir', 'C:\\Program Files (x86)\\Microsoft SDKs\\Windows Kits\\10\\ExtensionSDKs'), ('Framework40Version', 'v4.0'), ('FrameworkDir', 'C:\\Windows\\Microsoft.NET\\Framework\\'), ('FrameworkDir32', 'C:\\Windows\\Microsoft.NET\\Framework\\'), ('FrameworkDir64', 'C:\\Windows\\Microsoft.NET\\Framework64\\'), ('FrameworkVersion', 'v4.0.30319'), ('FrameworkVersion32', 'v4.0.30319'), ('FrameworkVersion64', 'v4.0.30319'), ('HOMEDRIVE', 'C:'), ('HOMEPATH', '\\Users\\PC'), ('INCLUDE', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\ATLMFC\\include;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\include;C:\\Program Files (x86)\\Windows Kits\\10\\include\\10.0.19041.0\\ucrt;C:\\Program Files (x86)\\Windows Kits\\10\\include\\10.0.19041.0\\shared;C:\\Program Files (x86)\\Windows Kits\\10\\include\\10.0.19041.0\\um;C:\\Program Files (x86)\\Windows Kits\\10\\include\\10.0.19041.0\\winrt;C:\\Program Files (x86)\\Windows Kits\\10\\include\\10.0.19041.0\\cppwinrt\nIntelliJ IDEA Community Edition=C:\\Program Files\\JetBrains\\IntelliJ IDEA Community Edition 2022.2.2\\bin;\nJetBrains Rider=C:\\Program Files\\JetBrains\\JetBrains Rider 2021.3.3\\bin;'), ('LIB', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\ATLMFC\\lib\\x64;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\lib\\x64;C:\\Program Files (x86)\\Windows Kits\\10\\lib\\10.0.19041.0\\ucrt\\x64;C:\\Program Files (x86)\\Windows Kits\\10\\lib\\10.0.19041.0\\um\\x64'), ('LIBPATH', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\ATLMFC\\lib\\x64;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\lib\\x64;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\lib\\x86\\store\\references;C:\\Program Files (x86)\\Windows Kits\\10\\UnionMetadata\\10.0.19041.0;C:\\Program Files (x86)\\Windows Kits\\10\\References\\10.0.19041.0;C:\\Windows\\Microsoft.NET\\Framework\\v4.0.30319'), ('LOCALAPPDATA', 'C:\\Users\\PC\\AppData\\Local'), ('LOGONSERVER', '\\\\DESKTOP-S1BRI82'), ('NUMBER_OF_PROCESSORS', '12'), ('OneDrive', 'C:\\Users\\PC\\OneDrive'), ('OpenCV_DIR', 'C:\\opencv'), ('OPENSSL_CONF', 'C:\\Program Files\\OpenSSL-Win64\\bin\\openssl.cfg'), ('OS', 'Windows_NT'), ('Path', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\IDE\\\\Extensions\\Microsoft\\IntelliCode\\CLI;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\bin\\HostX86\\x64;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\bin\\HostX86\\x86;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\IDE\\VC\\VCPackages;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\IDE\\CommonExtensions\\Microsoft\\TestWindow;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\IDE\\CommonExtensions\\Microsoft\\TeamFoundation\\Team Explorer;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\MSBuild\\Current\\bin\\Roslyn;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Team Tools\\Performance Tools\\x64;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Team Tools\\Performance Tools;C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\Common\\VSPerfCollectionTools\\vs2019\\\\x64;C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\Common\\VSPerfCollectionTools\\vs2019\\;C:\\Program Files (x86)\\Microsoft SDKs\\Windows\\v8.1A\\bin\\NETFX 4.5.1 Tools\\;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\Tools\\devinit;C:\\Program Files (x86)\\Windows Kits\\10\\bin\\10.0.19041.0\\x86;C:\\Program Files (x86)\\Windows Kits\\10\\bin\\x86;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\\\MSBuild\\Current\\Bin;C:\\Windows\\Microsoft.NET\\Framework\\v4.0.30319;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\IDE\\;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\Tools\\;C:\\dev\\ros2_humble\\opt\\yaml_cpp_vendor\\bin;C:\\dev\\ros2_humble\\opt\\rviz_ogre_vendor\\bin;C:\\dev\\ros2_humble\\opt\\rviz_assimp_vendor\\bin;C:\\dev\\ros2_humble\\opt\\libcurl_vendor\\bin;C:\\dev\\ros2_humble\\Scripts;C:\\dev\\ros2_humble\\bin;C:\\Python38\\Scripts;C:\\Python38\\;C:\\ProgramData\\QFS\\QF-Test\\qftestpath;C:\\Program Files\\Common Files\\Oracle\\Java\\javapath;C:\\Program Files (x86)\\Common Files\\Oracle\\Java\\javapath;C:\\Windows\\system32;C:\\Windows;C:\\Windows\\System32\\Wbem;C:\\Windows\\System32\\WindowsPowerShell\\v1.0\\;C:\\Windows\\System32\\OpenSSH\\;C:\\Program Files\\Intel\\WiFi\\bin\\;C:\\Program Files\\Common Files\\Intel\\WirelessCommon\\;C:\\Program Files\\Microsoft VS Code\\bin;C:\\Program Files\\NVIDIA Corporation\\NVIDIA NvDLISR;C:\\Program Files (x86)\\NVIDIA Corporation\\PhysX\\Common;C:\\Program Files\\Git\\cmd;C:\\Program Files\\Microchip\\xc8\\v2.31\\bin;C:\\Program Files\\nodejs\\;C:\\ProgramData\\chocolatey\\bin;C:\\Users\\PC\\Documents\\Aulas\\4ano\\1semestre\\SETR\\avrdude-v7.0-rc1-windows-x64;C:\\Users\\PC\\Documents\\Aulas\\4ano\\1semestre\\SETR\\arduino-cli_0.21.1_Windows_64bit;C:\\Program Files\\CMake\\bin;C:\\Program Files\\rti_connext_dds-5.3.1\\bin;C:\\Program Files\\Graphviz\\bin;C:\\Program Files\\OpenSSL-Win64\\bin;C:\\opencv\\x64\\vc15\\bin;C:\\Users\\PC\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Program Files\\JetBrains\\JetBrains Rider 2021.3.3\\bin;C:\\Program Files\\JetBrains\\CLion 2021.3.3\\bin;C:\\Users\\PC\\AppData\\Roaming\\npm;C:\\Program Files\\heroku\\bin;C:\\Program Files\\JetBrains\\IntelliJ IDEA Community Edition 2022.2.2\\bin;;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\IDE\\CommonExtensions\\Microsoft\\CMake\\CMake\\bin;C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\IDE\\CommonExtensions\\Microsoft\\CMake\\Ninja'), ('PATHEXT', '.COM;.EXE;.BAT;.CMD;.VBS;.VBE;.JS;.JSE;.WSF;.WSH;.MSC;.PY;.PYW'), ('PKG_CONFIG_PATH', 'C:\\dev\\ros2_humble\\lib\\pkgconfig'), ('Platform', 'x64'), ('PROCESSOR_ARCHITECTURE', 'AMD64'), ('PROCESSOR_IDENTIFIER', 'Intel64 Family 6 Model 158 Stepping 10, GenuineIntel'), ('PROCESSOR_LEVEL', '6'), ('PROCESSOR_REVISION', '9e0a'), ('ProgramData', 'C:\\ProgramData'), ('ProgramFiles', 'C:\\Program Files'), ('ProgramFiles(x86)', 'C:\\Program Files (x86)'), ('ProgramW6432', 'C:\\Program Files'), ('PROMPT', '$P$G'), ('PSModulePath', 'C:\\Program Files\\WindowsPowerShell\\Modules;C:\\Windows\\system32\\WindowsPowerShell\\v1.0\\Modules'), ('PUBLIC', 'C:\\Users\\Public'), ('PYTHONPATH', 'C:\\ci\\ws\\install\\lib\\python3.8\\dist-packages;C:\\dev\\ros2_humble\\Lib\\site-packages;C:\\Python38\\'), ('Qt5_DIR', 'C:\\Qt\\Qt5.12.12\\5.12.12\\msvc2017_64'), ('QT_QPA_PLATFORM_PLUGIN_PATH', 'C:\\Qt\\Qt5.12.12\\5.12.12\\msvc2017_64\\plugins\\platforms'), ('ROS_DISTRO', 'humble'), ('ROS_LOCALHOST_ONLY', '0'), ('ROS_PYTHON_VERSION', '3'), ('ROS_VERSION', '2'), ('SystemDrive', 'C:'), ('SystemRoot', 'C:\\Windows'), ('TEMP', 'C:\\Users\\PC\\AppData\\Local\\Temp'), ('TMP', 'C:\\Users\\PC\\AppData\\Local\\Temp'), ('UCRTVersion', '10.0.19041.0'), ('UniversalCRTSdkDir', 'C:\\Program Files (x86)\\Windows Kits\\10\\'), ('USERDOMAIN', 'DESKTOP-S1BRI82'), ('USERDOMAIN_ROAMINGPROFILE', 'DESKTOP-S1BRI82'), ('USERNAME', 'PC'), ('USERPROFILE', 'C:\\Users\\PC'), ('VCIDEInstallDir', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\IDE\\VC\\'), ('VCINSTALLDIR', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\'), ('VCToolsInstallDir', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Tools\\MSVC\\14.29.30133\\'), ('VCToolsRedistDir', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\VC\\Redist\\MSVC\\14.29.30133\\'), ('VCToolsVersion', '14.29.30133'), ('VisualStudioVersion', '16.0'), ('VS160COMNTOOLS', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\Tools\\'), ('VSCMD_ARG_app_plat', 'Desktop'), ('VSCMD_ARG_HOST_ARCH', 'x86'), ('VSCMD_ARG_TGT_ARCH', 'x64'), ('VSCMD_VER', '16.11.21'), ('VSINSTALLDIR', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\'), ('windir', 'C:\\Windows'), ('WindowsLibPath', 'C:\\Program Files (x86)\\Windows Kits\\10\\UnionMetadata\\10.0.19041.0;C:\\Program Files (x86)\\Windows Kits\\10\\References\\10.0.19041.0'), ('WindowsSdkBinPath', 'C:\\Program Files (x86)\\Windows Kits\\10\\bin\\'), ('WindowsSdkDir', 'C:\\Program Files (x86)\\Windows Kits\\10\\'), ('WindowsSDKLibVersion', '10.0.19041.0\\'), ('WindowsSdkVerBinPath', 'C:\\Program Files (x86)\\Windows Kits\\10\\bin\\10.0.19041.0\\'), ('WindowsSDKVersion', '10.0.19041.0\\'), ('WindowsSDK_ExecutablePath_x64', 'C:\\Program Files (x86)\\Microsoft SDKs\\Windows\\v8.1A\\bin\\NETFX 4.5.1 Tools\\x64\\'), ('WindowsSDK_ExecutablePath_x86', 'C:\\Program Files (x86)\\Microsoft SDKs\\Windows\\v8.1A\\bin\\NETFX 4.5.1 Tools\\'), ('ZES_ENABLE_SYSMAN', '1'), ('__devinit_path', 'C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\Community\\Common7\\Tools\\devinit\\devinit.exe'), ('__DOTNET_ADD_32BIT', '1'), ('__DOTNET_ADD_64BIT', '1'), ('__DOTNET_PREFERRED_BITNESS', '32'), ('__VSCMD_PREINIT_PATH', 'C:\\dev\\ros2_humble\\opt\\yaml_cpp_vendor\\bin;C:\\dev\\ros2_humble\\opt\\rviz_ogre_vendor\\bin;C:\\dev\\ros2_humble\\opt\\rviz_assimp_vendor\\bin;C:\\dev\\ros2_humble\\opt\\libcurl_vendor\\bin;C:\\dev\\ros2_humble\\Scripts;C:\\dev\\ros2_humble\\bin;C:\\Python38\\Scripts;C:\\Python38\\;C:\\ProgramData\\QFS\\QF-Test\\qftestpath;C:\\Program Files\\Common Files\\Oracle\\Java\\javapath;C:\\Program Files (x86)\\Common Files\\Oracle\\Java\\javapath;C:\\Windows\\system32;C:\\Windows;C:\\Windows\\System32\\Wbem;C:\\Windows\\System32\\WindowsPowerShell\\v1.0\\;C:\\Windows\\System32\\OpenSSH\\;C:\\Program Files\\Intel\\WiFi\\bin\\;C:\\Program Files\\Common Files\\Intel\\WirelessCommon\\;C:\\Program Files\\Microsoft VS Code\\bin;C:\\Program Files\\NVIDIA Corporation\\NVIDIA NvDLISR;C:\\Program Files (x86)\\NVIDIA Corporation\\PhysX\\Common;C:\\Program Files\\Git\\cmd;C:\\Program Files\\Microchip\\xc8\\v2.31\\bin;C:\\Program Files\\nodejs\\;C:\\ProgramData\\chocolatey\\bin;C:\\Users\\PC\\Documents\\Aulas\\4ano\\1semestre\\SETR\\avrdude-v7.0-rc1-windows-x64;C:\\Users\\PC\\Documents\\Aulas\\4ano\\1semestre\\SETR\\arduino-cli_0.21.1_Windows_64bit;C:\\Program Files\\CMake\\bin;C:\\Program Files\\rti_connext_dds-5.3.1\\bin;C:\\Program Files\\Graphviz\\bin;C:\\Program Files\\OpenSSL-Win64\\bin;C:\\opencv\\x64\\vc15\\bin;C:\\Users\\PC\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Program Files\\JetBrains\\JetBrains Rider 2021.3.3\\bin;C:\\Program Files\\JetBrains\\CLion 2021.3.3\\bin;C:\\Users\\PC\\AppData\\Roaming\\npm;C:\\Program Files\\heroku\\bin;C:\\Program Files\\JetBrains\\IntelliJ IDEA Community Edition 2022.2.2\\bin;'), ('__VSCMD_script_err_count', '0')]), 'shell': False} +[0.172000] (flatland_viz) StderrLine: {'line': b'CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):\n'} +[0.172000] (flatland_viz) StderrLine: {'line': b' Compatibility with CMake < 2.8.12 will be removed from a future version of\n'} +[0.172000] (flatland_viz) StderrLine: {'line': b' CMake.\n'} +[0.172000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.172000] (flatland_viz) StderrLine: {'line': b' Update the VERSION argument value or use a ... suffix to tell\n'} +[0.172000] (flatland_viz) StderrLine: {'line': b' CMake that the project does not need compatibility with older versions.\n'} +[0.172000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.172000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.219000] (-) TimerEvent: {} +[0.328000] (-) TimerEvent: {} +[0.437000] (-) TimerEvent: {} +[0.547000] (-) TimerEvent: {} +[0.656000] (-) TimerEvent: {} +[0.766000] (-) TimerEvent: {} +[0.875000] (-) TimerEvent: {} +[0.984000] (-) TimerEvent: {} +[1.047000] (flatland_viz) StdoutLine: {'line': b'-- The C compiler identification is MSVC 19.29.30147.0\n'} +[1.094000] (-) TimerEvent: {} +[1.203000] (-) TimerEvent: {} +[1.312000] (-) TimerEvent: {} +[1.375000] (flatland_viz) StdoutLine: {'line': b'-- The CXX compiler identification is MSVC 19.29.30147.0\n'} +[1.422000] (-) TimerEvent: {} +[1.531000] (flatland_viz) StdoutLine: {'line': b'-- Detecting C compiler ABI info\n'} +[1.531000] (-) TimerEvent: {} +[1.641000] (-) TimerEvent: {} +[1.750000] (-) TimerEvent: {} +[1.859000] (-) TimerEvent: {} +[1.969000] (-) TimerEvent: {} +[2.078000] (-) TimerEvent: {} +[2.187000] (-) TimerEvent: {} +[2.297000] (-) TimerEvent: {} +[2.406000] (-) TimerEvent: {} +[2.437000] (flatland_viz) StdoutLine: {'line': b'-- Detecting C compiler ABI info - done\n'} +[2.437000] (flatland_viz) StdoutLine: {'line': b'-- Check for working C compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe - skipped\n'} +[2.437000] (flatland_viz) StdoutLine: {'line': b'-- Detecting C compile features\n'} +[2.453000] (flatland_viz) StdoutLine: {'line': b'-- Detecting C compile features - done\n'} +[2.484000] (flatland_viz) StdoutLine: {'line': b'-- Detecting CXX compiler ABI info\n'} +[2.516000] (-) TimerEvent: {} +[2.625000] (-) TimerEvent: {} +[2.734000] (-) TimerEvent: {} +[2.844000] (-) TimerEvent: {} +[2.859000] (flatland_viz) StdoutLine: {'line': b'-- Detecting CXX compiler ABI info - done\n'} +[2.859000] (flatland_viz) StdoutLine: {'line': b'-- Check for working CXX compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe - skipped\n'} +[2.859000] (flatland_viz) StdoutLine: {'line': b'-- Detecting CXX compile features\n'} +[2.875000] (flatland_viz) StdoutLine: {'line': b'-- Detecting CXX compile features - done\n'} +[2.891000] (flatland_viz) StderrLine: {'line': b'CMake Error at CMakeLists.txt:10 (if):\n'} +[2.891000] (flatland_viz) StderrLine: {'line': b' if given arguments:\n'} +[2.891000] (flatland_viz) StderrLine: {'line': b'\n'} +[2.891000] (flatland_viz) StderrLine: {'line': b' "STREQUAL" "14.04"\n'} +[2.891000] (flatland_viz) StderrLine: {'line': b'\n'} +[2.891000] (flatland_viz) StderrLine: {'line': b' Unknown arguments specified\n'} +[2.891000] (flatland_viz) StderrLine: {'line': b'\n'} +[2.891000] (flatland_viz) StderrLine: {'line': b'\n'} +[2.891000] (flatland_viz) StdoutLine: {'line': b'-- Configuring incomplete, errors occurred!\n'} +[2.891000] (flatland_viz) StdoutLine: {'line': b'See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log".\n'} +[2.906000] (flatland_viz) CommandEnded: {'returncode': 1} +[2.953000] (-) TimerEvent: {} +[2.969000] (flatland_viz) JobEnded: {'identifier': 'flatland_viz', 'rc': 1} +[2.984000] (-) EventReactorShutdown: {} diff --git a/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/command.log b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/command.log new file mode 100644 index 00000000..24b2724c --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/command.log @@ -0,0 +1,6 @@ +Invoking command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz': INCLUDE=%INCLUDE% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +Invoked command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz' returned '1': INCLUDE=%INCLUDE% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz diff --git a/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stderr.log b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stderr.log new file mode 100644 index 00000000..102ec265 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stderr.log @@ -0,0 +1,16 @@ +CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): + Compatibility with CMake < 2.8.12 will be removed from a future version of + CMake. + + Update the VERSION argument value or use a ... suffix to tell + CMake that the project does not need compatibility with older versions. + + +CMake Error at CMakeLists.txt:10 (if): + if given arguments: + + "STREQUAL" "14.04" + + Unknown arguments specified + + diff --git a/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stdout.log b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stdout.log new file mode 100644 index 00000000..84a8b624 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stdout.log @@ -0,0 +1,14 @@ +-- The C compiler identification is MSVC 19.29.30147.0 +-- The CXX compiler identification is MSVC 19.29.30147.0 +-- Detecting C compiler ABI info +-- Detecting C compiler ABI info - done +-- Check for working C compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe - skipped +-- Detecting C compile features +-- Detecting C compile features - done +-- Detecting CXX compiler ABI info +-- Detecting CXX compiler ABI info - done +-- Check for working CXX compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe - skipped +-- Detecting CXX compile features +-- Detecting CXX compile features - done +-- Configuring incomplete, errors occurred! +See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log". diff --git a/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stdout_stderr.log b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stdout_stderr.log new file mode 100644 index 00000000..f8d9562f --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/stdout_stderr.log @@ -0,0 +1,30 @@ +CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): + Compatibility with CMake < 2.8.12 will be removed from a future version of + CMake. + + Update the VERSION argument value or use a ... suffix to tell + CMake that the project does not need compatibility with older versions. + + +-- The C compiler identification is MSVC 19.29.30147.0 +-- The CXX compiler identification is MSVC 19.29.30147.0 +-- Detecting C compiler ABI info +-- Detecting C compiler ABI info - done +-- Check for working C compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe - skipped +-- Detecting C compile features +-- Detecting C compile features - done +-- Detecting CXX compiler ABI info +-- Detecting CXX compiler ABI info - done +-- Check for working CXX compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe - skipped +-- Detecting CXX compile features +-- Detecting CXX compile features - done +CMake Error at CMakeLists.txt:10 (if): + if given arguments: + + "STREQUAL" "14.04" + + Unknown arguments specified + + +-- Configuring incomplete, errors occurred! +See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log". diff --git a/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/streams.log b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/streams.log new file mode 100644 index 00000000..027ed2f6 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-03-07/flatland_viz/streams.log @@ -0,0 +1,36 @@ +[0.125s] Invoking command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz': INCLUDE=%INCLUDE% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +[0.156s] CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): +[0.156s] Compatibility with CMake < 2.8.12 will be removed from a future version of +[0.156s] CMake. +[0.156s] +[0.156s] Update the VERSION argument value or use a ... suffix to tell +[0.156s] CMake that the project does not need compatibility with older versions. +[0.156s] +[0.156s] +[1.031s] -- The C compiler identification is MSVC 19.29.30147.0 +[1.359s] -- The CXX compiler identification is MSVC 19.29.30147.0 +[1.515s] -- Detecting C compiler ABI info +[2.421s] -- Detecting C compiler ABI info - done +[2.421s] -- Check for working C compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe - skipped +[2.421s] -- Detecting C compile features +[2.437s] -- Detecting C compile features - done +[2.468s] -- Detecting CXX compiler ABI info +[2.843s] -- Detecting CXX compiler ABI info - done +[2.843s] -- Check for working CXX compiler: C:/Program Files (x86)/Microsoft Visual Studio/2019/Community/VC/Tools/MSVC/14.29.30133/bin/Hostx86/x64/cl.exe - skipped +[2.843s] -- Detecting CXX compile features +[2.859s] -- Detecting CXX compile features - done +[2.875s] CMake Error at CMakeLists.txt:10 (if): +[2.875s] if given arguments: +[2.875s] +[2.875s] "STREQUAL" "14.04" +[2.875s] +[2.875s] Unknown arguments specified +[2.875s] +[2.875s] +[2.875s] -- Configuring incomplete, errors occurred! +[2.875s] See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log". +[2.890s] Invoked command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz' returned '1': INCLUDE=%INCLUDE% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz diff --git a/flatland_viz/log/build_2022-11-19_19-03-07/logger_all.log b/flatland_viz/log/build_2022-11-19_19-03-07/logger_all.log new file mode 100644 index 00000000..40785eef --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-03-07/logger_all.log @@ -0,0 +1,71 @@ +[0.714s] colcon DEBUG Command line arguments: ['C:\\Python38\\Scripts\\colcon', 'build', '--cmake-args', '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja'] +[0.715s] colcon DEBUG Parsed command line arguments: Namespace(ament_cmake_args=None, base_paths=['.'], build_base='build', catkin_cmake_args=None, catkin_skip_building_tests=False, cmake_args=['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja'], cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, cmake_target=None, cmake_target_skip_unavailable=False, continue_on_error=False, event_handlers=None, executor='parallel', ignore_user_meta=False, install_base='install', log_base=None, log_level=None, main=>, merge_install=False, metas=['./colcon.meta'], packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_end=None, packages_ignore=None, packages_ignore_regex=None, packages_select=None, packages_select_build_failed=False, packages_select_by_dep=None, packages_select_regex=None, packages_select_test_failures=False, packages_skip=None, packages_skip_build_finished=False, packages_skip_by_dep=None, packages_skip_regex=None, packages_skip_test_passed=False, packages_skip_up_to=None, packages_start=None, packages_up_to=None, packages_up_to_regex=None, parallel_workers=12, paths=None, symlink_install=False, test_result_base=None, verb_extension=, verb_name='build', verb_parser=) +[0.769s] colcon.colcon_core.package_discovery Level 1 discover_packages(colcon_meta) check parameters +[0.770s] colcon.colcon_core.package_discovery Level 1 discover_packages(recursive) check parameters +[0.770s] colcon.colcon_core.package_discovery Level 1 discover_packages(ignore) check parameters +[0.770s] colcon.colcon_core.package_discovery Level 1 discover_packages(path) check parameters +[0.770s] colcon.colcon_core.package_discovery Level 1 discover_packages(colcon_meta) discover +[0.770s] colcon.colcon_core.package_discovery Level 1 discover_packages(recursive) discover +[0.770s] colcon.colcon_core.package_discovery INFO Crawling recursively for packages in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz' +[0.770s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.770s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'ignore' +[0.770s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'ignore_ament_install' +[0.771s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['colcon_pkg'] +[0.771s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'colcon_pkg' +[0.771s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['colcon_meta'] +[0.771s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'colcon_meta' +[0.771s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['ros'] +[0.771s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'ros' +[0.808s] colcon.colcon_core.package_identification DEBUG Package '.' with type 'ros.catkin' and name 'flatland_viz' +[0.808s] colcon.colcon_core.package_discovery Level 1 discover_packages(recursive) using defaults +[0.808s] colcon.colcon_core.package_discovery Level 1 discover_packages(ignore) discover +[0.808s] colcon.colcon_core.package_discovery Level 1 discover_packages(ignore) using defaults +[0.808s] colcon.colcon_core.package_discovery Level 1 discover_packages(path) discover +[0.808s] colcon.colcon_core.package_discovery Level 1 discover_packages(path) using defaults +[0.863s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_args' from command line to '['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja']' +[0.863s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_target' from command line to 'None' +[0.863s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_target_skip_unavailable' from command line to 'False' +[0.863s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_clean_cache' from command line to 'False' +[0.863s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_clean_first' from command line to 'False' +[0.863s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_force_configure' from command line to 'False' +[0.863s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'ament_cmake_args' from command line to 'None' +[0.864s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'catkin_cmake_args' from command line to 'None' +[0.864s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'catkin_skip_building_tests' from command line to 'False' +[0.864s] colcon.colcon_core.verb DEBUG Building package 'flatland_viz' with the following arguments: {'ament_cmake_args': None, 'build_base': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\build\\flatland_viz', 'catkin_cmake_args': None, 'catkin_skip_building_tests': False, 'cmake_args': ['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja'], 'cmake_clean_cache': False, 'cmake_clean_first': False, 'cmake_force_configure': False, 'cmake_target': None, 'cmake_target_skip_unavailable': False, 'install_base': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\install\\flatland_viz', 'merge_install': False, 'path': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz', 'symlink_install': False, 'test_result_base': None} +[0.866s] colcon.colcon_core.executor INFO Executing jobs using 'parallel' executor +[0.887s] colcon.colcon_parallel_executor.executor.parallel DEBUG run_until_complete +[0.887s] colcon.colcon_ros.task.catkin.build INFO Building ROS package in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz' with build type 'catkin' +[0.887s] colcon.colcon_cmake.task.cmake.build INFO Building CMake package in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz' +[0.898s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_core.shell.sh': Not used on Windows systems +[0.898s] colcon.colcon_core.shell INFO Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell +[1.029s] colcon.colcon_core.event_handler.log_command DEBUG Invoking command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz': INCLUDE=%INCLUDE% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +[3.785s] colcon.colcon_core.event_handler.log_command DEBUG Invoked command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz' returned '1': INCLUDE=%INCLUDE% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +[3.820s] colcon.colcon_core.environment Level 1 create_environment_scripts_only(flatland_viz) +[3.837s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz' for CMake module files +[3.838s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz' for CMake config files +[3.839s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\bin' +[3.839s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\bin' +[3.839s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\lib\pkgconfig\flatland_viz.pc' +[3.840s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\Lib\site-packages' +[3.840s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\Scripts' +[3.845s] colcon.colcon_core.shell INFO Creating package script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\flatland_viz\package.ps1' +[3.847s] colcon.colcon_core.shell INFO Creating package script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\flatland_viz\package.bat' +[3.848s] colcon.colcon_core.shell INFO Creating package descriptor 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\flatland_viz\package.dsv' +[3.849s] colcon.colcon_core.environment Level 1 create_file_with_runtime_dependencies(C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\colcon-core\packages\flatland_viz) +[3.867s] colcon.colcon_parallel_executor.executor.parallel DEBUG run_until_complete finished with '1' +[3.867s] colcon.colcon_core.event_reactor DEBUG joining thread +[3.878s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_notification.desktop_notification.notify2': 'notify2' not found +[3.878s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_notification.desktop_notification.notify_send': Not used on non-Linux systems +[3.878s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[3.878s] colcon.colcon_notification.desktop_notification INFO Sending desktop notification using 'win32' +[8.926s] colcon.colcon_core.event_reactor DEBUG joined thread +[8.934s] colcon.colcon_core.shell INFO Creating prefix script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\local_setup.ps1' +[8.935s] colcon.colcon_core.shell INFO Creating prefix util module 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\_local_setup_util_ps1.py' +[8.937s] colcon.colcon_core.shell INFO Creating prefix chain script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\setup.ps1' +[8.944s] colcon.colcon_core.shell INFO Creating prefix script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\local_setup.bat' +[8.945s] colcon.colcon_core.shell INFO Creating prefix util module 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\_local_setup_util_bat.py' +[8.946s] colcon.colcon_core.shell INFO Creating prefix chain script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\setup.bat' diff --git a/flatland_viz/log/build_2022-11-19_19-05-41/events.log b/flatland_viz/log/build_2022-11-19_19-05-41/events.log new file mode 100644 index 00000000..14811e38 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-05-41/events.log @@ -0,0 +1,27 @@ +[0.000000] (-) TimerEvent: {} +[0.000000] (flatland_viz) JobQueued: {'identifier': 'flatland_viz', 'dependencies': OrderedDict()} +[0.000000] (flatland_viz) JobStarted: {'identifier': 'flatland_viz'} +[0.031000] (flatland_viz) JobProgress: {'identifier': 'flatland_viz', 'progress': 'cmake'} +[0.031000] (flatland_viz) Command: {'cmd': ['C:\\Program Files\\CMake\\bin\\cmake.EXE', 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz', '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja', '-DCATKIN_INSTALL_INTO_PREFIX_ROOT=0', '-DCMAKE_INSTALL_PREFIX=C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\install\\flatland_viz'], 'cwd': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\build\\flatland_viz', 'env': OrderedDict([('ALLUSERSPROFILE', 'C:\\ProgramData'), ('APPDATA', 'C:\\Users\\PC\\AppData\\Roaming'), ('CARBON_MEM_DISABLE', '1'), ('ChocolateyInstall', 'C:\\ProgramData\\chocolatey'), ('ChocolateyLastPathUpdate', '132961487583511216'), ('CLion', 'C:\\Program Files\\JetBrains\\CLion 2021.3.3\\bin;'), ('COLCON', '1'), ('CommonProgramFiles', 'C:\\Program Files\\Common Files'), ('CommonProgramFiles(x86)', 'C:\\Program Files (x86)\\Common Files'), ('CommonProgramW6432', 'C:\\Program Files\\Common Files'), ('COMPUTERNAME', 'DESKTOP-S1BRI82'), ('ComSpec', 'C:\\Windows\\system32\\cmd.exe'), ('DriverData', 'C:\\Windows\\System32\\Drivers\\DriverData'), ('HOMEDRIVE', 'C:'), ('HOMEPATH', '\\Users\\PC\nIntelliJ IDEA Community Edition=C:\\Program Files\\JetBrains\\IntelliJ IDEA Community Edition 2022.2.2\\bin;\nJetBrains Rider=C:\\Program Files\\JetBrains\\JetBrains Rider 2021.3.3\\bin;'), ('LOCALAPPDATA', 'C:\\Users\\PC\\AppData\\Local'), ('LOGONSERVER', '\\\\DESKTOP-S1BRI82'), ('NUMBER_OF_PROCESSORS', '12'), ('OneDrive', 'C:\\Users\\PC\\OneDrive'), ('OpenCV_DIR', 'C:\\opencv'), ('OPENSSL_CONF', 'C:\\Program Files\\OpenSSL-Win64\\bin\\openssl.cfg'), ('OS', 'Windows_NT'), ('Path', 'C:\\Python38\\Scripts;C:\\Python38\\;C:\\ProgramData\\QFS\\QF-Test\\qftestpath;C:\\Program Files\\Common Files\\Oracle\\Java\\javapath;C:\\Program Files (x86)\\Common Files\\Oracle\\Java\\javapath;C:\\Windows\\system32;C:\\Windows;C:\\Windows\\System32\\Wbem;C:\\Windows\\System32\\WindowsPowerShell\\v1.0\\;C:\\Windows\\System32\\OpenSSH\\;C:\\Program Files\\Intel\\WiFi\\bin\\;C:\\Program Files\\Common Files\\Intel\\WirelessCommon\\;C:\\Program Files\\Microsoft VS Code\\bin;C:\\Program Files\\NVIDIA Corporation\\NVIDIA NvDLISR;C:\\Program Files (x86)\\NVIDIA Corporation\\PhysX\\Common;C:\\Program Files\\Git\\cmd;C:\\Program Files\\Microchip\\xc8\\v2.31\\bin;C:\\Program Files\\nodejs\\;C:\\ProgramData\\chocolatey\\bin;C:\\Users\\PC\\Documents\\Aulas\\4ano\\1semestre\\SETR\\avrdude-v7.0-rc1-windows-x64;C:\\Users\\PC\\Documents\\Aulas\\4ano\\1semestre\\SETR\\arduino-cli_0.21.1_Windows_64bit;C:\\Program Files\\CMake\\bin;C:\\Program Files\\rti_connext_dds-5.3.1\\bin;C:\\Program Files\\Graphviz\\bin;C:\\Program Files\\OpenSSL-Win64\\bin;C:\\opencv\\x64\\vc15\\bin;C:\\Ninja;C:\\Users\\PC\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Program Files\\JetBrains\\JetBrains Rider 2021.3.3\\bin;C:\\Program Files\\JetBrains\\CLion 2021.3.3\\bin;C:\\Users\\PC\\AppData\\Roaming\\npm;C:\\Program Files\\heroku\\bin;C:\\Program Files\\JetBrains\\IntelliJ IDEA Community Edition 2022.2.2\\bin;'), ('PATHEXT', '.COM;.EXE;.BAT;.CMD;.VBS;.VBE;.JS;.JSE;.WSF;.WSH;.MSC;.PY;.PYW'), ('PROCESSOR_ARCHITECTURE', 'AMD64'), ('PROCESSOR_IDENTIFIER', 'Intel64 Family 6 Model 158 Stepping 10, GenuineIntel'), ('PROCESSOR_LEVEL', '6'), ('PROCESSOR_REVISION', '9e0a'), ('ProgramData', 'C:\\ProgramData'), ('ProgramFiles', 'C:\\Program Files'), ('ProgramFiles(x86)', 'C:\\Program Files (x86)'), ('ProgramW6432', 'C:\\Program Files'), ('PROMPT', '$P$G'), ('PSModulePath', 'C:\\Program Files\\WindowsPowerShell\\Modules;C:\\Windows\\system32\\WindowsPowerShell\\v1.0\\Modules'), ('PUBLIC', 'C:\\Users\\Public'), ('PYTHONPATH', 'C:\\Python38\\'), ('Qt5_DIR', 'C:\\Qt\\Qt5.12.12\\5.12.12\\msvc2017_64'), ('QT_QPA_PLATFORM_PLUGIN_PATH', 'C:\\Qt\\Qt5.12.12\\5.12.12\\msvc2017_64\\plugins\\platforms'), ('SystemDrive', 'C:'), ('SystemRoot', 'C:\\Windows'), ('TEMP', 'C:\\Users\\PC\\AppData\\Local\\Temp'), ('TMP', 'C:\\Users\\PC\\AppData\\Local\\Temp'), ('USERDOMAIN', 'DESKTOP-S1BRI82'), ('USERDOMAIN_ROAMINGPROFILE', 'DESKTOP-S1BRI82'), ('USERNAME', 'PC'), ('USERPROFILE', 'C:\\Users\\PC'), ('windir', 'C:\\Windows'), ('ZES_ENABLE_SYSMAN', '1')]), 'shell': False} +[0.047000] (flatland_viz) StderrLine: {'line': b'CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):\n'} +[0.047000] (flatland_viz) StderrLine: {'line': b' Compatibility with CMake < 2.8.12 will be removed from a future version of\n'} +[0.047000] (flatland_viz) StderrLine: {'line': b' CMake.\n'} +[0.047000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.047000] (flatland_viz) StderrLine: {'line': b' Update the VERSION argument value or use a ... suffix to tell\n'} +[0.047000] (flatland_viz) StderrLine: {'line': b' CMake that the project does not need compatibility with older versions.\n'} +[0.047000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.047000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.078000] (flatland_viz) StderrLine: {'line': b'CMake Error at CMakeLists.txt:10 (if):\n'} +[0.078000] (flatland_viz) StderrLine: {'line': b' if given arguments:\n'} +[0.078000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.078000] (flatland_viz) StderrLine: {'line': b' "STREQUAL" "14.04"\n'} +[0.078000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.078000] (flatland_viz) StderrLine: {'line': b' Unknown arguments specified\n'} +[0.078000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.078000] (flatland_viz) StderrLine: {'line': b'\n'} +[0.078000] (flatland_viz) StdoutLine: {'line': b'-- Configuring incomplete, errors occurred!\n'} +[0.078000] (flatland_viz) StdoutLine: {'line': b'See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log".\n'} +[0.094000] (flatland_viz) CommandEnded: {'returncode': 1} +[0.094000] (-) TimerEvent: {} +[0.140000] (flatland_viz) JobEnded: {'identifier': 'flatland_viz', 'rc': 1} +[0.156000] (-) EventReactorShutdown: {} diff --git a/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/command.log b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/command.log new file mode 100644 index 00000000..1e5f2011 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/command.log @@ -0,0 +1,6 @@ +Invoking command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz': HOMEPATH=%HOMEPATH% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +Invoked command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz' returned '1': HOMEPATH=%HOMEPATH% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz diff --git a/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stderr.log b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stderr.log new file mode 100644 index 00000000..102ec265 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stderr.log @@ -0,0 +1,16 @@ +CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): + Compatibility with CMake < 2.8.12 will be removed from a future version of + CMake. + + Update the VERSION argument value or use a ... suffix to tell + CMake that the project does not need compatibility with older versions. + + +CMake Error at CMakeLists.txt:10 (if): + if given arguments: + + "STREQUAL" "14.04" + + Unknown arguments specified + + diff --git a/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stdout.log b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stdout.log new file mode 100644 index 00000000..2cfe1293 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stdout.log @@ -0,0 +1,2 @@ +-- Configuring incomplete, errors occurred! +See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log". diff --git a/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stdout_stderr.log b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stdout_stderr.log new file mode 100644 index 00000000..6cd1a02c --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/stdout_stderr.log @@ -0,0 +1,18 @@ +CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): + Compatibility with CMake < 2.8.12 will be removed from a future version of + CMake. + + Update the VERSION argument value or use a ... suffix to tell + CMake that the project does not need compatibility with older versions. + + +CMake Error at CMakeLists.txt:10 (if): + if given arguments: + + "STREQUAL" "14.04" + + Unknown arguments specified + + +-- Configuring incomplete, errors occurred! +See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log". diff --git a/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/streams.log b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/streams.log new file mode 100644 index 00000000..58b2f3ee --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-05-41/flatland_viz/streams.log @@ -0,0 +1,24 @@ +[0.031s] Invoking command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz': HOMEPATH=%HOMEPATH% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +[0.047s] CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required): +[0.047s] Compatibility with CMake < 2.8.12 will be removed from a future version of +[0.047s] CMake. +[0.047s] +[0.047s] Update the VERSION argument value or use a ... suffix to tell +[0.047s] CMake that the project does not need compatibility with older versions. +[0.047s] +[0.047s] +[0.078s] CMake Error at CMakeLists.txt:10 (if): +[0.078s] if given arguments: +[0.078s] +[0.078s] "STREQUAL" "14.04" +[0.078s] +[0.078s] Unknown arguments specified +[0.078s] +[0.078s] +[0.078s] -- Configuring incomplete, errors occurred! +[0.078s] See also "C:/Users/PC/Documents/Aulas/5ano/1semestre/RI/flatland/flatland_viz/build/flatland_viz/CMakeFiles/CMakeOutput.log". +[0.094s] Invoked command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz' returned '1': HOMEPATH=%HOMEPATH% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz diff --git a/flatland_viz/log/build_2022-11-19_19-05-41/logger_all.log b/flatland_viz/log/build_2022-11-19_19-05-41/logger_all.log new file mode 100644 index 00000000..86106945 --- /dev/null +++ b/flatland_viz/log/build_2022-11-19_19-05-41/logger_all.log @@ -0,0 +1,71 @@ +[0.543s] colcon DEBUG Command line arguments: ['C:\\Python38\\Scripts\\colcon', 'build', '--cmake-args', '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja'] +[0.543s] colcon DEBUG Parsed command line arguments: Namespace(ament_cmake_args=None, base_paths=['.'], build_base='build', catkin_cmake_args=None, catkin_skip_building_tests=False, cmake_args=['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja'], cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, cmake_target=None, cmake_target_skip_unavailable=False, continue_on_error=False, event_handlers=None, executor='parallel', ignore_user_meta=False, install_base='install', log_base=None, log_level=None, main=>, merge_install=False, metas=['./colcon.meta'], packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_end=None, packages_ignore=None, packages_ignore_regex=None, packages_select=None, packages_select_build_failed=False, packages_select_by_dep=None, packages_select_regex=None, packages_select_test_failures=False, packages_skip=None, packages_skip_build_finished=False, packages_skip_by_dep=None, packages_skip_regex=None, packages_skip_test_passed=False, packages_skip_up_to=None, packages_start=None, packages_up_to=None, packages_up_to_regex=None, parallel_workers=12, paths=None, symlink_install=False, test_result_base=None, verb_extension=, verb_name='build', verb_parser=) +[0.598s] colcon.colcon_core.package_discovery Level 1 discover_packages(colcon_meta) check parameters +[0.598s] colcon.colcon_core.package_discovery Level 1 discover_packages(recursive) check parameters +[0.598s] colcon.colcon_core.package_discovery Level 1 discover_packages(ignore) check parameters +[0.598s] colcon.colcon_core.package_discovery Level 1 discover_packages(path) check parameters +[0.598s] colcon.colcon_core.package_discovery Level 1 discover_packages(colcon_meta) discover +[0.598s] colcon.colcon_core.package_discovery Level 1 discover_packages(recursive) discover +[0.598s] colcon.colcon_core.package_discovery INFO Crawling recursively for packages in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz' +[0.598s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.599s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'ignore' +[0.599s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'ignore_ament_install' +[0.599s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['colcon_pkg'] +[0.599s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'colcon_pkg' +[0.599s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['colcon_meta'] +[0.599s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'colcon_meta' +[0.599s] colcon.colcon_core.package_identification Level 1 _identify(.) by extensions ['ros'] +[0.599s] colcon.colcon_core.package_identification Level 1 _identify(.) by extension 'ros' +[0.633s] colcon.colcon_core.package_identification DEBUG Package '.' with type 'ros.catkin' and name 'flatland_viz' +[0.633s] colcon.colcon_core.package_discovery Level 1 discover_packages(recursive) using defaults +[0.633s] colcon.colcon_core.package_discovery Level 1 discover_packages(ignore) discover +[0.633s] colcon.colcon_core.package_discovery Level 1 discover_packages(ignore) using defaults +[0.633s] colcon.colcon_core.package_discovery Level 1 discover_packages(path) discover +[0.633s] colcon.colcon_core.package_discovery Level 1 discover_packages(path) using defaults +[0.688s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_args' from command line to '['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja']' +[0.688s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_target' from command line to 'None' +[0.689s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_target_skip_unavailable' from command line to 'False' +[0.689s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_clean_cache' from command line to 'False' +[0.689s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_clean_first' from command line to 'False' +[0.689s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'cmake_force_configure' from command line to 'False' +[0.689s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'ament_cmake_args' from command line to 'None' +[0.689s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'catkin_cmake_args' from command line to 'None' +[0.689s] colcon.colcon_core.verb Level 5 set package 'flatland_viz' build argument 'catkin_skip_building_tests' from command line to 'False' +[0.689s] colcon.colcon_core.verb DEBUG Building package 'flatland_viz' with the following arguments: {'ament_cmake_args': None, 'build_base': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\build\\flatland_viz', 'catkin_cmake_args': None, 'catkin_skip_building_tests': False, 'cmake_args': ['-DCMAKE_EXPORT_COMPILE_COMMANDS=ON', '-G', 'Ninja'], 'cmake_clean_cache': False, 'cmake_clean_first': False, 'cmake_force_configure': False, 'cmake_target': None, 'cmake_target_skip_unavailable': False, 'install_base': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz\\install\\flatland_viz', 'merge_install': False, 'path': 'C:\\Users\\PC\\Documents\\Aulas\\5ano\\1semestre\\RI\\flatland\\flatland_viz', 'symlink_install': False, 'test_result_base': None} +[0.692s] colcon.colcon_core.executor INFO Executing jobs using 'parallel' executor +[0.711s] colcon.colcon_parallel_executor.executor.parallel DEBUG run_until_complete +[0.711s] colcon.colcon_ros.task.catkin.build INFO Building ROS package in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz' with build type 'catkin' +[0.711s] colcon.colcon_cmake.task.cmake.build INFO Building CMake package in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz' +[0.718s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_core.shell.sh': Not used on Windows systems +[0.718s] colcon.colcon_core.shell INFO Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell +[0.746s] colcon.colcon_core.event_handler.log_command DEBUG Invoking command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz': HOMEPATH=%HOMEPATH% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +[0.808s] colcon.colcon_core.event_handler.log_command DEBUG Invoked command in 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\build\flatland_viz' returned '1': HOMEPATH=%HOMEPATH% +IntelliJ IDEA Community Edition=C:\Program Files\JetBrains\IntelliJ IDEA Community Edition 2022.2.2\bin; +JetBrains Rider=C:\Program Files\JetBrains\JetBrains Rider 2021.3.3\bin; C:\Program Files\CMake\bin\cmake.EXE C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja -DCATKIN_INSTALL_INTO_PREFIX_ROOT=0 -DCMAKE_INSTALL_PREFIX=C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz +[0.836s] colcon.colcon_core.environment Level 1 create_environment_scripts_only(flatland_viz) +[0.853s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz' for CMake module files +[0.854s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz' for CMake config files +[0.855s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\bin' +[0.855s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\bin' +[0.855s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\lib\pkgconfig\flatland_viz.pc' +[0.856s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\Lib\site-packages' +[0.856s] colcon.colcon_core.environment Level 1 checking 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\Scripts' +[0.861s] colcon.colcon_core.shell INFO Creating package script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\flatland_viz\package.ps1' +[0.863s] colcon.colcon_core.shell INFO Creating package script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\flatland_viz\package.bat' +[0.864s] colcon.colcon_core.shell INFO Creating package descriptor 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\flatland_viz\package.dsv' +[0.865s] colcon.colcon_core.environment Level 1 create_file_with_runtime_dependencies(C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\flatland_viz\share\colcon-core\packages\flatland_viz) +[0.877s] colcon.colcon_parallel_executor.executor.parallel DEBUG run_until_complete finished with '1' +[0.877s] colcon.colcon_core.event_reactor DEBUG joining thread +[0.888s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_notification.desktop_notification.notify2': 'notify2' not found +[0.888s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_notification.desktop_notification.notify_send': Not used on non-Linux systems +[0.888s] colcon.colcon_core.plugin_system INFO Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[0.888s] colcon.colcon_notification.desktop_notification INFO Sending desktop notification using 'win32' +[5.927s] colcon.colcon_core.event_reactor DEBUG joined thread +[5.934s] colcon.colcon_core.shell INFO Creating prefix script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\local_setup.ps1' +[5.935s] colcon.colcon_core.shell INFO Creating prefix util module 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\_local_setup_util_ps1.py' +[5.937s] colcon.colcon_core.shell INFO Creating prefix chain script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\setup.ps1' +[5.944s] colcon.colcon_core.shell INFO Creating prefix script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\local_setup.bat' +[5.945s] colcon.colcon_core.shell INFO Creating prefix util module 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\_local_setup_util_bat.py' +[5.946s] colcon.colcon_core.shell INFO Creating prefix chain script 'C:\Users\PC\Documents\Aulas\5ano\1semestre\RI\flatland\flatland_viz\install\setup.bat' diff --git a/flatland_viz/log/latest b/flatland_viz/log/latest new file mode 120000 index 00000000..b57d247c --- /dev/null +++ b/flatland_viz/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/flatland_viz/log/latest_build b/flatland_viz/log/latest_build new file mode 120000 index 00000000..c1e9cdbc --- /dev/null +++ b/flatland_viz/log/latest_build @@ -0,0 +1 @@ +build_2022-11-19_19-05-41 \ No newline at end of file From 5d0585f35f57e8c23f50bf2af76b8484119389cd Mon Sep 17 00:00:00 2001 From: schmiddey Date: Wed, 30 Aug 2023 19:25:42 +0200 Subject: [PATCH 66/66] changed to c++17 and added ns to diff_drive and laser plugin --- flatland_msgs/CMakeLists.txt | 2 +- flatland_plugins/CMakeLists.txt | 8 ++++---- flatland_plugins/src/diff_drive.cpp | 8 ++++++++ flatland_plugins/src/laser.cpp | 4 ++++ flatland_server/CMakeLists.txt | 8 ++++---- flatland_viz/CMakeLists.txt | 2 +- 6 files changed, 22 insertions(+), 10 deletions(-) diff --git a/flatland_msgs/CMakeLists.txt b/flatland_msgs/CMakeLists.txt index c80ce9b4..b87c6ba1 100644 --- a/flatland_msgs/CMakeLists.txt +++ b/flatland_msgs/CMakeLists.txt @@ -8,7 +8,7 @@ endif() # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/flatland_plugins/CMakeLists.txt b/flatland_plugins/CMakeLists.txt index e3d79242..063cdd9d 100644 --- a/flatland_plugins/CMakeLists.txt +++ b/flatland_plugins/CMakeLists.txt @@ -3,12 +3,12 @@ cmake_minimum_required(VERSION 3.5) project(flatland_plugins) # Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() +# if(NOT CMAKE_CXX_STANDARD) +# set(CMAKE_CXX_STANDARD 17) +# endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -std=c++17) endif() diff --git a/flatland_plugins/src/diff_drive.cpp b/flatland_plugins/src/diff_drive.cpp index 60880e59..be53b3b4 100644 --- a/flatland_plugins/src/diff_drive.cpp +++ b/flatland_plugins/src/diff_drive.cpp @@ -110,6 +110,14 @@ void DiffDrive::OnInitialize(const YAML::Node & config) throw YAMLException("Body with name " + Q(body_name) + " does not exist"); } + //append namespace to topics + std::string ns = this->GetModel()->GetNameSpace(); + twist_topic = ns + "/" + twist_topic; + odom_topic = ns + "/" + odom_topic; + ground_truth_topic = ns + "/" + ground_truth_topic; + twist_pub_topic = ns + "/" + twist_pub_topic; + + // publish and subscribe to topics using std::placeholders::_1; twist_sub_ = node_->create_subscription( diff --git a/flatland_plugins/src/laser.cpp b/flatland_plugins/src/laser.cpp index f044b006..e93a008e 100644 --- a/flatland_plugins/src/laser.cpp +++ b/flatland_plugins/src/laser.cpp @@ -68,6 +68,10 @@ void Laser::OnInitialize(const YAML::Node & config) ParseParameters(config); update_timer_.SetRate(update_rate_); + + //add namespace + std::string ns = GetModel()->GetName(); + topic_ = ns + "/" + topic_; scan_publisher_ = node_->create_publisher(topic_, 1); // construct the body to laser transformation matrix once since it never diff --git a/flatland_server/CMakeLists.txt b/flatland_server/CMakeLists.txt index 51f0c792..7ee6cbf8 100644 --- a/flatland_server/CMakeLists.txt +++ b/flatland_server/CMakeLists.txt @@ -3,12 +3,12 @@ cmake_minimum_required(VERSION 3.5) project(flatland_server) # Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() +# if(NOT CMAKE_CXX_STANDARD) +# set(CMAKE_CXX_STANDARD 17) +# endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -std=c++17) endif() ## Find macros and libraries diff --git a/flatland_viz/CMakeLists.txt b/flatland_viz/CMakeLists.txt index 8243d33b..5e7cc787 100644 --- a/flatland_viz/CMakeLists.txt +++ b/flatland_viz/CMakeLists.txt @@ -3,7 +3,7 @@ project(flatland_viz) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")