diff --git a/.gitignore b/.gitignore index 1139ed6..eecddd4 100644 --- a/.gitignore +++ b/.gitignore @@ -43,3 +43,9 @@ Assignment1/CMakeSettings.json **/.vs/VSWorkspaceState.json .idea/ cmake-build-debug/ + + +Final/SFML-2.4.2 +Final/.vs +**/.vs/ +**/CMakeSettings.json diff --git a/build_instructions b/Assignment3/build_instructions similarity index 100% rename from build_instructions rename to Assignment3/build_instructions diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b2b496..bef4920 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,6 @@ -cmake_minimum_required(VERSION 3.6) -project(PhysicallyBasedSimulation LANGUAGES CXX) - +cmake_minimum_required(VERSION 3.5.1) +project(FinalProject LANGUAGES CXX) add_subdirectory( - Final -) - + Final +) \ No newline at end of file diff --git a/Final/CMakeLists.txt b/Final/CMakeLists.txt index 9c4a4b6..3b5b3e3 100644 --- a/Final/CMakeLists.txt +++ b/Final/CMakeLists.txt @@ -1,30 +1,37 @@ cmake_minimum_required(VERSION 3.5.1) project(FinalProject LANGUAGES CXX) -set(CMAKE_BUILD_TYPE Debug) +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/modules/" ${CMAKE_MODULE_PATH}) if(MSVC) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /std:c++latest /wd4141") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /fp:strict /std:c++latest /wd4141") set(CMAKE_CXX_FLAGS_RELEASE "/Ox /Ot /DNDEBUG") else() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -Wall") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++1z -Wall") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG") endif() -set(SOURCE_FILES - ${CMAKE_CURRENT_SOURCE_DIR}/Main.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/Polygon.cpp +file(GLOB SOURCE_FILES + "*.h" + "*.cpp" ) -add_executable(FinalProject ${SOURCE_FILES}) +add_executable(FinalProject ${SOURCE_FILES} ${HEADERS}) target_include_directories(FinalProject PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) -set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/modules/" ${CMAKE_MODULE_PATH}) +set(SFML_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/SFML-2.4.2) find_package(SFML 2 REQUIRED network audio graphics window system) - -if(SFML_FOUND) - include_directories(${SFML_INCLUDE_DIR}) - target_link_libraries(FinalProject ${SFML_LIBRARIES} ${SFML_DEPENDENCIES}) -endif() \ No newline at end of file +include_directories(${SFML_INCLUDE_DIR}) +target_link_libraries(FinalProject ${SFML_LIBRARIES} ${SFML_DEPENDENCIES}) + +find_package(Eigen3 REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIR}) +target_link_libraries(FinalProject ${EIGEN3_LIBRARIES} ${EIGEN3_DEPENDENCIES}) + +FIND_PACKAGE(OpenMP) +IF(OPENMP_FOUND) + SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +ENDIF() diff --git a/Final/Console.hpp b/Final/Console.hpp new file mode 100644 index 0000000..6d6847b --- /dev/null +++ b/Final/Console.hpp @@ -0,0 +1,122 @@ +#pragma once + +#include + +#include +#include +#include +#include + +class console +{ +public: + static console& instance() + { + static console console; + return console; + } + + void init() + { + if (!font.loadFromFile("arial.ttf")) + { + // error... + } + + m_text.setFont(font); + m_text.setCharacterSize(12); + m_text.setFillColor(sf::Color::White); + } + + template + void set_param(const std::string& name, const T& value) + { + std::stringstream stm; + stm << std::fixed << std::setprecision(2) << value; + + m_params[name] = stm.str(); + update(); + } + + void print(sf::RenderWindow& window) + { + std::lock_guard ga{ m_lock }; + + window.draw(m_text); + } + +private: + console() + { + + } + + void update() + { + std::stringstream stm; + + for (auto itr : m_params) + stm << itr.first + ": " + itr.second << std::endl; + + { + std::lock_guard ga{ m_lock }; + m_text.setString(stm.str()); + } + } + + std::mutex m_lock; + sf::Text m_text; + std::unordered_map m_params; + sf::Font font; +}; + +template +void measure(const std::string name, SmootherType& smoother, const FuncType& func) +{ + using namespace std::chrono; + using clock = high_resolution_clock; + + const auto start_frame = clock::now(); + + func(); + + const auto elapsed = duration_cast(clock::now() - start_frame); + + smoother.add(static_cast(elapsed.count())); + + console::instance().set_param(name, smoother.get()); +}; + +template()>>>> +void measure(const std::string name, const FuncType& func) +{ + using namespace std::chrono; + using clock = high_resolution_clock; + + const auto start_frame = clock::now(); + + func(); + + const auto elapsed = duration_cast(clock::now() - start_frame); + + console::instance().set_param(name, elapsed.count()); +}; + +template()>>>> +auto measure(const std::string name, const FuncType& func) +{ + using namespace std::chrono; + using clock = high_resolution_clock; + + const auto start_frame = clock::now(); + + auto result = func(); + + const auto elapsed = duration_cast(clock::now() - start_frame); + + console::instance().set_param(name, elapsed.count()); + + return result; +}; \ No newline at end of file diff --git a/Final/ContactInfo.hpp b/Final/ContactInfo.hpp new file mode 100644 index 0000000..c5b06d6 --- /dev/null +++ b/Final/ContactInfo.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include "physical_object.h" + + +class contact_info +{ +public: + contact_info(physical_object& point_owner, + const point_t& point, + physical_object& line_owner, + const line_t& line, + const double penetration_depth, + const Vector2d& point_of_intersection) + : point_owner_(point_owner), + point_(point), + line_owner_(line_owner), + line_(line), + penetration_depth_(penetration_depth), + point_of_intersection_(point_of_intersection) + { + } + + physical_object& line_owner() const { return line_owner_; } + physical_object& point_owner() const { return point_owner_; } + + const point_t& line_start_point() const { return line_owner_.points()[std::get<0>(line_)]; } + const point_t& line_end_point() const { return line_owner_.points()[std::get<1>(line_)]; } + + Vector2d line_start_offset() const { return std::get<0>(line_start_point()); } + Vector2d line_end_offset() const { return std::get<0>(line_end_point()); } + Vector2d line_offset() const { return (line_start_offset() + line_end_offset()) / 2.0; } + + const point_t& contact_point() const { return point_; } + const Vector2d& contact_point_offset() const { return std::get<0>(point_); } + const Vector2d& contact_point_velocity() const { return std::get<1>(point_); } + + Vector2d relative_velocity() const + { + const Vector2d line_velocity = + (std::get<1>(line_start_point()) + + std::get<1>(line_end_point())) / 2.0; + + return contact_point_velocity() - line_velocity; + } + + Vector2d normal() const + { + return ::normal(line_start_offset(), line_end_offset()); + } + + double penetration_depth() const { return penetration_depth_; } + +private: + physical_object& point_owner_; + point_t point_; + + physical_object& line_owner_; + line_t line_; + + double penetration_depth_; + Vector2d point_of_intersection_; +}; \ No newline at end of file diff --git a/Final/Main.cpp b/Final/Main.cpp index de85acb..4a5d653 100644 --- a/Final/Main.cpp +++ b/Final/Main.cpp @@ -1,106 +1,448 @@ -/****************************************************************** -* -* Main.cpp -* -* Description: This is an implementation of a collision detection -* exmple. -* -* Physically-Based Simulation Proseminar WS 2016 -* -* Interactive Graphics and Simulation Group -* Institute of Computer Science -* University of Innsbruck -* -*******************************************************************/ - -/* Standard includes */ -#include +#include +#include #include #include -#include -/* Local includes */ #include "Polygon.h" -#include "globals.h" -#include "chrono_timer.h" +#include "Console.hpp" +#include "smoother.h" +#include "collision_detection.h" +#include "collision_resolution.h" -/*----------------------------------------------------------------*/ +const sf::Color clear_color({ 10, 10, 20 }); -bool add_polygon = false; -bool increase_polygon = false; -bool random_polygon = false; -int polygon = 3; -std::vector > polygons (0); +enum class game_loop +{ + fixed = 0, + variable, + unkown +}; -double xs = 0; -double ys = 0; +void render(sf::RenderWindow& window, const std::vector& polygons, + const sf::VertexArray& custom_polygon) +{ + window.clear(clear_color); + + for (auto& polygon : polygons) + { + polygon.draw(window); + } -int width = 600; -int height = 600; + for (auto& polygon : polygons) + { + polygon.draw_debug(window); + } -long long dt = 0; + console::instance().print(window); -int main() -{ - sf::RenderWindow window(sf::VideoMode(width, height), "2D Collision detection"); - ChronoTimer t("Unnamed"); + // --------------------------------------------------- + // draw custom polygon being created + if(custom_polygon.getVertexCount() != 0) { - Polygon p = Polygon::Circle(Vector2(200,300),30); - std::shared_ptr < Polygon > sp = std::make_shared(p); - polygons.push_back(sp); - Polygon p1 = Polygon::Square(Vector2(200,500),Vector2(-300,50)); - std::shared_ptr < Polygon > sp1 = std::make_shared(p1); - polygons.push_back(sp1); + window.draw(custom_polygon); + + for(auto i = 0u; i < custom_polygon.getVertexCount(); ++i) + { + sf::CircleShape corner(4); + corner.setOrigin(corner.getRadius(), corner.getRadius()); + corner.setPosition(custom_polygon[i].position); + corner.setFillColor(sf::Color::Transparent); + corner.setOutlineColor(sf::Color::Yellow); + corner.setOutlineThickness(2); + + window.draw(corner); + } } - while (window.isOpen()) + + // --------------------------------------------------- + + window.display(); +} + +void update(std::vector& polygons, const double dt) +{ + // detect collisions + auto contacts = measure("Update: Collision Detection", [&] { - dt = t.get_milliseconds(); + return collision_detection(polygons); + }); + // apply impulses + measure("Update: Collision Resolution", [&] + { + collision_resolution(contacts, dt); + }); + + // integrate positions/velocities + measure("Update: Integration", [&] + { + #pragma omp parallel for + for (auto i = 0; i < polygons.size(); ++i) + { + polygons[i].update(dt); + } + }); + + // apply position correction + measure("Update: Position Correction", [&] + { + correct_positions(contacts); + }); +} + +int main() +{ + auto increase_polygon = false; + auto create_custom_polygon = false; + polygon *selected = nullptr; + + auto polygon_vertex_count = 4; + auto scroll_vertex_count = 0; + constexpr auto max_vertices = 10; + + auto custom_polygon = sf::VertexArray(sf::LineStrip); + + console::instance().init(); + + constexpr auto width = 1280; + constexpr auto height = 800; + + const auto mouse_correction_x = 10; + const auto mouse_correction_y = 35; + + sf::RenderWindow window(sf::VideoMode(width, height), "2D Collision detection"); + +/* + sf::RenderWindow window(sf::VideoMode::getFullscreenModes().front(), "2D Collision detection", sf::Style::Fullscreen); + const auto mouse_correction_x = 0; + const auto mouse_correction_y = 0; +*/ + + // --------------------------------------------------------------------------- + // prepare scene + std::vector polygons; + + polygons.emplace_back(polygon::create_rectangle(Vector2d(400, 700), Vector2d(710, 40))); + polygons.back().get_physical_object().rotate(-5 * M_PI / 180); + polygons.back().scale(1); + + polygons.emplace_back(polygon::create_rectangle(Vector2d(900, 650), Vector2d(700, 40))); + polygons.back().get_physical_object().rotate(30 * M_PI / 180); + polygons.back().scale(1); + + polygons.emplace_back(polygon::create_random(Vector2d(600, 400), 6)); + polygons.back().scale(8); + + polygons.emplace_back(polygon::create_random(Vector2d(200, 420), 6)); + polygons.back().scale(8); + + polygons.emplace_back(polygon::create_circle(Vector2d(900, 300), 60)); + + const auto static_scene_size = polygons.size(); + + // --------------------------------------------------------------------------- + + auto game_loop_type = game_loop::variable; + + // function object used to process windows messages + auto process_events = [&, xs = 0, ys = 0, debug = false] () mutable + { sf::Event event; while (window.pollEvent(event)) { - if (event.type == sf::Event::Closed || - sf::Keyboard::isKeyPressed(sf::Keyboard::Escape) || - sf::Keyboard::isKeyPressed(sf::Keyboard::Q) ) - window.close(); - if(event.type == sf::Event::MouseMoved) { // <- this is how you check to see if an event - xs = sf::Mouse::getPosition().x - window.getPosition().x - 10; - ys = sf::Mouse::getPosition().y - window.getPosition().y - 35; - } - if(event.type == sf::Event::MouseButtonPressed) { // <- this is how you check to see if an event - Polygon p=Polygon(Vector2(xs,ys),polygon); - std::shared_ptr sp = std::make_shared(p); - polygons.push_back(sp); - increase_polygon = true; - } - if(event.type == sf::Event::MouseButtonReleased) { // <- this is how you check to see if an event - increase_polygon = false; - polygons.at(polygons.size()-1)->set_ready(); + switch (event.type) + { + case sf::Event::Closed: window.close(); break; + case sf::Event::MouseMoved: + { + xs = sf::Mouse::getPosition().x - window.getPosition().x - mouse_correction_x; + ys = sf::Mouse::getPosition().y - window.getPosition().y - mouse_correction_y; + if(selected != nullptr) + selected->set_center(as_world_coordinates(Vector2d({xs, + ys}))); + } break; + + case sf::Event::MouseWheelScrolled: + { + + scroll_vertex_count++; + if(scroll_vertex_count >= 5) + { + scroll_vertex_count = 0; + if(event.mouseWheelScroll.delta > 0) + polygon_vertex_count++; + else + polygon_vertex_count--; + if(polygon_vertex_count < 3) + polygon_vertex_count = max_vertices; + if(polygon_vertex_count > max_vertices) + polygon_vertex_count = 3; + } + } break; + + case sf::Event::MouseButtonPressed: + { + for(auto &polygon : polygons) + { + if(polygon.get_physical_object().get_type() != + object_type::fixed) + continue; + //Check if touching a polygon + const auto polypos = as_screen_coordinates(polygon.get_physical_object() + .center_of_mass_global()); + const auto polypos2 = Vector2d(polypos.x,polypos.y); + + const auto distance_to_center = (polypos2 - Vector2d({xs, + ys}) + ).norm(); + + const auto radius = polygon.get_physical_object() + .get_scale() / 2; + if (distance_to_center < radius) + { + selected = &polygon; + break; + } + } + + if(selected == nullptr) + { + if (event.mouseButton.button == sf::Mouse::Left) + { + custom_polygon.append( + { + { + static_cast(xs), + static_cast(ys) + }, + + sf::Color::Yellow + }); + + create_custom_polygon = true; + + } + else if (event.mouseButton.button == sf::Mouse::Right) + { + if (create_custom_polygon) + { + // cancel + custom_polygon.clear(); + create_custom_polygon = false; + } + else + { + if(polygon_vertex_count == max_vertices) + { + polygons.emplace_back(polygon::create_circle + (Vector2d(xs, ys), 10)); + } + else + { + polygons.emplace_back(polygon::create_random( + Vector2d(xs, ys), polygon_vertex_count)); + } + + auto &polygon = polygons.back(); + + polygon.enable_debug_info(debug); + polygon.get_physical_object().rotate(M_PI / 2); + + increase_polygon = true; + } + } + } + + } break; + + case sf::Event::MouseButtonReleased: + { + if(increase_polygon) + { + increase_polygon = false; + + polygons.at(polygons.size() - 1) + .get_physical_object() + .set_type(object_type::dynamic); + } + + selected = nullptr; + + } break; + + case sf::Event::KeyPressed: + { + if (sf::Keyboard::isKeyPressed(sf::Keyboard::Escape) || + sf::Keyboard::isKeyPressed(sf::Keyboard::Q)) + { + window.close(); + } + + if (sf::Keyboard::isKeyPressed(sf::Keyboard::R)) + { + while (polygons.size() > static_scene_size) + polygons.pop_back(); + } + + if (sf::Keyboard::isKeyPressed(sf::Keyboard::G)) + { + game_loop_type = static_cast(static_cast(game_loop_type) + 1); + if (game_loop_type == game_loop::unkown) + game_loop_type = game_loop::fixed; + } + + if (sf::Keyboard::isKeyPressed(sf::Keyboard::D)) + { + debug = !debug; + + for (auto& polygon : polygons) + polygon.enable_debug_info(debug); + } + + if(sf::Keyboard::isKeyPressed(sf::Keyboard::Return)) + { + if(create_custom_polygon && custom_polygon.getVertexCount() > 2) + { + create_custom_polygon = false; + + polygons.emplace_back( + polygon::create_custom(custom_polygon)); + + custom_polygon.clear(); + + polygons.back().get_physical_object + ().rotate(0); + + polygons.back() + .get_physical_object() + .set_type(object_type::dynamic); + + polygons.back().enable_debug_info(debug); + } + } + + if(sf::Keyboard::isKeyPressed(sf::Keyboard::Space)) + { + if(create_custom_polygon && custom_polygon.getVertexCount() > 2) + { + create_custom_polygon = false; + + polygons.emplace_back( + polygon::create_custom(custom_polygon)); + + custom_polygon.clear(); + + polygons.back().get_physical_object + ().rotate(0); + + polygons.back() + .get_physical_object() + .set_type(object_type::fixed); + + polygons.back().enable_debug_info(debug); + } + } + + } break; + + default: break; } - if(sf::Keyboard::isKeyPressed(sf::Keyboard::I)) { // <- this is how you check to see if an event - polygon = ((polygon + 1) % 7) + 3; - } - if(sf::Keyboard::isKeyPressed(sf::Keyboard::D)) { // <- this is how you check to see if an event - polygon = ((polygon - 1) % 7) + 3; - } - if(sf::Keyboard::isKeyPressed(sf::Keyboard::C)) { // <- this is how you check to see if an event - polygon = circle; - } - } - if(increase_polygon) - polygons.at(polygons.size()-1)->increase(.01); - - std::cout<draw()); - polygon->update(t.get_milliseconds() - dt); } - window.display(); - } + }; + + using namespace std::chrono; + using clock = high_resolution_clock; + + smoother draw_time_smooth; + smoother update_time_smooth; + + auto last_time = clock::now(); + + // fixed game loop function object + const auto fixed_game_loop = [&, interval = duration(16ms)]() + { + auto elapsed_ms = duration_cast(clock::now() - last_time); + + if (elapsed_ms < interval) + { + std::this_thread::sleep_for(0ms); + return; + } + + last_time = clock::now(); + + process_events(); + + while (elapsed_ms >= interval) + { + if (increase_polygon) + { + polygons.back().scale(1 + interval.count() * 2); + } + + measure("Update", update_time_smooth, [&] + { + update(polygons, interval.count()); + }); + + elapsed_ms = duration_cast(elapsed_ms - interval); + } + + console::instance().set_param("Update", update_time_smooth.get()); + + measure("Draw", draw_time_smooth, [&] + { + render(window, polygons,custom_polygon); + }); + }; + + // variable game loop function object + const auto variable_game_loop = [&]() + { + auto elapsed_s = duration_cast>(clock::now() - last_time); + last_time = clock::now(); + + process_events(); + + if (increase_polygon) + polygons.back().scale(1 + elapsed_s.count() * 2); + + measure("Update", update_time_smooth, [&] + { + update(polygons, elapsed_s.count()); + }); + + measure("Draw", draw_time_smooth, [&] + { + render(window, polygons,custom_polygon); + }); + }; + + while (window.isOpen()) + { + switch (game_loop_type) + { + case game_loop::fixed: + console::instance().set_param("GameLoop", "Fixed"); + fixed_game_loop(); + break; + case game_loop::variable: + console::instance().set_param("GameLoop", "Variable"); + variable_game_loop(); + break; + case game_loop::unkown: + default: + break; + } + if(polygon_vertex_count == max_vertices) + console::instance().set_param("Vertex Count", "Circle"); + else + console::instance().set_param("Vertex Count", polygon_vertex_count); + + } return 0; } \ No newline at end of file diff --git a/Final/Polygon.cpp b/Final/Polygon.cpp index 7d23656..62e4ed5 100644 --- a/Final/Polygon.cpp +++ b/Final/Polygon.cpp @@ -2,122 +2,385 @@ // Created by ivan on 16/01/18. // -#include #include #include -#include -#include #include "Polygon.h" -#include "globals.h" +#include -#define PI 3.14159265 +std::default_random_engine rng; +std::vector> create_mass_points( + const double density, + const std::vector& points) +{ + constexpr auto depth = 1.0; -using namespace std; + // Calculate value of shoelace formula + auto area = 0.0; + auto j = points.size() - 1; -Polygon::Polygon(Vector2 pos) : center(pos.x(),pos.y()), - vertices(4), - scale(1), - velocity({0,0}), - ready(false) { - this->points = vector(vertices); -} + for (auto i = 0u; i < points.size(); i++) + { + const auto point_i = points[i]; + const auto point_j = points[j]; -Polygon::Polygon(Vector2 pos, int vertices) : center(pos.x(),pos.y()), - vertices((vertices==10)?100:vertices), - scale(1), - velocity({0,0}), - ready(false) { - if (vertices != circle) { - this->points = vector(vertices); - const auto angle = 360.0 / (double)vertices; + area += (point_j.x() + point_i.x()) * (point_j.y() - point_i.y()); - std::uniform_real_distribution rnd_distr_distance(-angle/2, angle/2); + j = i; // j is previous vertex to i + } - for (int i = 0; i < vertices; ++i) { - const Vector2 direction = {cos((double)i * (angle + rnd_distr_distance(rng)) * PI / 180), - sin((double)i * (angle + rnd_distr_distance(rng)) * PI / 180)}; + area = abs(area / 2.0); - this->points[i] = direction; - } - } else { - this->points = vector(vertices); + // uniformly distribute mass + const auto total_mass = area * depth * density; + const auto mass_per_point = total_mass / points.size(); - const auto angle = 360.0 / (double)vertices; + // create mass points + std::vector> mass_points; + for(const auto& p : points) + { + mass_points.push_back({ as_world_coordinates(p), mass_per_point }); + } - for (int i = 0; i < vertices; ++i) { - const Vector2 direction = {cos((double)i * angle * PI / 180), sin((double)i * angle * PI / 180)}; + return mass_points; +} - this->points[i] = direction; - } +polygon::polygon(const Vector2d& center, std::vector points) + : physical_object_(as_world_coordinates(center), create_mass_points(0.05, points)) +{ + // initialize shape + shape_.setPointCount(points.size()); + + for (auto i = 0u; i < points.size(); i++) + { + shape_.setPoint(i, to_sf(points[i])); } + + const auto cof = as_screen_coordinates(physical_object_.center_of_mass_local()); + + shape_.setOutlineThickness(-0.05f); + shape_.setOrigin(cof); + + cof_shape_.setFillColor(sf::Color::Yellow); + cof_shape_.setRadius(0.05f); + cof_shape_.setOrigin( + { + cof_shape_.getRadius(), + cof_shape_.getRadius() + }); + + update_shapes(); +} + +void polygon::update(const double dt) +{ + physical_object_.update(dt); + + update_shapes(); + + update_color(color_); +} + +void polygon::update_shapes() +{ + shape_.setRotation(-static_cast(physical_object_.rotation().angle() * 180 / M_PI)); + shape_.setPosition(as_screen_coordinates(physical_object_.position())); + + cof_shape_.setPosition(as_screen_coordinates(physical_object_.center_of_mass_global())); } -void Polygon::update(double dt) { - dt /= 50; - if(ready) { - this->velocity += gravity * dt; - auto old_center = this->center; - this->center += this->velocity * dt; - ///////////////////////////////// - //////COLLISION DETECTION//////// - ///////////////////////////////// +void polygon::draw_debug(sf::RenderWindow& window) const +{ + if (!debug_output_) return; + + window.draw(cof_shape_); + + sf::CircleShape circle(2); + + circle.setOutlineColor(sf::Color{ 255, 127, 0 }); + circle.setFillColor(sf::Color{ 255, 127, 0 }); + circle.setOutlineThickness(1); + circle.setOrigin(circle.getRadius(), circle.getRadius()); + + sf::Vertex normal_line[2]; + + for (const auto& contact : contacts_) + { + const auto center = physical_object_.center_of_mass_global(); + const auto point = center + contact.contact_point_offset(); + + circle.setPosition(as_screen_coordinates(point)); + window.draw(circle); + + const auto n = contact.normal(); + + normal_line[0] = sf::Vertex(as_screen_coordinates(point), sf::Color{ 255, 127, 0 }); + normal_line[1] = sf::Vertex(as_screen_coordinates(point + n), sf::Color{ 255, 127, 0 }); + + window.draw(normal_line, 2, sf::Lines); } } -sf::ConvexShape Polygon::draw() const { - // create an empty shape - sf::ConvexShape convex; +void polygon::draw(sf::RenderWindow& window) const +{ + window.draw(shape_); +} + +void polygon::scale(const double factor) +{ + shape_.scale( + static_cast(factor), + static_cast(factor)); + + cof_shape_.scale( + static_cast(factor), + static_cast(factor)); + + physical_object_.set_scale(shape_.getScale().x); +} + +void polygon::update_color(const sf::Color& color) +{ + const sf::Color debug_color = { 100, 100, 100 }; + const sf::Color debug_color_dark = { 15, 15, 15 }; + const sf::Color debug_color_light = { 50, 50, 50 }; + + if(debug_output_) + { + switch (physical_object_.get_type()) + { + case object_type::fixed: + shape_.setOutlineColor(debug_color); + shape_.setFillColor(debug_color_light); + break; - convex.setPointCount(points.size()); + default: + shape_.setOutlineColor(debug_color); + shape_.setFillColor(debug_color_dark); + break; + } - for(int i=0;icenter - (points[i] * this->scale); - convex.setPoint(i, sf::Vector2f(point.x(), point.y())); + return; + } + + shape_.setOutlineColor(color); + + const sf::Color dark = { + static_cast(color.r / 2), + static_cast(color.g / 2), + static_cast(color.b / 2), + }; + + const sf::Color darker = { + static_cast(color.r / 10), + static_cast(color.g / 10), + static_cast(color.b / 10), + }; + + switch (physical_object_.get_type()) + { + case object_type::fixed: shape_.setFillColor(darker); break; + case object_type::dynamic: shape_.setFillColor(dark);; + case object_type::kinematic: shape_.setFillColor(dark);; + default: ; } - return convex; } -void Polygon::increase(double dt) { - this->scale+=dt; +void polygon::set_color(const sf::Color& color) +{ + color_ = color; + + update_color(color); } -void Polygon::set_ready() { - this->ready=true; +void polygon::set_center(const Vector2d& new_center) { + this->physical_object_.position(new_center); } -std::ostream& operator<<(std::ostream& os, const Polygon& p) +std::ostream& operator<<(std::ostream& os, const polygon& p) { os << "Polygon:"; - for (auto & point : p.points) { - Vector2 real_pos = - p.center + (p.scale * point); - os<< " " << real_pos.x() << ":" << real_pos.y(); + + const auto& shape = p.get_shape(); + + for (auto i = 0u; i < shape.getPointCount(); ++i) + { + const auto point = shape.getPoint(i); + + os<< " " << point.x << ":" << point.y; } + return os; } -Polygon Polygon::Square(Vector2 pos, Vector2 scale){ - Polygon retval = Polygon(pos); - retval.points[0] = Vector2(0,0); - retval.points[1] = Vector2(0,scale.y()); - retval.points[2] = scale; - retval.points[3] = Vector2(scale.x(),0); - return retval; +polygon polygon::create_rectangle(const Vector2d& pos, const Vector2d& size) +{ + const auto aspect = size.x() / size.y(); + const auto half_height = 0.5; + const auto half_width = half_height * aspect; + + std::vector points; + points.emplace_back(-half_width, -half_height); + points.emplace_back(-half_width, half_height); + points.emplace_back(half_width, half_height); + points.emplace_back(half_width, -half_height); + + polygon p(pos, points); + + p.scale(size.x() / half_width / 2); + p.set_color({ + static_cast(sf::Color::Cyan.r * 0.7), + static_cast(sf::Color::Cyan.g * 0.7), + static_cast(sf::Color::Cyan.b * 0.7), + }); + + return p; +} + +polygon polygon::create_circle(const Vector2d& center, const double radius) +{ + constexpr auto vertex_count = 20; + + const auto angle = 360.0 / vertex_count; + + std::vector points; + for (auto i = 0; i < vertex_count; ++i) + { + points.emplace_back( + cos(i * angle * M_PI / 180.0), + sin(i * angle * M_PI / 180.0)); + } + + polygon p(center, points); + + p.scale(radius); + p.set_color(sf::Color::Red); + + return p; +} + +polygon polygon::create_random(const Vector2d& center, const size_t vertex_count) +{ + const auto angle = 2 * M_PI / vertex_count; + + std::uniform_real_distribution rnd( + -angle / vertex_count, angle / vertex_count); + + std::vector points; + for (auto i = 0u; i < vertex_count; ++i) + { + const auto final_angle = i * angle + rnd(rng); + points.emplace_back( + std::cos(final_angle), + std::sin(final_angle)); + } + + polygon p(center, points); + + p.scale(15); + p.set_color(sf::Color::Green); + + return p; } -Polygon Polygon::Line(Vector2 start, Vector2 end){ - Polygon retval = Polygon(start); - retval.points[0] = Vector2(0,0); - retval.points[1] = Vector2(0.0001,0.0001); - retval.points[2] = end; - retval.points[3] = Vector2(end.x()-0.0001,end.y()-0.0001); - return retval; +Vector2d calc_centroid(const std::vector vertices) +{ + auto x = 0.0; + auto y = 0.0; + auto signed_area = 0.0; + auto x0 = 0.0; // Current vertex X + auto y0 = 0.0; // Current vertex Y + auto x1 = 0.0; // Next vertex X + auto y1 = 0.0; // Next vertex Y + auto a = 0.0; // Partial signed area + + // For all vertices except last + auto i = 0u; + for (i=0; i < vertices.size() - 1; ++i) + { + x0 = vertices[i].x(); + y0 = vertices[i].y(); + + x1 = vertices[i+1].x(); + y1 = vertices[i+1].y(); + + a = x0 * y1 - x1 * y0; + + signed_area += a; + + x += (x0 + x1) * a; + y += (y0 + y1) * a; + } + + // Do last vertex separately to avoid performing an expensive + // modulus operation in each iteration. + x0 = vertices[i].x(); + y0 = vertices[i].y(); + + x1 = vertices[0].x(); + y1 = vertices[0].y(); + + a = x0 * y1 - x1*y0; + + signed_area += a; + + x += (x0 + x1)*a; + y += (y0 + y1)*a; + + signed_area *= 0.5; + + x /= 6.0 * signed_area; + y /= 6.0 * signed_area; + + return { x, y }; } -Polygon Polygon::Circle(Vector2 center, double radius){ - Polygon retval = Polygon(center,circle); - retval.increase(radius); - return retval; -} \ No newline at end of file +polygon polygon::create_custom(sf::VertexArray custom_polygon) +{ + const auto actual_vertex_count = custom_polygon.getVertexCount(); + + std::vector temp_points(actual_vertex_count); + + for (auto i = 0u; i < custom_polygon.getVertexCount(); ++i) + { + temp_points[i] = Vector2d({ custom_polygon[i].position.x, + custom_polygon[i].position.y }); + } + + const auto center = calc_centroid(temp_points); + + std::vector points(actual_vertex_count); + + Vector2d min = temp_points[0] - center; + Vector2d max = temp_points[0] - center; + + for (auto i = 0u; i < actual_vertex_count; ++i) + { + points[i] = temp_points[i] - center; + + if (points[i].squaredNorm() < min.squaredNorm()) + { + min = points[i]; + } + + if (points[i].squaredNorm() > min.squaredNorm()) + { + max = points[i]; + } + } + + const auto scale = (max.norm() + min.norm()) / 2.0; + for (auto i = 0u; i < actual_vertex_count; ++i) + { + points[i] /= scale; + } + + + polygon p(center, points); + + p.scale(scale); + p.set_color(sf::Color::Yellow); + + return p; +} diff --git a/Final/Polygon.h b/Final/Polygon.h index e55e08f..4d7a2ae 100644 --- a/Final/Polygon.h +++ b/Final/Polygon.h @@ -1,42 +1,109 @@ -// -// Created by ivan on 16/01/18. -// - #ifndef PHYSICALLYBASEDSIMULATION_POLYGON_H #define PHYSICALLYBASEDSIMULATION_POLYGON_H -#include -#include +#include "physical_object.h" + #include +#include +#include "ContactInfo.hpp" + +constexpr auto screen_scale = 20; // px = 1m -#include "Vec2.h" +/** + * \brief Visual representation of physical object + */ +class polygon +{ +public: + polygon(const Vector2d& center, std::vector points); -static const Vector2 gravity = Vector2(0, 9.81); + static polygon create_rectangle(const Vector2d& pos, const Vector2d& scale); + static polygon create_circle(const Vector2d& center, const double radius); + static polygon create_random(const Vector2d& center, const size_t vertex_count); + static polygon create_custom(sf::VertexArray custom_polygon); -class Polygon { - const int vertices; - double scale; - Vector2 center; - Vector2 velocity; - std::vector points; - bool ready; + void update(const double dt); + void draw(sf::RenderWindow& window) const; + void draw_debug(sf::RenderWindow& window) const; - Polygon(Vector2 pos); + const sf::Shape& get_shape() const { return shape_; } -public: + physical_object& get_physical_object() { return physical_object_; } + const physical_object& get_physical_object() const { return physical_object_; } + + void scale(double dt); + void update_color(const sf::Color& color); + void set_center(const Vector2d& new_center); + + void add_contacts(const std::vector& contacts) + { + for (auto& contact : contacts) + { + if (&contact.point_owner() != &physical_object_) + continue; + + contacts_.push_back(contact); + } + } - Polygon(Vector2 pos, int vertices); + void clear_contacts() + { + contacts_.clear(); + } - static Polygon Square(Vector2 pos, Vector2 scale); - static Polygon Line(Vector2 start, Vector2 end); - static Polygon Circle(Vector2 center, double radius); + void enable_debug_info(const bool enable) { debug_output_ = enable; } + void toggle_debug_info() { debug_output_ = !debug_output_; } - void update(double dt); - sf::ConvexShape draw() const; - void increase(double dt); - void set_ready(); - friend std::ostream& operator<<(std::ostream& os, const Polygon& p); + friend std::ostream& operator<<(std::ostream& os, const polygon& p); +private: + sf::ConvexShape shape_; + sf::CircleShape cof_shape_; + + physical_object physical_object_; + std::vector contacts_; + + bool debug_output_ = false; + + sf::Color color_; + + void update_shapes(); + + void set_color(const sf::Color& color); }; +// ============================================================ +// utility functions to transform coordinates +// from/to screen space using a fixed scaling + +inline sf::Vector2f as_screen_coordinates(const Vector2d& v) +{ + return + { + static_cast(v.x()) * screen_scale, + -static_cast(v.y() * screen_scale) + }; +} + +inline Vector2d as_world_coordinates(const sf::Vector2f& v) +{ + return { v.x / screen_scale, -v.y / screen_scale}; +} + +inline Vector2d as_world_coordinates(const Vector2d& v) +{ + return { v.x() / screen_scale, -v.y() / screen_scale }; +} + +// ============================================================ + +inline sf::Vector2f to_sf(const Vector2d& v) +{ + return + { + static_cast(v.x()), + static_cast(v.y()) + }; +} + #endif //PHYSICALLYBASEDSIMULATION_POLYGON_H diff --git a/Final/Vec2.h b/Final/Vec2.h deleted file mode 100644 index 27dd020..0000000 --- a/Final/Vec2.h +++ /dev/null @@ -1,155 +0,0 @@ -/****************************************************************** -* -* Vec2.h -* -* Description: Code providing helper function for handling 2D -* vectors; standard operators are provided -* -* Physically-Based Simulation Proseminar WS 2015 -* -* Interactive Graphics and Simulation Group -* Institute of Computer Science -* University of Innsbruck -* -*******************************************************************/ - -#ifndef __VEC2_T_H__ -#define __VEC2_T_H__ - - -template -class Vector2T -{ -public: - Vector2T() { m_xy[0] = (T)0; m_xy[1] = (T)0;} - Vector2T(T v) { m_xy[0] = v; m_xy[1] = v; } - Vector2T(const T x, const T y) { m_xy[0] = x; m_xy[1] = y; } - Vector2T(const Vector2T &other) { *this = other; } - - Vector2T & operator=( const Vector2T &other ) - { - m_xy[0]= other.m_xy[0]; m_xy[1] = other.m_xy[1]; return *this; - } - - T& operator[](int i) { return m_xy[i]; } - - const T& x() const { return m_xy[0]; } - const T& y() const { return m_xy[1]; } - - const T& operator[](int i) const { return m_xy[i]; } - - bool operator==(const Vector2T &other) const - { - if(x()!=other.x()) return false; - if(y()!=other.y()) return false; - return true; - } - - bool operator!=(const Vector2T &other) const - { - return !(*this == other); - } - - Vector2T operator*( T s ) const - { - return Vector2T(m_xy[0]*s, m_xy[1]*s); - } - - Vector2T& operator*=( T s ) - { - m_xy[0] = m_xy[0] * s; - m_xy[1] = m_xy[1] * s; - return *this; - } - - Vector2T operator/( T s ) const - { - return Vector2T(m_xy[0]/s, m_xy[1]/s); - } - - Vector2T& operator/=( T s ) - { - m_xy[0] = m_xy[0] / s; - m_xy[1] = m_xy[1] / s; - return *this; - } - - Vector2T operator+( const Vector2T &v2 ) const - { - return Vector2T(x()+v2.x(),y()+v2.y()); - } - - Vector2T& operator+=( const Vector2T &v ) - { - m_xy[0] += v.x(); - m_xy[1] += v.y(); - return *this; - } - - Vector2T operator-( const Vector2T &v2 ) const - { - return Vector2T(x()-v2.x(),y()-v2.y()); - } - - Vector2T& operator-=( const Vector2T &v ) - { - m_xy[0] -= v.x(); - m_xy[1] -= v.y(); - return *this; - } - - Vector2T operator-() const - { - return Vector2T(-m_xy[0], -m_xy[1]); - } - - T operator|(const Vector2T &v2) const - { - return (*this)[0] * v2[0] + (*this)[1] * v2[1]; - } - - Vector2T normalized() const - { - return *this / length(); - } - - T norm() const - { - return length(); - } - - T sqrnorm() const - { - return squaredLength(); - } - - T length() const - { - return sqrt(x()*x() + y()*y()); - } - - T squaredLength() const - { - return (x()*x() + y()*y()); - } - - T magnitude() const - { - return sqrt(x() * x() + y() * y()); - } - - static int dim() { return 2; } - -private: - T m_xy[2]; -}; - -template -Vector2T operator*(T const &s, Vector2T const &v) -{ - return v*s; -} - -typedef Vector2T Vector2; - -#endif diff --git a/Final/arial.ttf b/Final/arial.ttf new file mode 100644 index 0000000..886789b Binary files /dev/null and b/Final/arial.ttf differ diff --git a/Final/chrono_timer.h b/Final/chrono_timer.h deleted file mode 100644 index 36336c5..0000000 --- a/Final/chrono_timer.h +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -#include -#include -#include - -class ChronoTimer { - const std::string name; - const std::chrono::time_point start; -public: - ChronoTimer(const std::string& name = "Unnamed") - : name(name), - start(std::chrono::high_resolution_clock::now()) { - } - ~ChronoTimer() { - auto elapsed = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); - std::cout << "Timer - " << name << ": " << elapsed.count() << " ms " << std::endl; - } - - long long get_milliseconds() - { - auto elapsed = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); - return elapsed.count(); - } -}; diff --git a/Final/collision_detection.h b/Final/collision_detection.h new file mode 100644 index 0000000..7c3579c --- /dev/null +++ b/Final/collision_detection.h @@ -0,0 +1,263 @@ +#ifndef COLLISION_DETECTION_H +#define COLLISION_DETECTION_H + +#include "physical_object.h" +#include "ContactInfo.hpp" +#include "Polygon.h" + + +/** + * \brief Returns if two line segments are intesecting + * and stores the intersection point in X and Y + * \param Ax Segment A starting x + * \param Ay Segment A starting y + * \param Bx Segment A ending x + * \param By Segment A ending y + * \param Cx Segment B starting x + * \param Cy Segment B starting y + * \param Dx Segment B ending x + * \param Dy Segment B ending y + * \param X [out] intersection x + * \param Y [out] intersection y + * \return True if segment A and B intersect, false otherwise + */ +inline bool lineSegmentIntersection( + double Ax, double Ay, + double Bx, double By, + double Cx, double Cy, + double Dx, double Dy, + double& X, double& Y) { + + double distAB, theCos, theSin, newX, ABpos; + + // Fail if either line segment is zero-length. + if (Ax == Bx && Ay == By || Cx == Dx && Cy == Dy) return false; + + // Fail if the segments share an end-point. + if (Ax == Cx && Ay == Cy || Bx == Cx && By == Cy + || Ax == Dx && Ay == Dy || Bx == Dx && By == Dy) { + return false; + } + + // (1) Translate the system so that point A is on the origin. + Bx -= Ax; By -= Ay; + Cx -= Ax; Cy -= Ay; + Dx -= Ax; Dy -= Ay; + + // Discover the length of segment A-B. + distAB = sqrt(Bx*Bx + By * By); + + // (2) Rotate the system so that point B is on the positive X axis. + theCos = Bx / distAB; + theSin = By / distAB; + newX = Cx * theCos + Cy * theSin; + Cy = Cy * theCos - Cx * theSin; Cx = newX; + newX = Dx * theCos + Dy * theSin; + Dy = Dy * theCos - Dx * theSin; Dx = newX; + + // Fail if segment C-D doesn't cross line A-B. + if (Cy<0. && Dy<0. || Cy >= 0. && Dy >= 0.) return false; + + // (3) Discover the position of the intersection point along line A-B. + ABpos = Dx + (Cx - Dx)*Dy / (Dy - Cy); + + // Fail if segment C-D crosses line A-B outside of segment A-B. + if (ABpos<0. || ABpos>distAB) return false; + + // (4) Apply the discovered position to line A-B in the original coordinate system. + X = Ax + ABpos * theCos; + Y = Ay + ABpos * theSin; + + // Success. + return true; +} + + +/** + * \brief Determines if a point is inside of the given object + * by shooting a ray from the point into a fixed direction. + * If the intersection count is odd, the point is inside + * \param point + * \param object + * \return True if inside, false otherwise + */ +inline bool inside( + const Vector2d& point, + const physical_object& object) +{ + auto intersections = 0; + + const Vector2d ray_dir = { 1, 0 }; + + const auto x2 = point.x() + ray_dir.x() * 100000.0; + const auto y2 = point.y() + ray_dir.y() * 100000.0; + + for (const auto& line : object.lines()) + { + const auto line_start_index = std::get<0>(line); + const auto line_end_index = std::get<1>(line); + + const auto line_start_point = object.points()[line_start_index]; + const auto line_end_point = object.points()[line_end_index]; + + const auto line_start = object.position() + std::get<0>(line_start_point); + const auto line_end = object.position() + std::get<0>(line_end_point); + + double ix, iy; + if (!lineSegmentIntersection( + point.x(), point.y(), + x2, y2, + line_start.x(), line_start.y(), + line_end.x(), line_end.y(), + ix, iy)) + continue; + + ++intersections; + } + + return intersections % 2 == 1; +} + +/** + * \brief Collects contacts between two objects + * Determines for each point in object_a if its + * in object_b and then finds the closest intersecting + * line and saves the contact + * \param object_a + * \param object_b + * \param [inout] contacts + */ +inline void find_intersections(physical_object& object_a, physical_object& object_b, std::vector& contacts) +{ + const auto position_a = object_a.position(); + const auto position_b = object_b.position(); + + for (const auto& point_a : object_a.points()) + { + const auto point_pos = position_a + std::get<0>(point_a); + + if (!inside(point_pos, object_b)) + continue; + + line_t nearest_line; + auto distance = std::numeric_limits::max(); + auto penetration_depth = 0.0; + Vector2d point_of_intersection; + + for (const auto& line : object_b.lines()) + { + const auto line_start_index = std::get<0>(line); + const auto line_end_index = std::get<1>(line); + + const auto& line_start_point = object_b.points()[line_start_index]; + const auto& line_end_point = object_b.points()[line_end_index]; + + const auto line_start = position_b + std::get<0>(line_start_point); + const auto line_end = position_b + std::get<0>(line_end_point); + + double ix, iy; + if (!lineSegmentIntersection( + position_a.x(), position_a.y(), + point_pos.x(), point_pos.y(), + line_start.x(), line_start.y(), + line_end.x(), line_end.y(), + ix, iy)) + continue; + + const auto current_distance = + (Vector2d(point_pos.x(), point_pos.y()) - Vector2d(ix, iy)).norm(); + + if (current_distance >= distance) + continue; + + distance = current_distance; + nearest_line = line; + penetration_depth = current_distance; + point_of_intersection = Vector2d(ix, iy); + } + + contacts.emplace_back(object_a, point_a, object_b, nearest_line, penetration_depth, point_of_intersection); + } +} + + +/** + * \brief Collects ALL intersections between a and b. + * Does a bounding sphere test first to improve + * performance + * \param a + * \param b + * \param [inout] contacts + * \return True if a and b intersect, false otherwise + */ +inline bool intersects(physical_object& a, physical_object& b, std::vector& contacts) +{ + // check bounding sphere + if ((a.center_of_mass_global() - + b.center_of_mass_global()).norm() > + a.bounding_radius() + + b.bounding_radius()) + return false; + + find_intersections(a, b, contacts); + find_intersections(b, a, contacts); + + return !contacts.empty(); +} + +/** + * \brief Returns a list of all contacts in the scene + * \param polygons + * \return List of contacts + */ +inline std::vector collision_detection(std::vector& polygons) +{ + std::vector global_contacts; + + #pragma omp parallel shared(global_contacts) + { + #pragma omp for + for (auto i = 0; i < polygons.size(); ++i) + { + polygons[i].clear_contacts(); + } + + #pragma omp for schedule(dynamic) + for (auto i = 0; i < polygons.size(); ++i) + { + auto& polygon_a = polygons[i]; + + for (auto j = i + 1; j < polygons.size(); ++j) + { + auto& polygon_b = polygons[j]; + + auto& object_a = polygon_a.get_physical_object(); + auto& object_b = polygon_b.get_physical_object(); + + // do not test fixed objects + if (object_a.get_type() == object_type::fixed && + object_b.get_type() == object_type::fixed) + continue; + + std::vector local_contacts; + if (!intersects(object_a, object_b, local_contacts)) + continue; + + // add contacts to polygons for debug info + #pragma omp critical + { + polygon_a.add_contacts(local_contacts); + polygon_b.add_contacts(local_contacts); + + std::move( + local_contacts.begin(), + local_contacts.end(), + std::back_inserter(global_contacts)); + } + } + } + } + + return global_contacts; +} +#endif // COLLISION_DETECTION_H diff --git a/Final/collision_resolution.h b/Final/collision_resolution.h new file mode 100644 index 0000000..b0914f4 --- /dev/null +++ b/Final/collision_resolution.h @@ -0,0 +1,229 @@ +#ifndef COLLISION_RESOLUTION_H +#define COLLISION_RESOLUTION_H + +#include "ContactInfo.hpp" + +/** + * \brief Calculates impulse norm J as shown in lecture + * \param a + * \param b + * \param offset_a + * \param offset_b + * \param normal + * \param rel_v_n + * \param restitution + * \param num_contacts + * \return Impulse norm + */ +inline double calc_impulse_norm( + const physical_object& a, + const physical_object& b, + const Vector2d& offset_a, + const Vector2d& offset_b, + const Vector2d& normal, + const double rel_v_n, + const double restitution, + const int num_contacts) +{ + const auto ra_n = cross2(offset_a, normal); + const auto rb_n = cross2(offset_b, normal); + + const auto t_a = a.inverse_mass() + a.inverse_inertia() * ra_n * ra_n; + const auto t_b = b.inverse_mass() + b.inverse_inertia() * rb_n * rb_n; + const auto denom = (t_a + t_b) * num_contacts; + + assert(std::abs(denom) > 0); + + return -(1 + restitution) * rel_v_n / denom; +} + +/** + * \brief Calculates the friction impulse using the tangential vector. + * Friction impulse norm is calculated the same way as the + * normal impulse norm but using the tangent instead of the normal. + * Distinguishes between static and dynamic friction + * \param a + * \param b + * \param offset_a + * \param offset_b + * \param normal + * \param rel_v + * \param rel_v_n + * \param j + * \param num_contacts + * \return + */ +inline Vector2d calc_friction_impulse( + physical_object& a, + physical_object& b, + const Vector2d& offset_a, + const Vector2d& offset_b, + const Vector2d& normal, + const Vector2d& rel_v, + const double rel_v_n, + const double j, + const int num_contacts) +{ + const auto tangent = (rel_v - normal * rel_v_n) + .normalized(); + + const auto ra_n = cross2(offset_a, tangent); + const auto rb_n = cross2(offset_b, tangent); + + const auto t_a = a.inverse_mass() + a.inverse_inertia() * ra_n * ra_n; + const auto t_b = b.inverse_mass() + b.inverse_inertia() * rb_n * rb_n; + const auto denom = (t_a + t_b) * num_contacts; + + assert(std::abs(denom) > 0); + + const auto jt = -rel_v.dot(tangent) / denom; + + if (std::abs(jt) < 0.000001) return { 0, 0 }; + + const auto static_friction = 0.61; + const auto dynamic_friction = 0.47; + + return std::abs(jt) < j * static_friction + ? (tangent * jt).eval() + : (tangent * -j * dynamic_friction).eval(); +} + +/** + * \brief Applies the contact impulse to the objects + * \param a + * \param b + * \param offset_a + * \param offset_b + * \param impulse + */ +inline void apply_impulse( + physical_object& a, + physical_object& b, + const Vector2d& offset_a, + const Vector2d& offset_b, + const Vector2d& impulse) +{ + a.apply_impulse(-impulse, offset_a); + b.apply_impulse( impulse, offset_b); +} + +/** + * \brief Applies normal and friction impulses to every object in the scene + * Uses a fixed restitution (0.3) if relative velocity is above a certain threshold + * (0 otherwise) to reduce jitter (restitution slop). + * Skips resolution if objects moving apart + * \param contacts + * \param dt + */ +inline void collision_resolution(const std::vector& contacts, const double dt) +{ + #pragma omp parallel for + for (auto i = 0; i < contacts.size(); ++i) + { + const auto& contact = contacts[i]; + + auto& a = contact.line_owner(); + auto& b = contact.point_owner(); + + const auto n = contact.normal(); + + // find the number of contacts between these two objects + const auto contact_points = std::count_if(contacts.begin(), contacts.end(), + [&b, &a](const contact_info& c) + { + return &c.point_owner() == &b && &c.line_owner() == &a || + &c.point_owner() == &a && &c.line_owner() == &b; + }); + + assert(contact_points > 0); + + // compute relative velocity + const auto rel_v = contact.relative_velocity(); + const auto rel_v_n = rel_v.dot(n); + + // do not apply impulses to objects + // which a parting + if (rel_v_n > 0) + continue; + + // apply restitution slop + const auto restitution_slop_sq = 0.5 * 0.5; + const auto e = rel_v.squaredNorm() < restitution_slop_sq + ? 0 + : 0.3; + + // normal impulse + const auto j = calc_impulse_norm(a, b, + contact.line_offset(), + contact.contact_point_offset(), + n, + rel_v_n, + e, + contact_points); + + assert(std::abs(j) < 100000); + + const auto normal_impulse = j * n; + + const auto friction_impulse = calc_friction_impulse(a, b, + contact.line_offset(), + contact.contact_point_offset(), + n, rel_v, rel_v_n, j, + contact_points); + + #pragma omp critical + { + apply_impulse(a, b, + contact.line_offset(), + contact.contact_point_offset(), + normal_impulse); + + + apply_impulse(a, b, + contact.line_offset(), + contact.contact_point_offset(), + friction_impulse); + } + } +} + +/** + * \brief Correct positions according to penetration depth + * to avoid objects sinking into each other. Also + * apply a slop here to avoid very small corrections + * \param contacts + */ +inline void correct_positions(const std::vector& contacts) +{ + #pragma omp parallel for + for (auto i = 0; i < contacts.size(); ++i) + { + const auto& contact = contacts[i]; + + auto& a = contact.line_owner(); + auto& b = contact.point_owner(); + + const auto n = contact.normal(); + + const auto percent = 0.3; // usually 20% to 80% + const auto slop = 0.05; // usually 0.01 to 0.1 + + // find the number of contacts between these two objects + const auto contact_points = std::count_if(contacts.begin(), contacts.end(), + [&b, &a](const contact_info& c) + { + return &c.point_owner() == &b && &c.line_owner() == &a || + &c.point_owner() == &a && &c.line_owner() == &b; + }); + + const Vector2d correction = std::max(contact.penetration_depth() - slop, 0.0) + / (a.inverse_mass() + b.inverse_mass()) * percent * n; + + #pragma omp critical + { + a.move(-a.inverse_mass() * correction / contact_points); + b.move(b.inverse_mass() * correction / contact_points); + } + } +} +#endif // COLLISION_RESOLUTION_H diff --git a/Final/globals.h b/Final/globals.h deleted file mode 100644 index 0d7fc19..0000000 --- a/Final/globals.h +++ /dev/null @@ -1,20 +0,0 @@ -// -// Created by ivan on 17/01/18. -// - -#ifndef PHYSICALLYBASEDSIMULATION_GLOBALS_H -#define PHYSICALLYBASEDSIMULATION_GLOBALS_H - -constexpr int circle = 30; - -extern int width; -extern int height; -constexpr double camera[] = {0.0, 1.0, 0.0, 1.0}; - -extern double xs; /* Coordinates of mouse */ -extern double ys; - - -static std::default_random_engine rng; - -#endif //PHYSICALLYBASEDSIMULATION_GLOBALS_H diff --git a/Final/physical_object.cpp b/Final/physical_object.cpp new file mode 100644 index 0000000..8b35d74 --- /dev/null +++ b/Final/physical_object.cpp @@ -0,0 +1,121 @@ +#include "physical_object.h" +#include + +physical_object::physical_object(const Vector2d& position, + const std::vector>& points) + : scale_(1), radius_(0), position_(position), rotation_(0), + velocity_(0, 0), force_(0, 0), torque_(0), angular_velocity_(0), points_(points.size()) +{ + // total mass + initial_mass_ = std::accumulate(points.begin(), points.end(), 0.0, + [](auto current_sum, auto p) + { + return current_sum + std::get<1>(p); + }); + + mass_ = initial_mass_; + + // initial position of center of mass + auto tmp = Vector2d(0, 0); + center_of_mass_ = std::accumulate(points.begin(), points.end(), tmp, + [](const auto& current_sum, auto p) + { + return current_sum + std::get<0>(p) * std::get<1>(p); + }) / mass_; + + for(auto i = 0u; i < points.size(); ++i) + { + // save offset + initial_offsets_.push_back( + std::get<0>(points[i]) - center_of_mass_); + + // save line + lines_.emplace_back(i, (i + 1) % points_.size()); + } + + update_points(); +} + +void physical_object::perform_symplectic_euler_step(const double dt) +{ + // integrate position/velocity + + const auto a = force_ / mass_; + const auto a_m = inverse_inertia_ * torque_; + + velocity_ += a * dt; + angular_velocity_ += a_m * dt; + + position_ += velocity_ * dt; + rotation_ = Rotation2D(rotation_.angle() + angular_velocity_ * dt); +} + +void physical_object::update(const double dt) +{ + switch (type_) + { + case object_type::fixed: return; // do not update fixed objects + case object_type::dynamic: accelerate(gravity); // apply gravity to dynamic objects only + default: break; + } + + perform_symplectic_euler_step(dt); + + update_points(); + + // reset + force_ = { 0.0, 0.0 }; + torque_ = 0.0; +} + +void physical_object::accelerate(const Vector2d& point, const Vector2d& acceleration) +{ + // integrate force + const auto force = acceleration * mass_; + + // integrate torque + const auto offset = point - center_of_mass_; + torque_ += cross2(offset, force); +} + +void physical_object::accelerate(const Vector2d& acceleration) +{ + // integrate force + force_ += acceleration * mass_; +} + +void physical_object::update_points() +{ + const auto rot = rotation_.toRotationMatrix(); + + auto inertia = 0.0; + for (auto i = 0u; i < points_.size(); ++i) + { + // transform offset according to + // current rotation and scale + const auto offset = rot * initial_offsets_[i] * scale_; + + points_[i] = + { + offset, + velocity_ + cross2(angular_velocity_, offset) + }; + + // save maximum offset + radius_ = std::max(radius_, offset.norm()); + + // update inertia + inertia += mass_ / points().size() * offset.squaredNorm(); + } + + inverse_inertia_ = 1.0 / inertia; + + center_of_mass_global_ = position_ + center_of_mass_; +} + + +void physical_object::position(const Vector2d& new_center) +{ + position_ = new_center; + update_points(); +} diff --git a/Final/physical_object.h b/Final/physical_object.h new file mode 100644 index 0000000..2ac1103 --- /dev/null +++ b/Final/physical_object.h @@ -0,0 +1,179 @@ +#ifndef PHYSICAL_OBJECT_H +#define PHYSICAL_OBJECT_H + +#define EIGEN_DONT_ALIGN_STATICALLY +#define EIGEN_DONT_VECTORIZE + +#include +#include +#include + +#define _USE_MATH_DEFINES +#include + +using Eigen::Vector2d; +using Eigen::Matrix2d; +using Rotation2D = Eigen::Rotation2D; + +// stores (offset, velocity) +using point_t = std::tuple; + +// stores point indices (start, end) +using line_t = std::tuple; + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Vector2d) +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(point_t) + +// ============================================================ +// utility functions +inline Vector2d normal(const Vector2d& a, const Vector2d& b) +{ + const auto dir = ((a + b) / 2.00).normalized(); + + const auto normal = Vector2d( + -(b - a).y(), + (b - a).x() + ).normalized(); + + return normal.dot(dir) < 0 + ? Vector2d(-normal.x(), -normal.y()) + : normal; +} + +inline double cross2(const Vector2d& a, const Vector2d& b) +{ + return a.x() * b.y() - b.x() * a.y(); +} + +inline Vector2d cross2(const Vector2d& v, const double s) +{ + return { s * v.y(), -s * v.x() }; +} + +inline Vector2d cross2(const double s, const Vector2d& v) +{ + return { -s * v.y(), s * v.x() }; +} + +// ============================================================ + +static const Vector2d gravity = { 0, -9.81 }; + +enum class object_type +{ + fixed, + dynamic, + kinematic // not implemented +}; + +/** + * \brief Main class for handling object movement + */ +class physical_object +{ +public: + explicit physical_object( + const Vector2d& position, + const std::vector>& points); + + void update(const double dt); + + void accelerate(const Vector2d& point, const Vector2d& acceleration); + void accelerate(const Vector2d& acceleration); + + void apply_impulse(const Vector2d& impulse, const Vector2d& offset) + { + velocity_ += inverse_mass() * impulse; + angular_velocity_ += inverse_inertia_ * cross2(offset, impulse); + } + + const Vector2d& position() const { return position_; } + const Rotation2D& rotation() const { return rotation_; } + const Vector2d& linear_velocity() const { return velocity_; } + const Vector2d& center_of_mass_local() const { return center_of_mass_; } + const Vector2d& center_of_mass_global() const { return center_of_mass_global_; } + double inverse_inertia() const { return inverse_inertia_; } + double bounding_radius() const { return radius_; } + const std::vector& points() const { return points_; } + const std::vector& lines() const { return lines_; } + void position(const Vector2d& new_center); + + double mass() const + { + return type_ == object_type::fixed + ? 0 + : mass_; + } + + double inverse_mass() const + { + return type_ == object_type::fixed + ? 0 + : 1.0 / mass_; + } + + /** + * \brief Scale object by moving points and updating mass + * \param scale + */ + void set_scale(const double scale) + { + scale_ = scale; + mass_ = initial_mass_ * scale; + + // updates offsets, inertia + // and bounding radius + update_points(); + } + + double get_scale() const { return scale_; } + + void set_type(const object_type type) { type_ = type; } + + object_type get_type() const { return type_; } + + void move(const Vector2d& v) + { + position_ += v; + } + + void rotate(const double a) + { + rotation_ = Rotation2D(rotation_.angle() + a); + update_points(); + } + + +private: + // save initial mass to allow scaling + double initial_mass_; + + double mass_; + double scale_; + double radius_; + + Vector2d center_of_mass_; + Vector2d center_of_mass_global_; + Vector2d position_; + Rotation2D rotation_; + + Vector2d velocity_; + Vector2d force_; + + double torque_; + double angular_velocity_; + double inverse_inertia_; + + std::vector initial_offsets_; + std::vector points_; + std::vector lines_; + + object_type type_ = object_type::fixed; + + void update_points(); + void perform_symplectic_euler_step(const double dt); +}; + + + +#endif // PHYSICAL_OBJECT_H diff --git a/Final/presentation/rigid_body_simulation.pdf b/Final/presentation/rigid_body_simulation.pdf new file mode 100644 index 0000000..f176a4e Binary files /dev/null and b/Final/presentation/rigid_body_simulation.pdf differ diff --git a/Final/presentation/rigid_body_simulation.pptx b/Final/presentation/rigid_body_simulation.pptx new file mode 100644 index 0000000..9e47744 Binary files /dev/null and b/Final/presentation/rigid_body_simulation.pptx differ diff --git a/Final/smoother.h b/Final/smoother.h new file mode 100644 index 0000000..ebedcf3 --- /dev/null +++ b/Final/smoother.h @@ -0,0 +1,27 @@ +#ifndef SMOOTHER_H +#define SMOOTHER_H + +#include +#include + +template +class smoother +{ +public: + void add(T v) + { + data.push_back(v); + if (data.size() > MAX) + data.pop_front(); + } + + T get() + { + return std::accumulate(data.begin(), data.end(), 0.0) / data.size(); + } + +private: + std::deque data; +}; + +#endif // SMOOTHER_H