├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── assets ├── README.md ├── objects │ ├── README.md │ ├── actuators │ │ ├── WheelMotor.cpp │ │ └── WheelMotor.h │ ├── bodies │ │ ├── SimpleBotBody.cpp │ │ └── SimpleBotBody.h │ ├── playgrounds │ │ ├── Dohyo.cpp │ │ ├── Dohyo.h │ │ ├── LineFollowerPath.cpp │ │ └── LineFollowerPath.h │ ├── robots │ │ ├── BaseBot.cpp │ │ ├── BaseBot.h │ │ ├── LineFollower.cpp │ │ ├── LineFollower.h │ │ ├── PhysicsBot.cpp │ │ ├── PhysicsBot.h │ │ ├── Sumobot.cpp │ │ └── Sumobot.h │ ├── sensors │ │ ├── LineDetectorObject.cpp │ │ ├── LineDetectorObject.h │ │ ├── RangeSensorObject.cpp │ │ └── RangeSensorObject.h │ └── shapes │ │ ├── CircleObject.cpp │ │ ├── CircleObject.h │ │ ├── LineObject.cpp │ │ ├── LineObject.h │ │ ├── QuadObject.cpp │ │ ├── QuadObject.h │ │ ├── RectObject.cpp │ │ └── RectObject.h └── textures │ ├── AnimationTestSpriteSheet.png │ ├── dohyo_scratched.png │ ├── line_follower_plated.png │ ├── scalebar10cm.png │ ├── scalebar1m.png │ ├── scalebar25cm.png │ ├── scalebar2m.png │ ├── scalebar4m.png │ ├── scalebar50cm.png │ ├── sumobot_body_circuited.png │ ├── sumobot_body_plated.png │ ├── sumobot_body_round_black.png │ ├── sumobot_body_round_red.png │ ├── wheel_sprite_left_green.png │ ├── wheel_sprite_left_orange.png │ ├── wheel_sprite_left_red.png │ ├── wheel_sprite_right_green.png │ ├── wheel_sprite_right_orange.png │ └── wheel_sprite_right_red.png ├── docs ├── Doxyfile ├── README.md └── images │ ├── line_follower_feature.png │ └── sumobot_feature.gif ├── external └── README.md ├── nsumoapp ├── CMakeLists.txt ├── NsumoApp.cpp ├── NsumoApp.h ├── controllers │ └── NsumoController │ │ ├── NsumoMicrocontroller.cpp │ │ ├── NsumoMicrocontroller.h │ │ ├── main.c │ │ ├── main_function.h │ │ ├── nsumo_simulated │ │ ├── assert_handler.c │ │ ├── ir_remote.c │ │ ├── qre1113.c │ │ ├── tb6612fng.c │ │ ├── trace.c │ │ └── vl53l0x.c │ │ └── voltage_lines.h ├── main.cpp └── scenes │ ├── NsumoScene.cpp │ └── NsumoScene.h ├── src ├── README.md ├── controllers │ └── components │ │ ├── CMicrocontroller.cpp │ │ ├── CMicrocontroller.h │ │ ├── ControllerComponent.h │ │ ├── KeyboardController.h │ │ ├── Microcontroller.cpp │ │ ├── Microcontroller.h │ │ ├── microcontroller_c_bindings.c │ │ ├── microcontroller_c_bindings.h │ │ └── microcontroller_c_setup.h ├── core │ ├── Application.cpp │ ├── Application.h │ ├── Component.h │ ├── Event.cpp │ └── Event.h ├── physics │ ├── Body2DUserData.h │ ├── ContactListener.cpp │ ├── ContactListener.h │ ├── PhysicsWorld.cpp │ ├── PhysicsWorld.h │ └── components │ │ ├── Body2D.cpp │ │ ├── Body2D.h │ │ ├── LineDetector.cpp │ │ ├── LineDetector.h │ │ ├── PhysicsComponent.h │ │ ├── RangeSensor.cpp │ │ └── RangeSensor.h ├── renderer │ ├── AssetsHelper.cpp │ ├── AssetsHelper.h │ ├── Camera.cpp │ ├── Camera.h │ ├── GLError.cpp │ ├── GLError.h │ ├── ImGuiMenu.cpp │ ├── ImGuiMenu.h │ ├── ImGuiOverlay.cpp │ ├── ImGuiOverlay.h │ ├── IndexBuffer.cpp │ ├── IndexBuffer.h │ ├── QuadCoords.h │ ├── Renderer.cpp │ ├── Renderer.h │ ├── Scalebar.cpp │ ├── Scalebar.h │ ├── Shader.cpp │ ├── Shader.h │ ├── SpriteAnimation.cpp │ ├── SpriteAnimation.h │ ├── TexCoords.h │ ├── Texture.cpp │ ├── Texture.h │ ├── VertexArray.cpp │ ├── VertexArray.h │ ├── VertexBuffer.cpp │ ├── VertexBuffer.h │ ├── VertexBufferLayout.h │ ├── components │ │ ├── CircleComponent.h │ │ ├── HollowCircleComponent.h │ │ ├── LineComponent.h │ │ ├── QuadComponent.cpp │ │ ├── QuadComponent.h │ │ ├── RectComponent.cpp │ │ ├── RectComponent.h │ │ └── RenderableComponent.h │ └── stb_image.cpp ├── scene │ ├── Scene.cpp │ ├── Scene.h │ ├── SceneMenu.cpp │ ├── SceneMenu.h │ ├── SceneObject.cpp │ └── SceneObject.h └── transforms │ └── components │ └── Transforms.h ├── testapp ├── Bots2DTestApp.cpp ├── Bots2DTestApp.h ├── CMakeLists.txt ├── README.md ├── controllers │ └── NsumoController │ │ ├── NsumoMicrocontroller.cpp │ │ ├── NsumoMicrocontroller.h │ │ ├── README.md │ │ ├── main.c │ │ ├── main_function.h │ │ ├── motor.c │ │ └── voltage_lines.h ├── main.cpp └── scenes │ ├── DrawTestScene.cpp │ ├── DrawTestScene.h │ ├── LineFollowerTestScene.cpp │ ├── LineFollowerTestScene.h │ ├── PhysicsBotTestScene.cpp │ ├── PhysicsBotTestScene.h │ ├── PhysicsTestScene.cpp │ ├── PhysicsTestScene.h │ ├── SpriteAnimationTestScene.cpp │ ├── SpriteAnimationTestScene.h │ ├── SumobotTestScene.cpp │ ├── SumobotTestScene.h │ ├── WheelMotorTestScene.cpp │ └── WheelMotorTestScene.h └── tools ├── README.md └── dc_motor_plot.py /.gitignore: -------------------------------------------------------------------------------- 1 | Makefile 2 | build/ 3 | tools/*svg 4 | docs/doxygen 5 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "external/Box2D"] 2 | path = external/Box2D 3 | url = https://github.com/erincatto/box2d 4 | [submodule "external/imgui"] 5 | path = external/imgui 6 | url = https://github.com/ocornut/imgui 7 | [submodule "external/glm"] 8 | path = external/glm 9 | url = https://github.com/g-truc/glm 10 | [submodule "external/glfw"] 11 | path = external/glfw 12 | url = https://github.com/glfw/glfw 13 | [submodule "external/stb"] 14 | path = external/stb 15 | url = https://github.com/nothings/stb 16 | [submodule "testapp/controllers/NsumoController/nsumo"] 17 | path = testapp/controllers/NsumoController/nsumo 18 | url = https://github.com/artfulbytes/nsumo_software.git 19 | [submodule "nsumoapp/controllers/NsumoController/nsumo"] 20 | path = nsumoapp/controllers/NsumoController/nsumo 21 | url = git@github.com:artfulbytes/nsumo_video.git 22 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(bots2d VERSION 1.0) 3 | 4 | # Get glad from inside GLFW 5 | set(GLAD_GL_SOURCE_FILES 6 | external/glfw/deps/glad_gl.c 7 | ) 8 | 9 | set(IMGUI_SOURCE_FILES 10 | external/imgui/imgui.cpp 11 | external/imgui/imgui_draw.cpp 12 | external/imgui/imgui_widgets.cpp 13 | external/imgui/backends/imgui_impl_opengl3.cpp 14 | external/imgui/backends/imgui_impl_glfw.cpp 15 | ) 16 | 17 | set (RENDERER_SOURCE_FILES 18 | src/renderer/Renderer.cpp 19 | src/renderer/Camera.cpp 20 | src/renderer/Scalebar.cpp 21 | src/renderer/GLError.cpp 22 | src/renderer/AssetsHelper.cpp 23 | src/renderer/VertexBuffer.cpp 24 | src/renderer/IndexBuffer.cpp 25 | src/renderer/VertexArray.cpp 26 | src/renderer/Shader.cpp 27 | src/renderer/ImGuiOverlay.cpp 28 | src/renderer/Texture.cpp 29 | src/renderer/components/RectComponent.cpp 30 | src/renderer/components/QuadComponent.cpp 31 | src/renderer/SpriteAnimation.cpp 32 | src/renderer/stb_image.cpp 33 | src/renderer/ImGuiMenu.cpp 34 | ) 35 | 36 | set (PHYSICS_SOURCE_FILES 37 | src/physics/PhysicsWorld.cpp 38 | src/physics/ContactListener.cpp 39 | src/physics/components/Body2D.cpp 40 | src/physics/components/RangeSensor.cpp 41 | src/physics/components/LineDetector.cpp 42 | ) 43 | 44 | set (CONTROLLER_SOURCE_FILES 45 | src/controllers/components/Microcontroller.cpp 46 | src/controllers/components/CMicrocontroller.cpp 47 | src/controllers/components/microcontroller_c_bindings.c 48 | ) 49 | 50 | set (BOTS2D_FILES 51 | src/core/Application.cpp 52 | src/core/Event.cpp 53 | src/scene/SceneObject.cpp 54 | src/scene/Scene.cpp 55 | src/scene/SceneMenu.cpp 56 | ${RENDERER_SOURCE_FILES} 57 | ${PHYSICS_SOURCE_FILES} 58 | ${CONTROLLER_SOURCE_FILES} 59 | ${GLAD_GL_SOURCE_FILES} 60 | ${IMGUI_SOURCE_FILES} 61 | ) 62 | 63 | add_library(bots2d STATIC 64 | ${BOTS2D_FILES} 65 | ) 66 | 67 | target_include_directories(bots2d PRIVATE src) 68 | target_include_directories(bots2d PRIVATE src/core) 69 | target_include_directories(bots2d PRIVATE src/renderer) 70 | target_include_directories(bots2d PRIVATE src/transforms) 71 | target_include_directories(bots2d PRIVATE src/physics) 72 | target_include_directories(bots2d PRIVATE src/controllers) 73 | target_include_directories(bots2d PRIVATE src/scene) 74 | target_include_directories(bots2d PRIVATE external/glfw/deps) 75 | target_include_directories(bots2d PRIVATE external/stb) 76 | target_include_directories(bots2d PRIVATE external/glm) 77 | target_include_directories(bots2d PRIVATE external/imgui) 78 | 79 | # Build box2d and glfw as static libs 80 | add_subdirectory(external/glfw) 81 | target_link_libraries(bots2d PRIVATE glfw) 82 | # Path to Box2D src to only build Box2D (e.g. not testbed) 83 | add_subdirectory(external/Box2D/src) 84 | target_link_libraries(bots2d PRIVATE box2d) 85 | 86 | # Make Dear ImGui use GLAD2 87 | add_definitions( -DIMGUI_IMPL_OPENGL_LOADER_GLAD2 ) 88 | 89 | set_target_properties(bots2d PROPERTIES 90 | CXX_STANDARD 17 91 | CXX_STANDARD_REQUIRED YES 92 | CXX_EXTENSIONS NO 93 | ) 94 | 95 | if(MSVC) 96 | target_compile_options(bots2d PRIVATE /W4 /WX) 97 | else() 98 | target_compile_options(bots2d PRIVATE -Wall -Wextra -pedantic -Werror) 99 | # Ignore compile flags to keep the external repo intact 100 | set_source_files_properties(src/renderer/stb_image.cpp PROPERTIES COMPILE_FLAGS "-Wno-sign-compare -Wno-unused-but-set-variable") 101 | endif() 102 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2021 Niklas Nilsson 2 | 3 | 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: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | 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. 8 | -------------------------------------------------------------------------------- /assets/README.md: -------------------------------------------------------------------------------- 1 | This folder contains reusable robotics assets (classes inheriting SceneObject) 2 | and rendering assets. This folder must be accessible by your application during 3 | runtime. 4 | -------------------------------------------------------------------------------- /assets/objects/README.md: -------------------------------------------------------------------------------- 1 | This folder contains reusable robotics assets (classes inheriting SceneObject) 2 | sorted by category. If you create new assets, add them to this folder. 3 | -------------------------------------------------------------------------------- /assets/objects/actuators/WheelMotor.h: -------------------------------------------------------------------------------- 1 | #ifndef WHEEL_MOTOR_H_ 2 | #define WHEEL_MOTOR_H_ 3 | 4 | #include "SceneObject.h" 5 | #include 6 | #include 7 | 8 | class PhysicsWorld; 9 | class SpriteAnimation; 10 | class Body2D; 11 | 12 | /** 13 | * A wheel with a built-in DC-motor. The DC-motor is modeled with basic DC-motor 14 | * equations computing torque/force from a voltage input. Tune voltageInConstant and angularSpeedConstant 15 | * for different acceleration and top speed. 16 | * 17 | * NOTE: Currently, top-view only! 18 | * Physics note: The model is simplified. As with a real DC motor, the force is larger when 19 | * a voltage is applied and the rotational speed is low. When the speed increases, the back 20 | * EMF increases, which makes the forward force lower. If we stop applying a voltage while the 21 | * motor and wheel is moving, the back EMF will slow the vehicle down. We mimic the motor loss 22 | * by applying coloumb friction in all direction. And to better mimic a real tire, we add an 23 | * additional sideway force negatively proportional to the sideway velocity (sidewayFrictionConstant). 24 | * This also determines the amount of sideway slipping. At the moment, there is no slipping 25 | * in the driving direction, that is, the grip is perfect no matter how large the 26 | * torque is. 27 | * 28 | */ 29 | class WheelMotor : public SceneObject 30 | { 31 | public: 32 | enum class Orientation { Left, Right }; 33 | enum class TextureType { None, Green, Orange, Red }; 34 | struct Specification { 35 | Specification(float width, float diameter, float wheelMass, TextureType textureType) : 36 | width(width), diameter(diameter), wheelMass(wheelMass), textureType(textureType) {} 37 | Specification(float voltageInConstant, float angularSpeedConstant, float maxVoltage, 38 | float frictionCoefficient, float sidewayFrictionConstant, float width, 39 | float diameter, float wheelMass, float loadedMass, TextureType textureType) : 40 | voltageInConstant(voltageInConstant), angularSpeedConstant(angularSpeedConstant), 41 | maxVoltage(maxVoltage), frictionCoefficient(frictionCoefficient), sidewayFrictionConstant(sidewayFrictionConstant), 42 | width(width), diameter(diameter), wheelMass(wheelMass), loadedMass(loadedMass), textureType(textureType) {} 43 | /** voltageInConstant = (Torque constant * Voltage constant) / Motor resistance. 44 | * Tweak this for lower/higher top speed */ 45 | float voltageInConstant = 0.00628f; 46 | /** angularSpeedConstant = Torque constant / Motor resistance. 47 | * Tweak this for slower/faster acceleration */ 48 | float angularSpeedConstant = 0.00178f; 49 | /** Max voltage that can be applied to the voltage line */ 50 | float maxVoltage = 6.0f; 51 | /** Friction (Coloumb) in ALL directions. 52 | * Not entirely realistic, because there is no friction force acting in the driving 53 | * direction. But it kind of introduces a "motor loss", which makes the vehicle 54 | * stop more realisticly, without it the robot lingers at low speed before making a 55 | * full stop. 56 | * */ 57 | float frictionCoefficient = 0.1f; 58 | /** Made-up constant to tweak the sideway friction, larger value 59 | * means more friction and less skidding */ 60 | float sidewayFrictionConstant = 100.0f; 61 | float width = 0.0f; 62 | float diameter = 0.0f; 63 | float wheelMass = 0.0f; 64 | /** The mass the attached body adds on the wheel */ 65 | float loadedMass = 0.0f; 66 | TextureType textureType = TextureType::None; 67 | }; 68 | 69 | WheelMotor(Scene *scene, const Specification &spec, Orientation orientation, 70 | const glm::vec2 &startPosition, float startRotation); 71 | ~WheelMotor(); 72 | 73 | void setVoltageIn(float voltage); 74 | void setDutyCycle(float dutyCycle); 75 | void setFrictionCoefficient(float frictionCoefficient); 76 | void setSidewayFrictionConstant(float sidewayFrictionConstant); 77 | float getFrictionCoefficient() const; 78 | float getSidewayFrictionConstant() const; 79 | void onFixedUpdate() override; 80 | float *getVoltageLine(); 81 | const Body2D *getBody() const { return m_body2D; } 82 | void setLoadedMass(float loadedMass); 83 | void setMass(float mass); 84 | float getMass() const; 85 | void setMaxVoltage(float maxVoltage); 86 | void setAngularSpeedConstant(float angularSpeedConstant); 87 | void setVoltageInConstant(float voltageInConstant); 88 | float getVoltageInConstant() const; 89 | float getMaxVoltage() const; 90 | float getAngularSpeedConstant() const; 91 | void enable(); 92 | void disable(); 93 | 94 | private: 95 | void setAnimation(); 96 | std::string getTextureName(Orientation orientation, TextureType textureType) const; 97 | void updateForce(); 98 | 99 | Specification m_spec; 100 | float m_voltageIn = 0.0f; 101 | std::unique_ptr m_animation; 102 | Body2D *m_body2D = nullptr; 103 | bool m_enabled = true; 104 | }; 105 | 106 | #endif /* WHEEL_MOTOR_H_ */ 107 | -------------------------------------------------------------------------------- /assets/objects/bodies/SimpleBotBody.cpp: -------------------------------------------------------------------------------- 1 | #include "bodies/SimpleBotBody.h" 2 | #include "PhysicsWorld.h" 3 | #include "components/Transforms.h" 4 | #include "components/Body2D.h" 5 | #include "components/RectComponent.h" 6 | #include "components/CircleComponent.h" 7 | #include "actuators/WheelMotor.h" 8 | #include "sensors/RangeSensorObject.h" 9 | #include "sensors/LineDetectorObject.h" 10 | 11 | #include 12 | 13 | SimpleBotBody::SimpleBotBody(Scene *scene, const Specification &spec, const glm::vec2 &startPosition, float startRotation) : 14 | SceneObject(scene) 15 | { 16 | assert(m_physicsWorld->getGravityType() == PhysicsWorld::Gravity::TopView); 17 | switch (spec.shape) { 18 | case SimpleBotBody::Shape::Rectangle: 19 | createRectangleBody(spec, startPosition, startRotation); 20 | break; 21 | case SimpleBotBody::Shape::Circle: 22 | createCircleBody(spec, startPosition, startRotation); 23 | break; 24 | } 25 | } 26 | 27 | SimpleBotBody::~SimpleBotBody() 28 | { 29 | } 30 | 31 | glm::vec2 SimpleBotBody::getPosition() const 32 | { 33 | return m_body2D->getPosition(); 34 | } 35 | 36 | float SimpleBotBody::getRotation() const 37 | { 38 | return m_body2D->getRotation(); 39 | } 40 | 41 | float SimpleBotBody::getForwardSpeed() const 42 | { 43 | return m_body2D->getForwardSpeed(); 44 | } 45 | 46 | float SimpleBotBody::getAngularSpeed() const 47 | { 48 | return m_body2D->getForwardSpeed(); 49 | } 50 | 51 | void SimpleBotBody::createRectangleBody(const Specification &spec, const glm::vec2 &startPosition, float startRotation) 52 | { 53 | m_transformComponent = std::make_unique(startPosition, glm::vec2{ spec.width, spec.length }, startRotation); 54 | const auto transform = static_cast(m_transformComponent.get()); 55 | 56 | switch(spec.textureType) { 57 | case SimpleBotBody::TextureType::SumobotPlated: 58 | m_renderableComponent = std::make_unique(transform, "sumobot_body_plated.png"); 59 | break; 60 | case SimpleBotBody::TextureType::SumobotCircuited: 61 | m_renderableComponent = std::make_unique(transform, "sumobot_body_circuited.png"); 62 | break; 63 | case SimpleBotBody::TextureType::SumobotRoundRed: 64 | case SimpleBotBody::TextureType::SumobotRoundBlack: 65 | assert(0); 66 | std::cout << "Can't use round texture for rectangle shape" << std::endl; 67 | break; 68 | case SimpleBotBody::TextureType::LineFollowerPlated: 69 | m_renderableComponent = std::make_unique(transform, "line_follower_plated.png"); 70 | break; 71 | case SimpleBotBody::TextureType::None: 72 | glm::vec4 color(0.0f, 0.0f, 1.0f, 1.0f); 73 | m_renderableComponent = std::make_unique(transform, color); 74 | break; 75 | } 76 | 77 | /* (The friction is added to the wheels, not the body) */ 78 | Body2D::Specification bodySpec(true, true, spec.mass, 0.0f, spec.angularDamping); 79 | m_physicsComponent = std::make_unique(*m_physicsWorld, transform, bodySpec); 80 | m_body2D = static_cast(m_physicsComponent.get()); 81 | } 82 | 83 | void SimpleBotBody::createCircleBody(const Specification &spec, const glm::vec2 &startPosition, float startRotation) 84 | { 85 | m_transformComponent = std::make_unique(startPosition, (spec.width / 2.0f), startRotation); 86 | const auto transform = static_cast(m_transformComponent.get()); 87 | 88 | switch(spec.textureType) { 89 | case SimpleBotBody::TextureType::SumobotPlated: 90 | case SimpleBotBody::TextureType::SumobotCircuited: 91 | case SimpleBotBody::TextureType::LineFollowerPlated: 92 | assert(0); 93 | std::cout << "Can't use rectangular texture for round shape" << std::endl; 94 | break; 95 | case SimpleBotBody::TextureType::SumobotRoundRed: 96 | m_renderableComponent = std::make_unique(transform, "sumobot_body_round_red.png"); 97 | break; 98 | case SimpleBotBody::TextureType::SumobotRoundBlack: 99 | m_renderableComponent = std::make_unique(transform, "sumobot_body_round_black.png"); 100 | break; 101 | case SimpleBotBody::TextureType::None: 102 | glm::vec4 color(0.0f, 0.0f, 1.0f, 1.0f); 103 | m_renderableComponent = std::make_unique(transform, color); 104 | break; 105 | } 106 | 107 | /* (The friction is added to the wheels, not the body) */ 108 | Body2D::Specification bodySpec(true, true, spec.mass, 0.0f, spec.angularDamping); 109 | m_physicsComponent = std::make_unique(*m_physicsWorld, transform, bodySpec); 110 | m_body2D = static_cast(m_physicsComponent.get()); 111 | } 112 | 113 | void SimpleBotBody::attachWheelMotor(const WheelMotor *wheelMotor, glm::vec2 relativePosition) 114 | { 115 | m_body2D->attachBodyWithRevoluteJoint(relativePosition, wheelMotor->getBody()); 116 | } 117 | 118 | void SimpleBotBody::attachSensor(const RangeSensorObject *rangeSensorObject, glm::vec2 relativePosition) 119 | { 120 | m_body2D->attachBodyWithWeldJoint(relativePosition, rangeSensorObject->getBody()); 121 | } 122 | 123 | void SimpleBotBody::attachSensor(const LineDetectorObject *lineDetectorObject, glm::vec2 relativePosition) 124 | { 125 | m_body2D->attachBodyWithWeldJoint(relativePosition, lineDetectorObject->getBody()); 126 | } 127 | 128 | void SimpleBotBody::onFixedUpdate() 129 | { 130 | } 131 | 132 | void SimpleBotBody::setMass(float mass) 133 | { 134 | m_body2D->setMass(mass); 135 | } 136 | 137 | float SimpleBotBody::getMass() const 138 | { 139 | return m_body2D->getMass(); 140 | } 141 | 142 | void SimpleBotBody::setAngularDamping(float damping) const 143 | { 144 | m_body2D->setAngularDamping(damping); 145 | } 146 | 147 | float SimpleBotBody::getAngularDamping() const 148 | { 149 | return m_body2D->getAngularDamping(); 150 | } 151 | -------------------------------------------------------------------------------- /assets/objects/bodies/SimpleBotBody.h: -------------------------------------------------------------------------------- 1 | #ifndef SIMPLE_BOT_BODY_H_ 2 | #define SIMPLE_BOT_BODY_H_ 3 | 4 | #include "SceneObject.h" 5 | #include 6 | 7 | class PhysicsWorld; 8 | class Body2D; 9 | class WheelMotor; 10 | class RangeSensorObject; 11 | class LineDetectorObject; 12 | 13 | /** 14 | * Simple robot body (cirular/rectangular) that should be used with other scene objects 15 | * to compose a full robot. Dimensions are adjustable, and motors and sensors can be attached to it. 16 | * 17 | * Note: It doesn't introduce any friction with the ground, instead that friction has 18 | * to be introduced by the wheels. But it does introduce rotational inertia (all 19 | * bodies in Box2D have inertia). Consequently, we mimic the real world better. 20 | * 21 | */ 22 | class SimpleBotBody : public SceneObject 23 | { 24 | public: 25 | enum class Shape { Rectangle, Circle }; 26 | enum class TextureType { None, SumobotPlated, SumobotCircuited, SumobotRoundRed, SumobotRoundBlack, 27 | LineFollowerPlated}; 28 | 29 | /* TODO: Make it possible to change center of mass */ 30 | struct Specification { 31 | Specification(float length, float width, float mass, 32 | Shape shape, TextureType textureType, 33 | float angularDamping) : 34 | length(length), width(width), mass(mass), 35 | shape(shape), textureType(textureType), 36 | angularDamping(angularDamping) {} 37 | float length; 38 | /** Stores the width if rectangle shape and radius if round shape. */ 39 | float width; 40 | float mass; 41 | Shape shape = Shape::Rectangle; 42 | TextureType textureType = TextureType::None; 43 | float angularDamping = 0.0f; 44 | }; 45 | 46 | SimpleBotBody(Scene *scene, const Specification &spec, const glm::vec2 &startPosition, float startRotation); 47 | ~SimpleBotBody(); 48 | glm::vec2 getPosition() const; 49 | float getRotation() const; 50 | float getForwardSpeed() const; 51 | float getAngularSpeed() const; 52 | void onFixedUpdate() override; 53 | void attachWheelMotor(const WheelMotor *wheelMotor, glm::vec2 relativePosition); 54 | void attachSensor(const RangeSensorObject *rangeSensorObject, glm::vec2 relativePosition); 55 | void attachSensor(const LineDetectorObject *lineDetectorObject, glm::vec2 relativePosition); 56 | void setMass(float mass); 57 | float getMass() const; 58 | void setAngularDamping(float damping) const; 59 | float getAngularDamping() const; 60 | 61 | private: 62 | void createRectangleBody(const Specification &spec, const glm::vec2 &startPosition, float startRotation); 63 | void createCircleBody(const Specification &spec, const glm::vec2 &startPosition, float startRotation); 64 | 65 | Body2D *m_body2D = nullptr; 66 | }; 67 | 68 | #endif /* SIMPLE_BOT_BODY_H_ */ 69 | -------------------------------------------------------------------------------- /assets/objects/playgrounds/Dohyo.cpp: -------------------------------------------------------------------------------- 1 | #include "playgrounds/Dohyo.h" 2 | #include "PhysicsWorld.h" 3 | #include "components/Transforms.h" 4 | #include "components/Body2D.h" 5 | #include "components/HollowCircleComponent.h" 6 | #include "components/RectComponent.h" 7 | #include "shapes/RectObject.h" 8 | 9 | Dohyo::Dohyo(Scene *scene, const Specification &spec, const glm::vec2 &position) : 10 | SceneObject(scene) 11 | { 12 | assert(m_physicsWorld->getGravityType() == PhysicsWorld::Gravity::TopView); 13 | m_transformComponent = std::make_unique(position, spec.innerRadius, spec.outerRadius); 14 | const auto transform = static_cast(m_transformComponent.get()); 15 | m_physicsComponent = std::make_unique(*m_physicsWorld, transform, Body2D::Specification{ false, false, 0.0f }); 16 | static_cast(m_physicsComponent.get())->setUserData(&m_userData); 17 | 18 | switch (spec.textureType) { 19 | case Dohyo::TextureType::Scratched: 20 | { 21 | m_quadObject = std::make_unique(scene, "dohyo_scratched.png", nullptr, nullptr, 22 | position, 23 | glm::vec2{ 2.0f * spec.outerRadius, 2.0f * spec.outerRadius }, 24 | 0.0f); 25 | break; 26 | } 27 | case Dohyo::TextureType::None: 28 | { 29 | m_renderableComponent = std::make_unique(transform, glm::vec4{ 0.0f, 0.0f, 0.0f, 1.0f }, 30 | glm::vec4{ 1.0f, 1.0f, 1.0f, 1.0f }); 31 | break; 32 | } 33 | } 34 | } 35 | 36 | Dohyo::~Dohyo() 37 | { 38 | } 39 | 40 | void Dohyo::onFixedUpdate() 41 | { 42 | } 43 | -------------------------------------------------------------------------------- /assets/objects/playgrounds/Dohyo.h: -------------------------------------------------------------------------------- 1 | #ifndef DOHYO_H_ 2 | #define DOHYO_H_ 3 | 4 | #include "SceneObject.h" 5 | #include "Body2DUserData.h" 6 | #include 7 | 8 | class PhysicsWorld; 9 | class RectObject; 10 | 11 | /** 12 | * A sumobot dohyo (circular arena) with a detectable border and adjustable dimensions. 13 | */ 14 | class Dohyo : public SceneObject 15 | { 16 | public: 17 | enum class TextureType { Scratched, None }; 18 | struct Specification { 19 | const float innerRadius; 20 | const float outerRadius; 21 | const TextureType textureType = TextureType::None; 22 | }; 23 | Dohyo(Scene *scene, const Specification &spec, const glm::vec2 &position); 24 | ~Dohyo(); 25 | void onFixedUpdate() override; 26 | 27 | private: 28 | Body2DUserData m_userData = { 0, 0, BodyId::Detectable }; 29 | std::unique_ptr m_quadObject; 30 | }; 31 | 32 | #endif /* DOHYO_H_ */ 33 | -------------------------------------------------------------------------------- /assets/objects/playgrounds/LineFollowerPath.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_FOLLOWER_PATH_H_ 2 | #define LINE_FOLLOWER_PATH_H_ 3 | 4 | #include "SceneObject.h" 5 | #include "Body2DUserData.h" 6 | #include 7 | #include 8 | #include "QuadCoords.h" 9 | 10 | class QuadObject; 11 | 12 | /** 13 | * Creates a line follower path from a set of points. The path is implemented as 14 | * an array of quads placed next to each other. 15 | * 16 | * Currently, it's limited to complete loops with right-angled 90-degrees 17 | * turns. 18 | */ 19 | class LineFollowerPath : public SceneObject 20 | { 21 | public: 22 | typedef std::vector PathPoints; 23 | enum class Blueprint { Simple, Tshaped, Mshaped }; 24 | static const PathPoints &getBlueprintPathPoints(LineFollowerPath::Blueprint blueprint); 25 | LineFollowerPath(Scene *scene, const glm::vec4 &lineColor, float lineWidth, const PathPoints &pathPoints); 26 | ~LineFollowerPath(); 27 | 28 | private: 29 | std::vector getRightAnglePathQuadCoords(const PathPoints &pathPoints, const float width); 30 | std::vector> m_pathQuads; 31 | }; 32 | 33 | #endif /* LINE_FOLLOWER_PATH_H_ */ 34 | -------------------------------------------------------------------------------- /assets/objects/robots/BaseBot.h: -------------------------------------------------------------------------------- 1 | #ifndef BASE_BOT_H_ 2 | #define BASE_BOT_H_ 3 | 4 | #include "SceneObject.h" 5 | #include "bodies/SimpleBotBody.h" 6 | #include "actuators/WheelMotor.h" 7 | #include "sensors/RangeSensorObject.h" 8 | #include "sensors/LineDetectorObject.h" 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | class WheelMotor; 16 | 17 | /** 18 | * Base class representing a general robot that can attach wheels and sensors. 19 | */ 20 | class BaseBot : public SceneObject 21 | { 22 | public: 23 | /** Enum for easier indexing of wheel motors */ 24 | enum class WheelMotorIndex { Left, Right, FrontLeft, FrontRight, BackLeft, BackRight }; 25 | /** Enum for easier indexing of range sensors */ 26 | enum class RangeSensorIndex { Left, Right, FrontLeft, Front, FrontRight }; 27 | /** Enum for easier indexing of line detector */ 28 | enum class LineDetectorIndex { Left, Right, SecondFrontLeft, FrontLeft, FrontRight, SecondFrontRight, BackLeft, BackRight }; 29 | /** Specifies the bot as a list of adjustable variables */ 30 | struct Specification { 31 | float bodyWidth; 32 | float bodyLength; 33 | float bodyMass; 34 | float wheelWidth; 35 | float wheelDiameter; 36 | float wheelMass; 37 | float wheelFrictionCoefficient; 38 | float wheelSidewayFrictionConstant; 39 | /** Motor constants (See WheelMotor.h for more info) */ 40 | float motorVoltageInConstant; 41 | float motorAngularSpeedConstant; 42 | float motorMaxVoltage; 43 | float angularDamping; 44 | SimpleBotBody::Shape bodyShape; 45 | SimpleBotBody::TextureType bodyTexture; 46 | WheelMotor::TextureType wheelTexture; 47 | std::vector> wheelMotorTuples; 48 | std::vector> rangeSensorTuples; 49 | std::vector> lineDetectorTuples; 50 | }; 51 | static void sanityCheckSpec(const Specification &spec); 52 | 53 | BaseBot(Scene *scene, const Specification &spec, 54 | const glm::vec2 &startPosition, const float startRotation); 55 | virtual ~BaseBot(); 56 | virtual void onFixedUpdate() override; 57 | float *getVoltageLine(WheelMotorIndex wheelMotorIndex) const; 58 | float *getVoltageLine(RangeSensorIndex rangeSensorIndex) const; 59 | float *getVoltageLine(LineDetectorIndex lineDetectorIndex) const; 60 | virtual void setDebug(bool enabled); 61 | void setWheelFrictionCoefficient(float frictionCoefficient); 62 | void setWheelSidewayFrictionConstant(float sidewayFrictionConstant); 63 | float getWheelFrictionCoefficient() const; 64 | float getWheelSidewayFrictionConstant() const; 65 | void setWheelMass(float mass); 66 | void setBodyMass(float mass); 67 | float getBodyMass() const; 68 | float getWheelMass() const; 69 | void setMotorMaxVoltage(float maxVoltage); 70 | void setMotorAngularSpeedConstant(float angularSpeedConstant); 71 | void setMotorVoltageInConstant(float voltageInConstant); 72 | glm::vec2 getAbsoluteWheelPosition(WheelMotorIndex wheelMotorIndex) const; 73 | float getMotorVoltageInConstant() const; 74 | float getMotorMaxVoltage() const; 75 | float getMotorAngularSpeedConstant() const; 76 | float getForwardSpeed() const; 77 | void setTimeMovingCallback(std::function onTimeMoving) const; 78 | void setTopSpeedCallback(std::function onTopSpeedChanged); 79 | void setForwardSpeedCallback(std::function onForwardSpeedChanged); 80 | void setForwardAccelerationCallback(std::function onForwardAccelerationChanged); 81 | void setTimeToTopSpeedCallback(std::function onTimeToTopSpeedChanged); 82 | void setTopSpeedAccelerationCallback(std::function onTopSpeedAccelerationChanged); 83 | void setTopAccelerationCallback(std::function onTopAccelerationChanged); 84 | void setTimeMovingCallback(std::function onTimeMovingChanged); 85 | void enableMotor(BaseBot::WheelMotorIndex wheelMotorIndex); 86 | void disableMotor(BaseBot::WheelMotorIndex wheelMotorIndex); 87 | float getAngularDamping() const; 88 | void setAngularDamping(float angularDamping); 89 | 90 | private: 91 | void createBody(const Specification &spec, const glm::vec2 &startPosition, float startRotation); 92 | void createWheelMotors(const Specification &spec); 93 | void createSensors(const Specification &spec); 94 | 95 | std::unique_ptr m_body; 96 | std::unordered_map> m_wheelMotors; 97 | std::unordered_map> m_rangeSensors; 98 | std::unordered_map> m_lineDetectors; 99 | bool m_debugEnabled = false; 100 | std::function m_onForwardSpeedChanged = nullptr; 101 | std::function m_onTopSpeedChanged = nullptr; 102 | std::function m_onForwardAccelerationChanged = nullptr; 103 | std::function m_onTimeToTopSpeedChanged = nullptr; 104 | std::function m_onTopSpeedAccelerationChanged = nullptr; 105 | std::function m_onTopAccelerationChanged = nullptr; 106 | std::function m_onTimeMovingChanged = nullptr; 107 | unsigned int m_lastFwdStandStillTime = 0; 108 | unsigned int m_lastRotStandStillTime = 0; 109 | float m_recordedTopSpeed = 0.0f; 110 | float m_recordedTopAcceleration = 0.0f; 111 | float m_timeToReachTopSpeed = 0.0f; 112 | float m_topSpeedAcceleration = 0.0f; 113 | unsigned int m_lastCallbackTime = 0; 114 | float m_lastForwardSpeed = 0.0f; 115 | bool m_wasAtTopSpeed = false; 116 | bool m_wasAtStandStill = false; 117 | bool m_wasAtRotStandStill = false; 118 | }; 119 | 120 | #endif /* BASE_BOT_H_ */ 121 | -------------------------------------------------------------------------------- /assets/objects/robots/LineFollower.cpp: -------------------------------------------------------------------------------- 1 | #include "robots/LineFollower.h" 2 | 3 | #include 4 | 5 | namespace { 6 | const std::unordered_map lineFollowerBlueprints 7 | ({ 8 | { LineFollower::Blueprint::FourFrontSensors, 9 | { 10 | /* TODO: Make the center of mass more realistic */ 11 | 0.07f, 0.094f, /* Body width, length */ 12 | 0.22f, /* Body mass */ 13 | 0.015f, 0.03f, /* Wheel width, diameter */ 14 | 0.02f, /* Wheel mass */ 15 | 0.1f, /* Coloumb friction coefficient */ 16 | 100.0f, /* Wheel sideway friction constant */ 17 | 0.00628f, /* Motor voltage in constant */ 18 | 0.00178f, /* Angular speed constant */ 19 | 3.0f, /* Motor max voltage */ 20 | 0.0f, /* Angular damping */ 21 | SimpleBotBody::Shape::Rectangle, 22 | SimpleBotBody::TextureType::LineFollowerPlated, 23 | WheelMotor::TextureType::Orange, 24 | { 25 | { LineFollower::WheelMotorIndex::Left, {-(0.07f + 0.015f) / 2, -0.027f } }, 26 | { LineFollower::WheelMotorIndex::Right, { (0.07f + 0.015f) / 2, -0.027f } }, 27 | }, 28 | { 29 | /* No range sensors */ 30 | }, 31 | { 32 | { LineFollower::LineDetectorIndex::SecondFrontLeft, {-0.010f, 0.045f}, {0.0f} }, 33 | { LineFollower::LineDetectorIndex::FrontLeft, {-0.005f, 0.045f}, {0.0f} }, 34 | { LineFollower::LineDetectorIndex::FrontRight, { 0.005f, 0.045f}, {0.0f} }, 35 | { LineFollower::LineDetectorIndex::SecondFrontRight, { 0.010f, 0.045f}, {0.0f} }, 36 | } 37 | } 38 | }, 39 | }); 40 | } 41 | 42 | const LineFollower::Specification &LineFollower::getBlueprintSpec(LineFollower::Blueprint blueprint) 43 | { 44 | auto blueprintItr = lineFollowerBlueprints.find(blueprint); 45 | if (blueprintItr == lineFollowerBlueprints.end()) { 46 | std::cout << "Blueprint not found!" << std::endl; 47 | assert(0); 48 | } 49 | return blueprintItr->second; 50 | } 51 | 52 | LineFollower::LineFollower(Scene *scene, const LineFollower::Specification &spec, 53 | const glm::vec2 &startPosition, const float startRotation) : 54 | BaseBot(scene, spec, startPosition, startRotation) 55 | { 56 | } 57 | 58 | LineFollower::~LineFollower() 59 | { 60 | } 61 | -------------------------------------------------------------------------------- /assets/objects/robots/LineFollower.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_FOLLOWER_H_ 2 | #define LINE_FOLLOWER_H_ 3 | 4 | #include "robots/BaseBot.h" 5 | 6 | #include 7 | 8 | /** 9 | * LineFollower - a robot that attempts to follow a path laid out as a black 10 | * (or sometimes white) stripe. 11 | * 12 | * The class provides pre-defined LineFollower specs as Blueprints. The class is flexible, and you 13 | * can create a new Blueprint/Specification if you want to better resemble your own LineFollower. 14 | * See BaseBot for a list of specification parameters. 15 | */ 16 | class LineFollower : public BaseBot 17 | { 18 | public: 19 | enum class Blueprint { FourFrontSensors }; 20 | static const Specification &getBlueprintSpec(Blueprint blueprint); 21 | 22 | LineFollower(Scene *scene, const Specification &spec, 23 | const glm::vec2 &startPosition, const float startRotation); 24 | ~LineFollower(); 25 | }; 26 | 27 | #endif /* LINE_FOLLOWER_H_ */ 28 | -------------------------------------------------------------------------------- /assets/objects/robots/PhysicsBot.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS_BOT_H_ 2 | #define PHYSICS_BOT_H_ 3 | 4 | #include "SceneObject.h" 5 | 6 | #include 7 | #include 8 | 9 | class RectObject; 10 | class Body2D; 11 | 12 | /* Test bot to play with the physics dynamics more directly */ 13 | class PhysicsBot : public SceneObject 14 | { 15 | public: 16 | PhysicsBot(Scene *scene, const glm::vec2 &size, const glm::vec2 &startPosition, const float startRotation); 17 | virtual ~PhysicsBot(); 18 | virtual void onFixedUpdate() override; 19 | void setForceFrontLeft(float force); 20 | void setForceBackLeft(float force); 21 | void setForceFrontRight(float force); 22 | void setForceBackRight(float force); 23 | 24 | void setVoltageFrontLeft(float voltage); 25 | void setVoltageBackLeft(float voltage); 26 | void setVoltageFrontRight(float voltage); 27 | void setVoltageBackRight(float voltage); 28 | 29 | void setSidewayFrictionConstant(float sidewayFrictionConstant); 30 | float getSidewayFrictionConstant() const { return m_sidewayFrictionConstant; } 31 | 32 | void setFrictionCoefficient(float frictionCoefficient); 33 | float getFrictionCoefficient() const { return m_frictionCoefficient; } 34 | 35 | void setTotalMass(float mass); 36 | float getTotalMass() const { return (m_wheelMass * m_wheelCount) + m_bodyMass; } 37 | 38 | float getForwardSpeed() const; 39 | void setForwardSpeedCallback(std::function onForwardSpeedChanged); 40 | void setForwardAccelerationCallback(std::function onForwardAccelerationChanged); 41 | void setTopSpeedCallback(std::function onTopSpeedChanged); 42 | void setAccelerationToTopSpeedCallback(std::function onAccelerationToTopSpeedChanged); 43 | void resetRecordedValues(); 44 | 45 | private: 46 | float getLoadedWheelMass() const { return (m_bodyMass / m_wheelCount) + m_wheelMass; } 47 | void setMotorForce(Body2D *body2D, float voltageIn); 48 | std::unique_ptr m_frontLeftWheel; 49 | std::unique_ptr m_backLeftWheel; 50 | std::unique_ptr m_frontRightWheel; 51 | std::unique_ptr m_backRightWheel; 52 | 53 | float m_sidewayFrictionConstant = 100.0f; 54 | float m_frictionCoefficient = 0.1f; 55 | float m_frontLeftVoltage = 0; 56 | float m_backLeftVoltage = 0; 57 | float m_frontRightVoltage = 0; 58 | float m_backRightVoltage = 0; 59 | float m_bodyMass = 0.4f; 60 | float m_wheelMass = 0.025f; 61 | const unsigned int m_wheelCount = 4; 62 | Body2D *m_body = nullptr; 63 | 64 | std::function m_onForwardSpeedChanged = nullptr; 65 | std::function m_onForwardAccelerationChanged = nullptr; 66 | std::function m_onTopSpeedChanged = nullptr; 67 | std::function m_onAccelerationToTopSpeedChanged = nullptr; 68 | unsigned int m_lastCallbackTime = 0; 69 | float m_lastForwardSpeed = 0.0f; 70 | float m_recordedTopSpeed = 0.0f; 71 | unsigned int m_lastStandStillTime = 0; 72 | float m_bestAccelerationToCurrentTopSpeed = 0.0f; 73 | float m_bestAccelerationRecordedAtSpeed = 0.0f; 74 | float m_timeToTopSpeed = 0.0f; 75 | }; 76 | 77 | #endif /* PHYSICS_BOT_H_ */ 78 | -------------------------------------------------------------------------------- /assets/objects/robots/Sumobot.h: -------------------------------------------------------------------------------- 1 | #ifndef SUMOBOT_H_ 2 | #define SUMOBOT_H_ 3 | 4 | #include "robots/BaseBot.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | /** 11 | * Sumobot: a robot that attempts to push another robot out of a circle (Dohyo). 12 | * Sumobot competition is a popular type of robotics competition. 13 | * 14 | * The class provides several pre-defined sumobots as Blueprints. The class is flexible, and you 15 | * can create a new Blueprint/Specification if you want to resemble your own Sumobot better. 16 | * See BaseBot for a list of specification parameters. 17 | */ 18 | class Sumobot : public BaseBot 19 | { 20 | public: 21 | enum class Blueprint { Nsumo, FourWheel, TwoWheelRectangle, TwoWheelRoundBlack, TwoWheelRoundRed }; 22 | static const Specification &getBlueprintSpec(Blueprint blueprint); 23 | 24 | Sumobot(Scene *scene, const Specification &spec, 25 | const glm::vec2 &startPosition, const float startRotation); 26 | ~Sumobot(); 27 | }; 28 | 29 | #endif /* SUMOBOT_H_ */ 30 | -------------------------------------------------------------------------------- /assets/objects/sensors/LineDetectorObject.cpp: -------------------------------------------------------------------------------- 1 | #include "sensors/LineDetectorObject.h" 2 | #include "components/LineDetector.h" 3 | #include "components/Transforms.h" 4 | #include "components/CircleComponent.h" 5 | #include "components/Body2D.h" 6 | 7 | #include 8 | 9 | LineDetectorObject::LineDetectorObject(Scene *scene, const Specification &spec, bool debugDrawEnabled, 10 | const glm::vec2 &startPosition) : 11 | SceneObject(scene) 12 | { 13 | CircleTransform *circleTransform; 14 | m_transformComponent = std::make_unique(); 15 | circleTransform = static_cast(m_transformComponent.get()); 16 | glm::vec4 color(1.0f, 0.5f, 0.0f, 1.0f); 17 | m_renderableComponent = std::make_unique(circleTransform, color); 18 | m_renderableComponent->setEnabled(debugDrawEnabled); 19 | m_physicsComponent = std::make_unique(*m_physicsWorld, circleTransform, startPosition, spec.updateRateSeconds); 20 | m_lineDetector = static_cast(m_physicsComponent.get()); 21 | } 22 | 23 | LineDetectorObject::~LineDetectorObject() 24 | { 25 | } 26 | 27 | Body2D *LineDetectorObject::getBody() const 28 | { 29 | return m_lineDetector->getBody(); 30 | } 31 | 32 | float *LineDetectorObject::getVoltageLine() const 33 | { 34 | return m_lineDetector->getVoltageLine(); 35 | } 36 | 37 | void LineDetectorObject::setDebugDraw(bool enabled) 38 | { 39 | m_renderableComponent->setEnabled(enabled); 40 | } 41 | 42 | void LineDetectorObject::onFixedUpdate() 43 | { 44 | } 45 | -------------------------------------------------------------------------------- /assets/objects/sensors/LineDetectorObject.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_DETECTOR_OBJECT_H_ 2 | #define LINE_DETECTOR_OBJECT_H_ 3 | 4 | #include "SceneObject.h" 5 | #include "PhysicsWorld.h" 6 | 7 | class LineDetector; 8 | class Body2D; 9 | 10 | /** 11 | * Line detector with debug draw to notice detection graphically. 12 | */ 13 | class LineDetectorObject : public SceneObject 14 | { 15 | public: 16 | struct Specification { 17 | const float updateRateSeconds = 0.0f; 18 | }; 19 | LineDetectorObject(Scene *scene, const Specification &spec, bool debugDraw, 20 | const glm::vec2 &startPosition = { 0.0f, 0.0f }); 21 | ~LineDetectorObject(); 22 | Body2D *getBody() const; 23 | void onFixedUpdate() override; 24 | float *getVoltageLine() const; 25 | void setDebugDraw(bool enable); 26 | 27 | private: 28 | LineDetector *m_lineDetector = nullptr; 29 | bool m_debugDrawEnabled = false; 30 | }; 31 | 32 | #endif /* LINE_DETECTOR_OBJECT_H_ */ 33 | -------------------------------------------------------------------------------- /assets/objects/sensors/RangeSensorObject.cpp: -------------------------------------------------------------------------------- 1 | #include "sensors/RangeSensorObject.h" 2 | #include "components/RangeSensor.h" 3 | #include "components/Transforms.h" 4 | #include "components/LineComponent.h" 5 | 6 | #include 7 | 8 | RangeSensorObject::RangeSensorObject(Scene *scene, const Specification &spec, bool debugDrawEnabled, 9 | const glm::vec2 startPosition) : 10 | SceneObject(scene) 11 | { 12 | LineTransform *transform = nullptr; 13 | m_transformComponent = std::make_unique(); 14 | transform = static_cast(m_transformComponent.get()); 15 | glm::vec4 color(0.0f, 0.5f, 0.0f, 1.0f); 16 | m_renderableComponent = std::make_unique(transform, color); 17 | m_renderableComponent->setEnabled(debugDrawEnabled); 18 | m_physicsComponent = std::make_unique(*m_physicsWorld, transform, 19 | startPosition, spec.relativeAngle, 20 | spec.minDistance, spec.maxDistance, 21 | spec.updateRateSeconds); 22 | m_rangeSensor = static_cast(m_physicsComponent.get()); 23 | } 24 | 25 | RangeSensorObject::~RangeSensorObject() 26 | { 27 | } 28 | 29 | void RangeSensorObject::setDebugDraw(bool enabled) 30 | { 31 | m_renderableComponent->setEnabled(enabled); 32 | } 33 | 34 | Body2D *RangeSensorObject::getBody() const 35 | { 36 | return m_rangeSensor->getBody(); 37 | } 38 | 39 | float *RangeSensorObject::getVoltageLine() const 40 | { 41 | return m_rangeSensor->getVoltageLine(); 42 | } 43 | 44 | void RangeSensorObject::onFixedUpdate() 45 | { 46 | } 47 | -------------------------------------------------------------------------------- /assets/objects/sensors/RangeSensorObject.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGE_SENSOR_OBJECT_H_ 2 | #define RANGE_SENSOR_OBJECT_H_ 3 | 4 | #include "SceneObject.h" 5 | #include "PhysicsWorld.h" 6 | 7 | class RangeSensor; 8 | class Body2D; 9 | 10 | /** 11 | * Range sensor with debug draw to notice detection graphically. 12 | */ 13 | class RangeSensorObject : public SceneObject 14 | { 15 | public: 16 | struct Specification { 17 | const float relativeAngle = 0.0f; 18 | const float minDistance = 0.0f; 19 | const float maxDistance = 0.0f; 20 | const float updateRateSeconds = 0.0f; 21 | }; 22 | RangeSensorObject(Scene *scene, const Specification &spec, bool debugDrawEnabled, 23 | const glm::vec2 startPosition = { 0.0f, 0.0f }); 24 | ~RangeSensorObject(); 25 | void setDebugDraw(bool enabled); 26 | Body2D *getBody() const; 27 | void onFixedUpdate() override; 28 | float *getVoltageLine() const; 29 | 30 | private: 31 | RangeSensor *m_rangeSensor = nullptr; 32 | }; 33 | 34 | #endif /* RANGE_SENSOR_OBJECT_H_ */ 35 | -------------------------------------------------------------------------------- /assets/objects/shapes/CircleObject.cpp: -------------------------------------------------------------------------------- 1 | #include "shapes/CircleObject.h" 2 | #include "components/Transforms.h" 3 | #include "components/CircleComponent.h" 4 | #include "components/HollowCircleComponent.h" 5 | 6 | CircleObject::CircleObject(Scene *scene, const glm::vec4 &color, 7 | const Body2D::Specification *spec, 8 | const glm::vec2 &position, float radius) : 9 | SceneObject(scene) 10 | { 11 | m_transformComponent = std::make_unique(position, radius, 0.0f); 12 | auto transform = static_cast(m_transformComponent.get()); 13 | m_renderableComponent = std::make_unique(transform, color); 14 | if (spec != nullptr) { 15 | m_physicsComponent = std::make_unique(*m_physicsWorld, transform, *spec); 16 | } 17 | } 18 | 19 | CircleObject::CircleObject(Scene *scene, const glm::vec4 &fillColor, const glm::vec4 &borderColor, 20 | const glm::vec2 &position, float innerRadius, float outerRadius) : 21 | SceneObject(scene) 22 | { 23 | m_transformComponent = std::make_unique(position, innerRadius, outerRadius); 24 | auto transform = static_cast(m_transformComponent.get()); 25 | m_renderableComponent = std::make_unique(transform, fillColor, borderColor); 26 | } 27 | 28 | CircleObject::~CircleObject() 29 | { 30 | } 31 | -------------------------------------------------------------------------------- /assets/objects/shapes/CircleObject.h: -------------------------------------------------------------------------------- 1 | #ifndef CIRCLE_OBJECT_ 2 | #define CIRCLE_OBJECT_ 3 | 4 | #include "SceneObject.h" 5 | #include "components/Body2D.h" 6 | #include 7 | 8 | /** 9 | * Simulates a simple circular object with physics (optional). It's needed because 10 | * a scene can't directly update components (CircleComponent). 11 | */ 12 | class CircleObject : public SceneObject 13 | { 14 | public: 15 | CircleObject(Scene *scene, const glm::vec4 &color, const Body2D::Specification *spec, 16 | const glm::vec2 &position, float radius); 17 | CircleObject(Scene *scene, const glm::vec4 &fillColor, const glm::vec4 &borderColor, 18 | const glm::vec2 &position, float innerRadius, float outerRadius); 19 | ~CircleObject(); 20 | }; 21 | 22 | #endif /* CIRCLE_OBJECT_H_ */ 23 | -------------------------------------------------------------------------------- /assets/objects/shapes/LineObject.cpp: -------------------------------------------------------------------------------- 1 | #include "shapes/LineObject.h" 2 | #include "components/Transforms.h" 3 | #include "components/LineComponent.h" 4 | #include "PhysicsWorld.h" 5 | 6 | LineObject::LineObject(Scene *scene, const glm::vec4 &color, const glm::vec2 &start, const glm::vec2 &end, float width) : 7 | SceneObject(scene) 8 | { 9 | m_transformComponent = std::make_unique(start, end, width); 10 | const auto transform = static_cast(m_transformComponent.get()); 11 | m_renderableComponent = std::make_unique(transform, color); 12 | } 13 | -------------------------------------------------------------------------------- /assets/objects/shapes/LineObject.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_OBJECT_H_ 2 | #define LINE_OBJECT_H_ 3 | 4 | #include "SceneObject.h" 5 | #include 6 | 7 | /** 8 | * Draws a line in a scene. It's needed because a scene can't directly 9 | * update components (LineComponent). 10 | */ 11 | class LineObject : public SceneObject 12 | { 13 | public: 14 | LineObject(Scene *scene, const glm::vec4 &color, const glm::vec2 &start, const glm::vec2 &end, float width); 15 | }; 16 | 17 | #endif /* LINE_OBJECT_H_ */ 18 | -------------------------------------------------------------------------------- /assets/objects/shapes/QuadObject.cpp: -------------------------------------------------------------------------------- 1 | #include "shapes/QuadObject.h" 2 | #include "components/QuadComponent.h" 3 | #include "components/Transforms.h" 4 | 5 | QuadObject::QuadObject(Scene *scene, const QuadCoords &quadCoords, const glm::vec4 &color, 6 | const Body2D::Specification *spec, bool detectable) : 7 | SceneObject(scene) 8 | { 9 | m_transformComponent = std::make_unique(quadCoords); 10 | auto transform = static_cast(m_transformComponent.get()); 11 | if (spec != nullptr) { 12 | m_physicsComponent = std::make_unique(*m_physicsWorld, transform, *spec); 13 | m_body2D = static_cast(m_physicsComponent.get()); 14 | if (detectable) { 15 | m_body2D->setUserData(&m_userData); 16 | } 17 | } 18 | 19 | m_renderableComponent = std::make_unique(transform, color); 20 | } 21 | -------------------------------------------------------------------------------- /assets/objects/shapes/QuadObject.h: -------------------------------------------------------------------------------- 1 | #ifndef QUAD_OBJECT_H_ 2 | #define QUAD_OBJECT_H 3 | 4 | #include "SceneObject.h" 5 | #include "Body2DUserData.h" 6 | #include "components/Body2D.h" 7 | #include 8 | 9 | struct QuadCoords; 10 | /** 11 | * Renders a detectable quad. It's needed because a Scene can't directly update components 12 | * (QuadComponent and Body2D). 13 | * 14 | * NOTE: Coordinates must form a convex polygon. 15 | */ 16 | class QuadObject : public SceneObject 17 | { 18 | public: 19 | QuadObject(Scene *scene, const QuadCoords &quadCoords, const glm::vec4 &color, 20 | const Body2D::Specification *spec, bool detectable); 21 | private: 22 | Body2DUserData m_userData = { 0, 0, BodyId::Detectable }; 23 | Body2D *m_body2D; 24 | }; 25 | 26 | #endif /* QUAD_OBJECT_H_ */ 27 | -------------------------------------------------------------------------------- /assets/objects/shapes/RectObject.cpp: -------------------------------------------------------------------------------- 1 | #include "shapes/RectObject.h" 2 | #include "components/Transforms.h" 3 | #include "components/RectComponent.h" 4 | 5 | RectObject::RectObject(Scene *scene, const glm::vec4 &color, 6 | const Body2D::Specification *spec, 7 | const glm::vec2 &position, const glm::vec2 &size, float rotation) : 8 | SceneObject(scene) 9 | { 10 | m_transformComponent = std::make_unique(position, size, rotation); 11 | const auto transform = static_cast(m_transformComponent.get()); 12 | m_renderableComponent = std::make_unique(transform, color); 13 | if (spec != nullptr) { 14 | m_physicsComponent = std::make_unique(*m_physicsWorld, transform, *spec); 15 | } 16 | } 17 | 18 | RectObject::RectObject(Scene *scene, const std::string& textureFilepath, const SpriteAnimation::Params *animationParams, 19 | const Body2D::Specification *spec, 20 | const glm::vec2 &position, const glm::vec2 &size, float rotation) : 21 | SceneObject(scene) 22 | { 23 | m_transformComponent = std::make_unique(position, size, rotation); 24 | const auto transform = static_cast(m_transformComponent.get()); 25 | if (animationParams != nullptr) { 26 | m_animation = std::make_unique(*animationParams); 27 | } 28 | m_renderableComponent = std::make_unique(transform, textureFilepath, m_animation.get()); 29 | if (spec != nullptr) { 30 | m_physicsComponent = std::make_unique(*m_physicsWorld, transform, *spec); 31 | } 32 | } 33 | 34 | Body2D *RectObject::getBody() const 35 | { 36 | return static_cast(m_physicsComponent.get()); 37 | } 38 | -------------------------------------------------------------------------------- /assets/objects/shapes/RectObject.h: -------------------------------------------------------------------------------- 1 | #ifndef RECT_OBJECT_H_ 2 | #define RECT_OBJECT_H_ 3 | 4 | #include "SceneObject.h" 5 | #include "components/Body2D.h" 6 | #include "SpriteAnimation.h" 7 | 8 | #include 9 | #include 10 | 11 | /** 12 | * A renderable rectangle with physics (optional). It's needed because a scene can't directly update 13 | * components (RectComponent and Body2D). 14 | */ 15 | class RectObject : public SceneObject 16 | { 17 | public: 18 | RectObject(Scene *scene, const glm::vec4 &color, 19 | const Body2D::Specification *spec, 20 | const glm::vec2 &position, const glm::vec2 &size, float rotation); 21 | RectObject(Scene *scene, const std::string& textureFilepath, const SpriteAnimation::Params *animationParams, 22 | const Body2D::Specification *spec, 23 | const glm::vec2 &position, const glm::vec2 &size, float rotation); 24 | Body2D *getBody() const; 25 | 26 | private: 27 | std::unique_ptr m_animation; 28 | }; 29 | 30 | #endif /* RECT_OBJECT_H_ */ 31 | -------------------------------------------------------------------------------- /assets/textures/AnimationTestSpriteSheet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/AnimationTestSpriteSheet.png -------------------------------------------------------------------------------- /assets/textures/dohyo_scratched.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/dohyo_scratched.png -------------------------------------------------------------------------------- /assets/textures/line_follower_plated.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/line_follower_plated.png -------------------------------------------------------------------------------- /assets/textures/scalebar10cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/scalebar10cm.png -------------------------------------------------------------------------------- /assets/textures/scalebar1m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/scalebar1m.png -------------------------------------------------------------------------------- /assets/textures/scalebar25cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/scalebar25cm.png -------------------------------------------------------------------------------- /assets/textures/scalebar2m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/scalebar2m.png -------------------------------------------------------------------------------- /assets/textures/scalebar4m.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/scalebar4m.png -------------------------------------------------------------------------------- /assets/textures/scalebar50cm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/scalebar50cm.png -------------------------------------------------------------------------------- /assets/textures/sumobot_body_circuited.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/sumobot_body_circuited.png -------------------------------------------------------------------------------- /assets/textures/sumobot_body_plated.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/sumobot_body_plated.png -------------------------------------------------------------------------------- /assets/textures/sumobot_body_round_black.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/sumobot_body_round_black.png -------------------------------------------------------------------------------- /assets/textures/sumobot_body_round_red.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/sumobot_body_round_red.png -------------------------------------------------------------------------------- /assets/textures/wheel_sprite_left_green.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/wheel_sprite_left_green.png -------------------------------------------------------------------------------- /assets/textures/wheel_sprite_left_orange.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/wheel_sprite_left_orange.png -------------------------------------------------------------------------------- /assets/textures/wheel_sprite_left_red.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/wheel_sprite_left_red.png -------------------------------------------------------------------------------- /assets/textures/wheel_sprite_right_green.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/wheel_sprite_right_green.png -------------------------------------------------------------------------------- /assets/textures/wheel_sprite_right_orange.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/wheel_sprite_right_orange.png -------------------------------------------------------------------------------- /assets/textures/wheel_sprite_right_red.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/assets/textures/wheel_sprite_right_red.png -------------------------------------------------------------------------------- /docs/README.md: -------------------------------------------------------------------------------- 1 | This folder contains files related to documentation. 2 | 3 | Note, the doxygen output should be generated to this directory 4 | so that github pages can access it. But this should be done 5 | on the gh-pages branch (NOT master). 6 | -------------------------------------------------------------------------------- /docs/images/line_follower_feature.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/docs/images/line_follower_feature.png -------------------------------------------------------------------------------- /docs/images/sumobot_feature.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/artfulbytes/bots2d/8518fb6afdb00b06db40ff7d91c3c632e01b26a9/docs/images/sumobot_feature.gif -------------------------------------------------------------------------------- /external/README.md: -------------------------------------------------------------------------------- 1 | This folder contains all external dependencies. They are included as unmodified git submodules. 2 | -------------------------------------------------------------------------------- /nsumoapp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(nsumoapp VERSION 1.0) 3 | 4 | # Modify to where the bots2d folder is stored 5 | set (BOTS2D_RELATIVE_PATH ../) 6 | 7 | set (NSUMOAPP_ASSETS 8 | ${BOTS2D_RELATIVE_PATH}/assets/objects/shapes/RectObject.cpp 9 | ${BOTS2D_RELATIVE_PATH}/assets/objects/shapes/QuadObject.cpp 10 | ${BOTS2D_RELATIVE_PATH}/assets/objects/shapes/CircleObject.cpp 11 | ${BOTS2D_RELATIVE_PATH}/assets/objects/shapes/LineObject.cpp 12 | ${BOTS2D_RELATIVE_PATH}/assets/objects/actuators/WheelMotor.cpp 13 | ${BOTS2D_RELATIVE_PATH}/assets/objects/bodies/SimpleBotBody.cpp 14 | ${BOTS2D_RELATIVE_PATH}/assets/objects/robots/BaseBot.cpp 15 | ${BOTS2D_RELATIVE_PATH}/assets/objects/robots/Sumobot.cpp 16 | ${BOTS2D_RELATIVE_PATH}/assets/objects/sensors/RangeSensorObject.cpp 17 | ${BOTS2D_RELATIVE_PATH}/assets/objects/sensors/LineDetectorObject.cpp 18 | ${BOTS2D_RELATIVE_PATH}/assets/objects/playgrounds/Dohyo.cpp 19 | ) 20 | 21 | set (NSUMOAPP_SCENES 22 | scenes/NsumoScene.cpp 23 | ) 24 | 25 | set (NSUMOAPP_CONTROLLERS 26 | controllers/NsumoController/main.c 27 | controllers/NsumoController/NsumoMicrocontroller.cpp 28 | controllers/NsumoController/nsumo/src/common/enum_to_string.c 29 | controllers/NsumoController/nsumo/src/common/ring_buffer.c 30 | controllers/NsumoController/nsumo/src/app/line.c 31 | controllers/NsumoController/nsumo/src/app/drive.c 32 | controllers/NsumoController/nsumo/src/app/enemy.c 33 | controllers/NsumoController/nsumo/src/app/timer.c 34 | controllers/NsumoController/nsumo/src/app/input_history.c 35 | controllers/NsumoController/nsumo/src/app/state_machine.c 36 | controllers/NsumoController/nsumo/src/app/state_retreat.c 37 | controllers/NsumoController/nsumo/src/app/state_attack.c 38 | controllers/NsumoController/nsumo/src/app/state_search.c 39 | controllers/NsumoController/nsumo/src/app/state_wait.c 40 | controllers/NsumoController/nsumo/src/app/state_manual.c 41 | controllers/NsumoController/nsumo_simulated/tb6612fng.c 42 | controllers/NsumoController/nsumo_simulated/qre1113.c 43 | controllers/NsumoController/nsumo_simulated/vl53l0x.c 44 | controllers/NsumoController/nsumo_simulated/trace.c 45 | controllers/NsumoController/nsumo_simulated/assert_handler.c 46 | controllers/NsumoController/nsumo_simulated/ir_remote.c 47 | ) 48 | 49 | set (NSUMOAPP_FILES 50 | ${NSUMOAPP_ASSETS} 51 | ${NSUMOAPP_SCENES} 52 | ${NSUMOAPP_CONTROLLERS} 53 | NsumoApp.cpp 54 | main.cpp 55 | ) 56 | 57 | add_executable(nsumoapp 58 | ${NSUMOAPP_FILES} 59 | ) 60 | 61 | target_include_directories(nsumoapp PRIVATE scenes) 62 | target_include_directories(nsumoapp PRIVATE controllers/NsumoController/nsumo/src) 63 | target_include_directories(nsumoapp PRIVATE controllers) 64 | target_include_directories(nsumoapp PRIVATE ${BOTS2D_RELATIVE_PATH}/assets/objects) 65 | target_include_directories(nsumoapp PRIVATE ${BOTS2D_RELATIVE_PATH}/src/core) 66 | target_include_directories(nsumoapp PRIVATE ${BOTS2D_RELATIVE_PATH}/src/renderer) 67 | target_include_directories(nsumoapp PRIVATE ${BOTS2D_RELATIVE_PATH}/src/transforms) 68 | target_include_directories(nsumoapp PRIVATE ${BOTS2D_RELATIVE_PATH}/src/scene) 69 | target_include_directories(nsumoapp PRIVATE ${BOTS2D_RELATIVE_PATH}/src/physics) 70 | target_include_directories(nsumoapp PRIVATE ${BOTS2D_RELATIVE_PATH}/src/controllers) 71 | target_include_directories(nsumoapp PRIVATE ${BOTS2D_RELATIVE_PATH}/src/controllers/components) 72 | target_include_directories(nsumoapp PRIVATE ${BOTS2D_RELATIVE_PATH}/external/glm) 73 | 74 | # Include bots2d and build it as a static lib 75 | add_subdirectory(${BOTS2D_RELATIVE_PATH} build) 76 | target_link_libraries(nsumoapp PRIVATE bots2d) 77 | 78 | target_compile_definitions(nsumoapp PRIVATE BUILD_TEST) 79 | 80 | set_target_properties(nsumoapp PROPERTIES 81 | CXX_STANDARD 17 82 | CXX_STANDARD_REQUIRED YES 83 | CXX_EXTENSIONS NO 84 | ) 85 | 86 | if(MSVC) 87 | target_compile_options(nsumoapp PRIVATE /W4 /WX) 88 | else() 89 | target_compile_options(nsumoapp PRIVATE -Wall -Wextra -pedantic -Werror) 90 | endif() 91 | -------------------------------------------------------------------------------- /nsumoapp/NsumoApp.cpp: -------------------------------------------------------------------------------- 1 | #include "NsumoApp.h" 2 | #include "SceneMenu.h" 3 | #include "NsumoScene.h" 4 | 5 | NsumoApp::NsumoApp() : 6 | Application("nsumoapp") 7 | { 8 | m_sceneMenu->registerScene("Nsumo"); 9 | m_sceneMenu->setCurrentScene("Nsumo"); 10 | } 11 | 12 | NsumoApp::~NsumoApp() 13 | { 14 | } 15 | 16 | void NsumoApp::onKeyEvent(const Event::Key &keyEvent) 17 | { 18 | (void)keyEvent; 19 | } 20 | 21 | void NsumoApp::onFixedUpdate() 22 | { 23 | } 24 | -------------------------------------------------------------------------------- /nsumoapp/NsumoApp.h: -------------------------------------------------------------------------------- 1 | #ifndef NSUMO_APP_H 2 | #define NSUMO_APP_H 3 | 4 | #include "Application.h" 5 | 6 | class NsumoApp : public Application 7 | { 8 | public: 9 | NsumoApp(); 10 | ~NsumoApp(); 11 | void onKeyEvent(const Event::Key &keyEvent) override; 12 | void onFixedUpdate() override; 13 | }; 14 | 15 | #endif // NSUMO_APP_H 16 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/NsumoMicrocontroller.cpp: -------------------------------------------------------------------------------- 1 | #include "NsumoController/NsumoMicrocontroller.h" 2 | 3 | extern "C" { 4 | #include "NsumoController/main_function.h" 5 | } 6 | 7 | NsumoMicrocontroller::NsumoMicrocontroller(Microcontroller::VoltageLines &voltageLines) : 8 | CMicrocontroller(voltageLines, _main) 9 | { 10 | } 11 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/NsumoMicrocontroller.h: -------------------------------------------------------------------------------- 1 | #ifndef NSUMO_MICROCONTROLLER_H 2 | #define NSUMO_MICROCONTROLLER_H 3 | 4 | #include "components/CMicrocontroller.h" 5 | 6 | class NsumoMicrocontroller : public CMicrocontroller 7 | { 8 | public: 9 | NsumoMicrocontroller(Microcontroller::VoltageLines &voltageLines); 10 | ~NsumoMicrocontroller() {} 11 | }; 12 | 13 | #endif // NSUMO_MICROCONTROLLER_H 14 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/main.c: -------------------------------------------------------------------------------- 1 | #include "NsumoController/main_function.h" 2 | #include "app/state_machine.h" 3 | 4 | void _main() { 5 | state_machine_run(); 6 | } 7 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/main_function.h: -------------------------------------------------------------------------------- 1 | #ifndef MAIN_FUNCTION_H 2 | #define MAIN_FUNCTION_H 3 | 4 | void _main(); 5 | 6 | #endif 7 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/nsumo_simulated/assert_handler.c: -------------------------------------------------------------------------------- 1 | #include "common/assert_handler.h" 2 | #include "common/defines.h" 3 | #include "common/trace.h" 4 | 5 | void assert_handler(uint16_t program_counter) 6 | { 7 | UNUSED(program_counter); 8 | TRACE("Assert"); 9 | while(1); 10 | } 11 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/nsumo_simulated/ir_remote.c: -------------------------------------------------------------------------------- 1 | #include "drivers/ir_remote.h" 2 | 3 | ir_cmd_e ir_remote_get_cmd(void) 4 | { 5 | return IR_CMD_NONE; 6 | } 7 | 8 | void ir_remote_init(void) 9 | { 10 | } 11 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/nsumo_simulated/qre1113.c: -------------------------------------------------------------------------------- 1 | #include "drivers/qre1113.h" 2 | #include "NsumoController/voltage_lines.h" 3 | #include "microcontroller_c_bindings.h" 4 | 5 | #define LINE_SENSOR_VOLTAGE_THRESHOLD (700u) 6 | #define LINE_NOT_DETECTED LINE_SENSOR_VOLTAGE_THRESHOLD 7 | #define LINE_DETECTED (0u) 8 | 9 | void qre1113_get_voltages(struct qre1113_voltages *voltages) 10 | { 11 | voltages->front_left = get_voltage(VOLTAGE_FRONT_LEFT_LINE_DETECTOR) ? LINE_DETECTED : LINE_NOT_DETECTED; 12 | voltages->front_right = get_voltage(VOLTAGE_FRONT_RIGHT_LINE_DETECTOR) ? LINE_DETECTED : LINE_NOT_DETECTED; 13 | voltages->back_left = get_voltage(VOLTAGE_BACK_LEFT_LINE_DETECTOR) ? LINE_DETECTED : LINE_NOT_DETECTED; 14 | voltages->back_right = get_voltage(VOLTAGE_BACK_RIGHT_LINE_DETECTOR) ? LINE_DETECTED : LINE_NOT_DETECTED; 15 | } 16 | 17 | void qre1113_init(void) 18 | { 19 | } 20 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/nsumo_simulated/tb6612fng.c: -------------------------------------------------------------------------------- 1 | #include "drivers/tb6612fng.h" 2 | #include "microcontroller_c_bindings.h" 3 | #include "NsumoController/voltage_lines.h" 4 | #include 5 | 6 | #define VOLTAGE_PER_BATTERY_FULL_CHARGE (4.2f) 7 | #define VOLTAGE_BATTERIES_FULL_CHARGE (2 * VOLTAGE_PER_BATTERY_FULL_CHARGE) 8 | #define MAX_VOLTAGE_ALLOWED (6.0f) 9 | #define LOSS_FACTOR (0.938f) 10 | #define SCALE_FACTOR (MAX_VOLTAGE_ALLOWED / VOLTAGE_BATTERIES_FULL_CHARGE) 11 | #define MAX_VOLTAGE (SCALE_FACTOR * LOSS_FACTOR * VOLTAGE_BATTERIES_FULL_CHARGE) 12 | 13 | static uint8_t left_duty_cycle = 0; 14 | static uint8_t right_duty_cycle = 0; 15 | static tb6612fng_mode_e left_mode; 16 | static tb6612fng_mode_e right_mode; 17 | 18 | static float duty_cycle_to_voltage(uint8_t duty_cycle) 19 | { 20 | return ((float)duty_cycle / 100.0f) * MAX_VOLTAGE; 21 | } 22 | 23 | void tb6612fng_set_mode(tb6612fng_e tb, tb6612fng_mode_e mode) 24 | { 25 | switch(mode) 26 | { 27 | case TB6612FNG_MODE_STOP: 28 | switch (tb) { 29 | case TB6612FNG_LEFT: 30 | set_voltage(VOLTAGE_FRONT_LEFT_MOTOR, 0); 31 | set_voltage(VOLTAGE_BACK_LEFT_MOTOR, 0); 32 | break; 33 | case TB6612FNG_RIGHT: 34 | set_voltage(VOLTAGE_FRONT_RIGHT_MOTOR, 0); 35 | set_voltage(VOLTAGE_BACK_RIGHT_MOTOR, 0); 36 | break; 37 | } 38 | break; 39 | case TB6612FNG_MODE_FORWARD: 40 | switch (tb) { 41 | case TB6612FNG_LEFT: 42 | set_voltage(VOLTAGE_FRONT_LEFT_MOTOR, duty_cycle_to_voltage(left_duty_cycle)); 43 | set_voltage(VOLTAGE_BACK_LEFT_MOTOR, duty_cycle_to_voltage(left_duty_cycle)); 44 | break; 45 | case TB6612FNG_RIGHT: 46 | set_voltage(VOLTAGE_FRONT_RIGHT_MOTOR, duty_cycle_to_voltage(right_duty_cycle)); 47 | set_voltage(VOLTAGE_BACK_RIGHT_MOTOR, duty_cycle_to_voltage(right_duty_cycle)); 48 | break; 49 | } 50 | break; 51 | case TB6612FNG_MODE_REVERSE: 52 | switch (tb) { 53 | case TB6612FNG_LEFT: 54 | set_voltage(VOLTAGE_FRONT_LEFT_MOTOR, -duty_cycle_to_voltage(left_duty_cycle)); 55 | set_voltage(VOLTAGE_BACK_LEFT_MOTOR, -duty_cycle_to_voltage(left_duty_cycle)); 56 | break; 57 | case TB6612FNG_RIGHT: 58 | set_voltage(VOLTAGE_FRONT_RIGHT_MOTOR, -duty_cycle_to_voltage(right_duty_cycle)); 59 | set_voltage(VOLTAGE_BACK_RIGHT_MOTOR, -duty_cycle_to_voltage(right_duty_cycle)); 60 | break; 61 | } 62 | break; 63 | } 64 | switch (tb) { 65 | case TB6612FNG_LEFT: 66 | left_mode = mode; 67 | break; 68 | case TB6612FNG_RIGHT: 69 | right_mode = mode; 70 | break; 71 | } 72 | } 73 | 74 | void tb6612fng_set_pwm(tb6612fng_e tb, uint8_t duty_cycle) 75 | { 76 | assert(duty_cycle <= 100); 77 | switch (tb) 78 | { 79 | case TB6612FNG_LEFT: 80 | left_duty_cycle = duty_cycle; 81 | tb6612fng_set_mode(tb, left_mode); 82 | break; 83 | case TB6612FNG_RIGHT: 84 | right_duty_cycle = duty_cycle; 85 | tb6612fng_set_mode(tb, right_mode); 86 | break; 87 | } 88 | } 89 | 90 | void tb6612fng_init() 91 | { 92 | } 93 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/nsumo_simulated/trace.c: -------------------------------------------------------------------------------- 1 | #include "common/trace.h" 2 | #include 3 | #include 4 | 5 | void trace(const char *format, ...) 6 | { 7 | va_list args; 8 | va_start(args, format); 9 | vprintf(format, args); 10 | va_end(args); 11 | } 12 | 13 | void trace_init() 14 | { 15 | } 16 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/nsumo_simulated/vl53l0x.c: -------------------------------------------------------------------------------- 1 | #include "drivers/vl53l0x.h" 2 | #include "microcontroller_c_bindings.h" 3 | #include "NsumoController/voltage_lines.h" 4 | 5 | // Should match max range of the sumobot spec 6 | #define VOLTAGE_TO_RANGE (800u) 7 | 8 | vl53l0x_result_e vl53l0x_read_range_multiple(vl53l0x_ranges_t ranges, bool *fresh_values) 9 | { 10 | ranges[VL53L0X_IDX_LEFT]= get_voltage(VOLTAGE_LEFT_RANGE_SENSOR) * VOLTAGE_TO_RANGE; 11 | ranges[VL53L0X_IDX_FRONT_LEFT] = get_voltage(VOLTAGE_FRONT_LEFT_RANGE_SENSOR) * VOLTAGE_TO_RANGE; 12 | ranges[VL53L0X_IDX_FRONT] = get_voltage(VOLTAGE_FRONT_RANGE_SENSOR) * VOLTAGE_TO_RANGE; 13 | ranges[VL53L0X_IDX_FRONT_RIGHT] = get_voltage(VOLTAGE_FRONT_RIGHT_RANGE_SENSOR) * VOLTAGE_TO_RANGE; 14 | ranges[VL53L0X_IDX_RIGHT] = get_voltage(VOLTAGE_RIGHT_RANGE_SENSOR) * VOLTAGE_TO_RANGE; 15 | *fresh_values = true; 16 | return VL53L0X_RESULT_OK; 17 | } 18 | 19 | vl53l0x_result_e vl53l0x_init(void) 20 | { 21 | return VL53L0X_RESULT_OK; 22 | } 23 | -------------------------------------------------------------------------------- /nsumoapp/controllers/NsumoController/voltage_lines.h: -------------------------------------------------------------------------------- 1 | #ifndef VOTLAGE_LINES_H 2 | #define VOLTAGE_LINES_H 3 | 4 | // Must match with how lines are assigned in the C++ microcontroller class 5 | typedef enum voltage_idx { 6 | VOLTAGE_FRONT_LEFT_MOTOR = 0, 7 | VOLTAGE_BACK_LEFT_MOTOR = 1, 8 | VOLTAGE_FRONT_RIGHT_MOTOR = 2, 9 | VOLTAGE_BACK_RIGHT_MOTOR = 3, 10 | VOLTAGE_FRONT_LEFT_LINE_DETECTOR = 4, 11 | VOLTAGE_BACK_LEFT_LINE_DETECTOR = 5, 12 | VOLTAGE_FRONT_RIGHT_LINE_DETECTOR = 6, 13 | VOLTAGE_BACK_RIGHT_LINE_DETECTOR = 7, 14 | VOLTAGE_LEFT_RANGE_SENSOR = 8, 15 | VOLTAGE_FRONT_LEFT_RANGE_SENSOR = 9, 16 | VOLTAGE_FRONT_RANGE_SENSOR = 10, 17 | VOLTAGE_FRONT_RIGHT_RANGE_SENSOR = 11, 18 | VOLTAGE_RIGHT_RANGE_SENSOR = 12, 19 | } voltage_idx_e; 20 | 21 | #endif // VOLTAGE_LINES_H 22 | -------------------------------------------------------------------------------- /nsumoapp/main.cpp: -------------------------------------------------------------------------------- 1 | #include "NsumoApp.h" 2 | 3 | int main() 4 | { 5 | NsumoApp app; 6 | app.run(); 7 | } 8 | -------------------------------------------------------------------------------- /nsumoapp/scenes/NsumoScene.h: -------------------------------------------------------------------------------- 1 | #ifndef NSUMO_SCENE_H 2 | #define NSUMO_SCENE_H 3 | 4 | #include "Scene.h" 5 | 6 | class Dohyo; 7 | class Sumobot; 8 | class KeyboardController; 9 | class NsumoMicrocontroller; 10 | 11 | class NsumoScene : public Scene 12 | { 13 | public: 14 | NsumoScene(); 15 | ~NsumoScene(); 16 | virtual void onFixedUpdate() override; 17 | private: 18 | std::unique_ptr m_dohyo; 19 | std::unique_ptr m_fourWheelBotOpponent; 20 | std::unique_ptr m_keyboardController; 21 | std::unique_ptr m_fourWheelBot; 22 | std::unique_ptr m_microcontroller; 23 | }; 24 | 25 | #endif // NSUMO_SCENE_H 26 | -------------------------------------------------------------------------------- /src/README.md: -------------------------------------------------------------------------------- 1 | This folder contains the framework headers and implementation files. 2 | -------------------------------------------------------------------------------- /src/controllers/components/CMicrocontroller.cpp: -------------------------------------------------------------------------------- 1 | #include "components/CMicrocontroller.h" 2 | 3 | extern "C" { 4 | #include "microcontroller_c_setup.h" 5 | } 6 | 7 | CMicrocontroller::CMicrocontroller(Microcontroller::VoltageLines &voltageLines, main_function mainFcn) : 8 | Microcontroller(voltageLines), 9 | m_mainFcn(mainFcn) 10 | { 11 | set_userdata(this); 12 | } 13 | 14 | CMicrocontroller::CMicrocontroller(Microcontroller::VoltageLines &voltageLines, main_function_userdata mainFcnUserdata) : 15 | Microcontroller(voltageLines), 16 | m_mainFcnUserdata(mainFcnUserdata) 17 | { 18 | } 19 | 20 | void CMicrocontroller::main() 21 | { 22 | if (m_mainFcn) { 23 | m_mainFcn(); 24 | } else { 25 | m_mainFcnUserdata(this); 26 | } 27 | } 28 | 29 | CMicrocontroller::~CMicrocontroller() 30 | { 31 | } 32 | -------------------------------------------------------------------------------- /src/controllers/components/CMicrocontroller.h: -------------------------------------------------------------------------------- 1 | #ifndef C_MICROCONTROLLER_H_ 2 | #define C_MICROCONTROLLER_H_ 3 | 4 | #include "Microcontroller.h" 5 | 6 | typedef void (*main_function)(void); 7 | typedef void (*main_function_userdata)(void *); 8 | /** 9 | * Base class for providing C-bindings to the controller code. 10 | * Useful if the code that runs on your target is pure C-code. 11 | * 12 | * This must be inherited, and the inheriting class needs to provide a 13 | * C-function main (wrapped inside extern "C", which should act as the 14 | * main function of the C-code. 15 | * 16 | * NOTE: If you want to simulate several C microcontrollers at the same 17 | * time, you can't define your "state" variables outside your functions, because 18 | * they will overwrite each other. You must define them inside your main function. 19 | */ 20 | class CMicrocontroller : public Microcontroller 21 | { 22 | public: 23 | /** 24 | * To pass a main function without userdata argument. ONLY one such microcontroller 25 | * can run at a time! 26 | */ 27 | CMicrocontroller(Microcontroller::VoltageLines &voltageLines, main_function mainFcn); 28 | /** 29 | * To pass a main function with userdata argument. Use this if you simulate multiple 30 | * C microcontrollers at a time. 31 | */ 32 | CMicrocontroller(Microcontroller::VoltageLines &voltageLines, main_function_userdata mainFcn); 33 | virtual ~CMicrocontroller() = 0; 34 | 35 | private: 36 | void main() override final; 37 | main_function m_mainFcn = nullptr; 38 | main_function_userdata m_mainFcnUserdata = nullptr; 39 | }; 40 | 41 | #endif /* C_MICROCONTROLLER_H_ */ 42 | -------------------------------------------------------------------------------- /src/controllers/components/ControllerComponent.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLER_COMPONENT_H_ 2 | #define CONTROLLER_COMPONENT_H_ 3 | 4 | #include "Component.h" 5 | #include "Event.h" 6 | 7 | /** 8 | * Base class for components that implement control behaviour based on 9 | * key events or pure logic. 10 | */ 11 | class ControllerComponent : public Component 12 | { 13 | public: 14 | virtual void onKeyEvent(const Event::Key &keyEvent) = 0; 15 | virtual void onFixedUpdate(float stepTime) = 0; 16 | virtual void resume() {}; 17 | virtual ~ControllerComponent() {} 18 | }; 19 | 20 | #endif /* CONTROLLER_COMPONENT_H_ */ 21 | -------------------------------------------------------------------------------- /src/controllers/components/KeyboardController.h: -------------------------------------------------------------------------------- 1 | #ifndef KEYBOARD_CONTROLLER_H_ 2 | #define KEYBOARD_CONTROLLER_H_ 3 | 4 | #include "ControllerComponent.h" 5 | 6 | /** 7 | * Base class for classes that controls a SceneObject based on keyboard input. 8 | */ 9 | class KeyboardController : public ControllerComponent 10 | { 11 | public: 12 | virtual void onKeyEvent(const Event::Key &keyEvent) = 0; 13 | virtual void onFixedUpdate(float stepTime) { (void)stepTime; }; 14 | }; 15 | 16 | #endif /* KEYBOARD_CONTROLLER_H_ */ 17 | -------------------------------------------------------------------------------- /src/controllers/components/Microcontroller.cpp: -------------------------------------------------------------------------------- 1 | #include "components/Microcontroller.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | Microcontroller::Microcontroller(Microcontroller::VoltageLines &voltageLines) : 9 | m_simulatorVoltageLines(voltageLines) 10 | { 11 | } 12 | 13 | Microcontroller::~Microcontroller() 14 | { 15 | m_running = false; 16 | m_sleepSteps = 0; 17 | /* Make sure we signal in case the thread is blocking */ 18 | m_conditionWake.notify_one(); 19 | if (m_thread.joinable()) { 20 | m_thread.join(); 21 | } else { 22 | std::cout << "Microcontroller not started?" << std::endl; 23 | } 24 | } 25 | 26 | void Microcontroller::start() 27 | { 28 | m_microcontrollerStarted = true; 29 | /* We need a separate function for starting the microcontroller because 30 | * we can't create/start inside the constructor when the object is not yet 31 | * fully created. Also, make sure we don't start it before the physics 32 | * has started. */ 33 | if (m_physicsStarted) { 34 | m_thread = std::thread(&Microcontroller::microcontrollerThreadFn, this); 35 | } 36 | } 37 | 38 | void Microcontroller::transferVoltageLevels() 39 | { 40 | m_voltageLinesMutex.lock(); 41 | for (int i = 0; i < Microcontroller::VoltageLine::Idx::Count; i++) { 42 | if (m_simulatorVoltageLines[i].level == nullptr) { 43 | /* Skip unused lines */ 44 | continue; 45 | } 46 | if (m_simulatorVoltageLines[i].type == Microcontroller::VoltageLine::Type::Output) { 47 | *m_simulatorVoltageLines[i].level = m_microcontrollerVoltageLineLevels[i]; 48 | } else { 49 | m_microcontrollerVoltageLineLevels[i] = *m_simulatorVoltageLines[i].level; 50 | } 51 | } 52 | m_voltageLinesMutex.unlock(); 53 | } 54 | 55 | /* Called by the simulator update loop, keep it short to avoid affecting 56 | * the frame rate. */ 57 | void Microcontroller::onFixedUpdate(float stepTime) 58 | { 59 | m_physicsStarted = true; 60 | 61 | /* Check if controller code has requested to sleep for X physics steps, 62 | * and if it has, count the steps and wake it up afterwards */ 63 | std::unique_lock uniqueLock(m_mutexSteps); 64 | m_currentStepTime = stepTime; 65 | if (m_sleepSteps > 0) { 66 | m_sleepSteps--; 67 | if (m_sleepSteps == 0) { 68 | m_conditionWake.notify_one(); 69 | } 70 | } 71 | m_stepsTaken++; 72 | uniqueLock.unlock(); 73 | 74 | 75 | transferVoltageLevels(); 76 | if (!m_thread.joinable() && m_microcontrollerStarted) { 77 | start(); 78 | } 79 | } 80 | 81 | void Microcontroller::microcontrollerThreadFn() 82 | { 83 | /* To make the main function behave more like a real main function, we allow 84 | * it to contain an endless loop. But a thread with an endless loop can't be 85 | * terminated, and will normally cause the program to hang when we join the thread. 86 | * As a workaround (similar to the one used by Boost), check the m_running 87 | * variable in the C callback functions and throw an exception if it's false. Note, 88 | * for this to work, the main function must call the C callback functions 89 | * regularly. */ 90 | try { 91 | main(); 92 | } catch (int e) { 93 | (void)e; 94 | } 95 | } 96 | 97 | void Microcontroller::onKeyEvent(const Event::Key &keyEvent) 98 | { 99 | (void)keyEvent; 100 | } 101 | 102 | float Microcontroller::getVoltageLevel(int idx) 103 | { 104 | m_voltageLinesMutex.lock(); 105 | assert(idx >= 0); 106 | assert(idx <= VoltageLine::Idx::Count); 107 | float level = m_microcontrollerVoltageLineLevels[idx]; 108 | m_voltageLinesMutex.unlock(); 109 | if (!m_running) { 110 | throw 0; 111 | } 112 | return level; 113 | } 114 | 115 | float get_voltage_cb(int idx, void *userdata) 116 | { 117 | assert(userdata); 118 | Microcontroller *microcontroller = static_cast(userdata); 119 | return microcontroller->getVoltageLevel(idx); 120 | } 121 | 122 | void Microcontroller::setVoltageLevel(int idx, float level) 123 | { 124 | m_voltageLinesMutex.lock(); 125 | assert(idx >= 0); 126 | assert(idx <= VoltageLine::Idx::Count); 127 | m_microcontrollerVoltageLineLevels[idx] = level; 128 | m_voltageLinesMutex.unlock(); 129 | if (!m_running) { 130 | throw 0; 131 | } 132 | } 133 | 134 | void set_voltage_cb(int idx, float level, void *userdata) 135 | { 136 | assert(userdata); 137 | Microcontroller *microcontroller = static_cast(userdata); 138 | microcontroller->setVoltageLevel(idx, level); 139 | } 140 | 141 | void Microcontroller::msSleep(int sleep_ms) 142 | { 143 | std::unique_lock uniqueLock(m_mutexSteps); 144 | m_sleepSteps = static_cast(sleep_ms / (m_currentStepTime * 1000)); 145 | m_conditionWake.wait(uniqueLock, [this] { return m_sleepSteps == 0; }); 146 | if (!m_running) { 147 | throw 0; 148 | } 149 | } 150 | 151 | void ms_sleep_cb(uint32_t sleep_ms, void *userdata) 152 | { 153 | assert(userdata); 154 | Microcontroller *microcontroller = static_cast(userdata); 155 | microcontroller->msSleep(sleep_ms); 156 | } 157 | 158 | uint32_t millis_cb(void *userdata) 159 | { 160 | assert(userdata); 161 | Microcontroller *microcontroller = static_cast(userdata); 162 | return microcontroller->millis(); 163 | } 164 | 165 | uint32_t Microcontroller::millis() 166 | { 167 | if (!m_running) { 168 | throw 0; 169 | } 170 | std::unique_lock uniqueLock(m_mutexSteps); 171 | return static_cast(1000 * m_stepsTaken * m_currentStepTime); 172 | } 173 | -------------------------------------------------------------------------------- /src/controllers/components/Microcontroller.h: -------------------------------------------------------------------------------- 1 | #ifndef MICROCONTROLLER_H_ 2 | #define MICROCONTROLLER_H_ 3 | 4 | #include 5 | /* These are C-callback functions. */ 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | float get_voltage_cb(int idx, void *userdata); 10 | void set_voltage_cb(int idx, float level, void *userdata); 11 | void ms_sleep_cb(uint32_t sleep_ms, void *userdata); 12 | uint32_t millis_cb(void *userdata); 13 | #ifdef __cplusplus 14 | } 15 | #endif 16 | 17 | /* This header is shared between C++ and C */ 18 | #ifdef __cplusplus 19 | #include "ControllerComponent.h" 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | /** 26 | * Base class for mimicking a real microcontroller. 27 | * 28 | * The execution of the microcontroller is offloaded to a separate thread to prevent 29 | * the simulator's update rate from depending on the microcontroller's update rate 30 | * (or vice versa). 31 | * 32 | * This class must be inherited and the inheritor must implement main. 33 | */ 34 | class Microcontroller : public ControllerComponent 35 | { 36 | public: 37 | /** 38 | * A voltage line simulates a real voltage line with a pointer 39 | * to a float value, which represents the voltage level. This pointer 40 | * enables sharing the value between a microcontroller and an "electrical" 41 | * object such as a motor or sensor. In other words, it's how the microcontroller 42 | * control output (e.g. motor) and read input (sensor). 43 | */ 44 | struct VoltageLine { 45 | enum Idx { 46 | A0 = 0, A1, A2, A3, A4, A5, A6, A7, 47 | B0, B1, B2, B3, B4, B5, B6, B7, 48 | Count 49 | }; 50 | /* Direction from the microcontroller's point of view (e.g. motor is output, a sensor is input) */ 51 | enum class Type { Input, Output }; 52 | Type type = Type::Input; 53 | float *level = nullptr; 54 | }; 55 | typedef std::array VoltageLines; 56 | 57 | /** 58 | * \param voltageLines List of voltage lines that may be connected to "electrical" objects. 59 | * Users must manually keep track of which voltage lines are connected to what objects. 60 | */ 61 | Microcontroller(VoltageLines &voltageLines); 62 | ~Microcontroller(); 63 | 64 | /** 65 | * Must be called to start the loop function. 66 | */ 67 | void start(); 68 | 69 | void onFixedUpdate(float stepTime) override final; 70 | 71 | /** 72 | * Microcontroller should typically not handle key events, but don't make this method final, 73 | * because it can be useful to override it when debugging. 74 | */ 75 | virtual void onKeyEvent(const Event::Key &keyEvent); 76 | 77 | /** 78 | * Get voltage level for voltage line at index idx 79 | */ 80 | float getVoltageLevel(int idx); 81 | 82 | /** 83 | * Set voltage level of voltage line at index idx 84 | */ 85 | void setVoltageLevel(int idx, float level); 86 | 87 | /** 88 | * Calculates and sleeps for the number of physics steps contained 89 | * in sleep_ms milliseconds. 90 | * 91 | * NOTE: The precision is determined by how small the physics step time is. 92 | * A smaller step time means better precision. 93 | * NOTE: The controller code SHOULD use this function and NOT the OS sleep function. 94 | */ 95 | void msSleep(int sleep_ms); 96 | 97 | /** 98 | * Get the time elapsed from the number of physics steps taken 99 | */ 100 | uint32_t millis(); 101 | 102 | private: 103 | /** This is the controller main function; it runs in a separate thread */ 104 | virtual void main() = 0; 105 | 106 | /** Thread function that runs the microcontroller loop */ 107 | void microcontrollerThreadFn(); 108 | 109 | /** The voltage lines which the simulator objects (e.g. sumobot, wheel motor) writes to and read from */ 110 | VoltageLines m_simulatorVoltageLines; 111 | std::thread m_thread; 112 | std::atomic m_running = true; 113 | bool m_physicsStarted = false; 114 | bool m_microcontrollerStarted = false; 115 | 116 | /** 117 | * Since the simulator and microcontroller code runs in separate threads, we must separate the voltage 118 | * lines to avoid race conditions. Do this by creating a separate voltage level array that the microcontroller 119 | * can write freely to, and synchronize it (safely with mutex) with the Simulator's voltage lines every 120 | * simulation iteration. 121 | */ 122 | std::mutex m_voltageLinesMutex; 123 | void transferVoltageLevels(); 124 | float m_microcontrollerVoltageLineLevels[VoltageLine::Idx::Count] = {0}; 125 | 126 | /** 127 | * To make the sleep behaviour of the controller consistent with the physics, we should 128 | * sleep for a certain number of physics steps. This results in a much more reliable sleep behaviour 129 | * compared to using the OS sleep function, because the OS sleep function has no guarantee for how 130 | * long it takes for a sleeping thread to wake up. This also enables us to consistently slow down and 131 | * speed up the simulation. 132 | */ 133 | std::condition_variable m_conditionWake; 134 | unsigned int m_sleepSteps = 0; 135 | unsigned int m_stepsTaken = 0; 136 | std::mutex m_mutexSteps; 137 | float m_currentStepTime = 0.0f; 138 | }; 139 | #endif /* __cplusplus */ 140 | 141 | #endif /* MICROCONTROLLER_H_ */ 142 | -------------------------------------------------------------------------------- /src/controllers/components/microcontroller_c_bindings.c: -------------------------------------------------------------------------------- 1 | #include "microcontroller_c_bindings.h" 2 | #include "microcontroller_c_setup.h" 3 | #include "components/Microcontroller.h" 4 | #include 5 | 6 | /** Can only be used by a single C microcontroller at a time */ 7 | static void *userdata_ptr = NULL; 8 | 9 | void set_userdata(void *userdata) 10 | { 11 | userdata_ptr = userdata; 12 | } 13 | 14 | float get_voltage(int idx) 15 | { 16 | return get_voltage_cb(idx, userdata_ptr); 17 | } 18 | 19 | void set_voltage(int idx, float level) 20 | { 21 | set_voltage_cb(idx, level, userdata_ptr); 22 | } 23 | 24 | void sleep_ms(uint32_t ms) 25 | { 26 | ms_sleep_cb(ms, userdata_ptr); 27 | } 28 | 29 | uint32_t millis() 30 | { 31 | return millis_cb(userdata_ptr); 32 | } 33 | 34 | float get_voltage_ud(int idx, void *userdata) 35 | { 36 | return get_voltage_cb(idx, userdata); 37 | } 38 | 39 | void set_voltage_ud(int idx, float level, void *userdata) 40 | { 41 | set_voltage_cb(idx, level, userdata); 42 | } 43 | 44 | void sleep_ms_ud(uint32_t ms, void *userdata) 45 | { 46 | ms_sleep_cb(ms, userdata); 47 | } 48 | 49 | uint32_t millis_ud(void *userdata) 50 | { 51 | return millis_cb(userdata); 52 | } 53 | -------------------------------------------------------------------------------- /src/controllers/components/microcontroller_c_bindings.h: -------------------------------------------------------------------------------- 1 | #ifndef MICROCONTROLLER_C_BINDINGS_H_ 2 | #define MICROCONTROLLER_C_BINDINGS_H_ 3 | 4 | #include 5 | 6 | /** 7 | * Helper functions that lets a microcontroller written in C communicate 8 | * with the rest of the simulator. A C microcontroller MUST use these 9 | * functions. Include this file in the C-file of your C microcontroller. 10 | * 11 | * If you only simulate a single C microcontroller, then you can use the 12 | * functions without passing userdata. But if you simulate multiple ones, 13 | * only one of them can avoid passing userdata, the rest must use the 14 | * userdata (<>_ud) functions, otherwise there is no way to know which 15 | * controller that made the call. 16 | */ 17 | 18 | 19 | /** 20 | * Get voltage level for the voltage line connected to index idx. 21 | */ 22 | float get_voltage(int idx); 23 | /** 24 | * Set voltage level for the voltage line connected to index idx. 25 | */ 26 | void set_voltage(int idx, float level); 27 | 28 | /** 29 | * Sleeps for X number of physics steps until roughly sleep_ms milliseconds 30 | * have passed (more accurate if physics step time is smaller). 31 | * 32 | * NOTE: If you need to sleep in your microcontroller code you should use 33 | * THIS function, and not the OS sleep function! */ 34 | void sleep_ms(uint32_t ms); 35 | 36 | /** 37 | * Get time elapsed from the number of physics steps taken. 38 | */ 39 | uint32_t millis(void); 40 | 41 | /** 42 | * Same behaviour as get_voltage, but should be used when running multiple 43 | * C microcontrollers at the same time. 44 | */ 45 | float get_voltage_ud(int idx, void *userdata); 46 | 47 | /** 48 | * Same behaviour as set_voltage, but should be used when running multiple 49 | * C microcontrollers at the same time. 50 | */ 51 | void set_voltage_ud(int idx, float level, void *userdata); 52 | 53 | /** 54 | * Same behaviour as sleep_ms, but should be used when running multiple 55 | * C microcontrollers at the same time. 56 | */ 57 | void sleep_ms_ud(uint32_t ms, void *userdata); 58 | 59 | /** 60 | * Same behaviour as millis, but should be used when running multiple 61 | * C microcontrollers at the same time. 62 | */ 63 | uint32_t millis_ud(void *userdata); 64 | 65 | 66 | #endif /* MICROCONTROLLER_C_BINDINGS_H_ */ 67 | -------------------------------------------------------------------------------- /src/controllers/components/microcontroller_c_setup.h: -------------------------------------------------------------------------------- 1 | #ifndef MICROCONTROLLER_C_SETUP_H_ 2 | #define MICROCONTROLLER_C_SETUP_H_ 3 | 4 | /** 5 | * Separate from micrcontroller_c_functions.h because this function shouldn't be visible to 6 | * the user (microcontroller main.c) of the other functions. 7 | */ 8 | 9 | /** 10 | * Set userdata statically to avoid having to pass userdata to each C function in your 11 | * simulated C microcontroller. NOTE, you can simulate at most ONE microcontroller at a time 12 | * that avoids passing userdata, the rest must pass userdata explicitly. 13 | */ 14 | void set_userdata(void *userdata); 15 | 16 | #endif /* MICROCONTROLLER_C_SETUP_H_ */ 17 | -------------------------------------------------------------------------------- /src/core/Application.h: -------------------------------------------------------------------------------- 1 | #ifndef APPLICATION_H_ 2 | #define APPLICATION_H_ 3 | 4 | #include "Event.h" 5 | #include 6 | #include 7 | 8 | #include "Scalebar.h" 9 | 10 | class Scene; 11 | class SceneMenu; 12 | struct GLFWwindow; 13 | 14 | /** 15 | * Application base class. Sets up OpenGL and GLFW (window and event handling), 16 | * and implements the main loop. 17 | * 18 | * You inherit this when you create your own Application, look at SimulatorTestApp for 19 | * an example. 20 | */ 21 | class Application 22 | { 23 | public: 24 | Application(std::string name); 25 | virtual ~Application(); 26 | /** Starts the main loop */ 27 | void run(); 28 | void onKeyCallback(const Event::Key &keyEvent); 29 | virtual void onKeyEvent(const Event::Key &keyEvent); 30 | 31 | /** Runs after every physics step */ 32 | virtual void onFixedUpdate(); 33 | 34 | protected: 35 | std::unique_ptr m_sceneMenu; 36 | void setCurrentScene(); 37 | 38 | private: 39 | bool isStepTimeTooSmall() const; 40 | void updatePhysics(float stepTime); 41 | void updateLogic(float stepTime); 42 | void updateAndRenderSceneMenu(); 43 | void render(); 44 | 45 | GLFWwindow *m_window = nullptr; 46 | std::unique_ptr m_scalebar = nullptr; 47 | float m_fps = 0.0f; 48 | float m_avgPhysicsSteps = 0.0f; 49 | Scene *m_currentScene = nullptr; 50 | }; 51 | 52 | #endif /* APPLICATION_H_ */ 53 | -------------------------------------------------------------------------------- /src/core/Component.h: -------------------------------------------------------------------------------- 1 | #ifndef COMPONENT_H_ 2 | #define COMPONENT_H_ 3 | 4 | /** 5 | * The base class all implemented components indirectly inherit from. 6 | */ 7 | class Component 8 | { 9 | public: 10 | virtual ~Component() { } 11 | /** 12 | * Called every physics step (if assigned to a Scene Object). 13 | */ 14 | virtual void onFixedUpdate() 15 | { 16 | }; 17 | }; 18 | 19 | #endif /* COMPONENT_H_ */ 20 | -------------------------------------------------------------------------------- /src/core/Event.cpp: -------------------------------------------------------------------------------- 1 | #include "Event.h" 2 | 3 | #include "GLFW/glfw3.h" 4 | 5 | Event::KeyCode Event::GLFWKeyToKeyCode(int GLFWKey) { 6 | switch (GLFWKey) 7 | { 8 | case GLFW_KEY_A: return Event::KeyCode::A; 9 | case GLFW_KEY_S: return Event::KeyCode::S; 10 | case GLFW_KEY_D: return Event::KeyCode::D; 11 | case GLFW_KEY_W: return Event::KeyCode::W; 12 | case GLFW_KEY_R: return Event::KeyCode::R; 13 | case GLFW_KEY_UP: return Event::KeyCode::Up; 14 | case GLFW_KEY_DOWN: return Event::KeyCode::Down; 15 | case GLFW_KEY_LEFT: return Event::KeyCode::Left; 16 | case GLFW_KEY_RIGHT: return Event::KeyCode::Right; 17 | case GLFW_KEY_SPACE: return Event::KeyCode::Space; 18 | case GLFW_KEY_ESCAPE: return Event::KeyCode::Escape; 19 | } 20 | return Event::KeyCode::Unhandled; 21 | } 22 | 23 | Event::KeyAction Event::GLFWActionToKeyAction(int GLFWAction) { 24 | switch (GLFWAction) 25 | { 26 | case GLFW_RELEASE: return Event::KeyAction::Release; 27 | case GLFW_PRESS: return Event::KeyAction::Press; 28 | case GLFW_REPEAT: return Event::KeyAction::Repeat; 29 | } 30 | return Event::KeyAction::Unhandled; 31 | } 32 | -------------------------------------------------------------------------------- /src/core/Event.h: -------------------------------------------------------------------------------- 1 | #ifndef EVENT_H_ 2 | #define EVENT_H_ 3 | 4 | /** 5 | * Wrapper class around GLFW window, key and scroll events. 6 | */ 7 | class Event 8 | { 9 | public: 10 | struct Window 11 | { 12 | int width; 13 | int height; 14 | }; 15 | 16 | struct Scroll 17 | { 18 | double xoffset; 19 | double yoffset; 20 | }; 21 | 22 | /* Create wrappers around GLFW */ 23 | enum class KeyCode 24 | { 25 | A, 26 | S, 27 | D, 28 | W, 29 | R, 30 | Escape, 31 | Space, 32 | Up, 33 | Down, 34 | Left, 35 | Right, 36 | Unhandled 37 | }; 38 | enum class KeyAction 39 | { 40 | Release, 41 | Press, 42 | Repeat, 43 | Unhandled 44 | }; 45 | struct Key 46 | { 47 | KeyCode code; 48 | KeyAction action; 49 | }; 50 | 51 | static KeyCode GLFWKeyToKeyCode(int GLFWAction); 52 | static KeyAction GLFWActionToKeyAction(int GLFWKey); 53 | }; 54 | 55 | #endif /* EVENT_H_ */ 56 | -------------------------------------------------------------------------------- /src/physics/Body2DUserData.h: -------------------------------------------------------------------------------- 1 | #ifndef BODY_2D_USER_DATA_H_ 2 | #define BODY_2D_USER_DATA_H_ 3 | 4 | #include 5 | 6 | enum class BodyId {LineDetector, Detectable}; 7 | 8 | struct Body2DUserData 9 | { 10 | std::atomic contactCount; 11 | std::atomic inContact; 12 | BodyId bodyId; 13 | }; 14 | 15 | #endif /* BODY_2D_USER_DATA_H_ */ 16 | -------------------------------------------------------------------------------- /src/physics/ContactListener.cpp: -------------------------------------------------------------------------------- 1 | #include "ContactListener.h" 2 | #include "Body2DUserData.h" 3 | 4 | void ContactListener::updateLineDetector(b2Contact *contact, bool begin) 5 | { 6 | b2BodyUserData bodyAUserData = contact->GetFixtureA()->GetBody()->GetUserData(); 7 | b2BodyUserData bodyBUserData = contact->GetFixtureB()->GetBody()->GetUserData(); 8 | if (bodyAUserData.pointer && bodyBUserData.pointer) { 9 | Body2DUserData *userDataA = reinterpret_cast(bodyAUserData.pointer); 10 | Body2DUserData *userDataB = reinterpret_cast(bodyBUserData.pointer); 11 | 12 | if (userDataA->bodyId == BodyId::LineDetector && userDataB->bodyId == BodyId::Detectable) { 13 | userDataA->contactCount += begin ? 1 : -1; 14 | } else if (userDataB->bodyId == BodyId::LineDetector && userDataA->bodyId == BodyId::Detectable) { 15 | userDataB->contactCount += begin ? 1 : -1; 16 | } 17 | } 18 | } 19 | 20 | void ContactListener::BeginContact(b2Contact *contact) { 21 | updateLineDetector(contact, true); 22 | } 23 | 24 | void ContactListener::EndContact(b2Contact *contact) { 25 | updateLineDetector(contact, false); 26 | } 27 | -------------------------------------------------------------------------------- /src/physics/ContactListener.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTACT_LISTENER_H_ 2 | #define CONTACT_LISTENER_H_ 3 | 4 | #include 5 | #include 6 | 7 | /** 8 | * Implements Box2D contact listener callbacks. 9 | */ 10 | class ContactListener : public b2ContactListener 11 | { 12 | public: 13 | void BeginContact(b2Contact *contact); 14 | void EndContact(b2Contact *contact); 15 | 16 | private: 17 | void updateLineDetector(b2Contact *contact, bool begin); 18 | }; 19 | 20 | #endif /* CONTACT_LISTENER_H_ */ 21 | -------------------------------------------------------------------------------- /src/physics/PhysicsWorld.cpp: -------------------------------------------------------------------------------- 1 | #include "PhysicsWorld.h" 2 | #include "ContactListener.h" 3 | 4 | #include "box2d/box2d.h" 5 | #include 6 | 7 | namespace { 8 | constexpr float maxWidthObject { 1.0f }; 9 | constexpr float minWidthObject { 0.01f }; 10 | constexpr float physScaleFactor { 10.0f }; 11 | constexpr float lengthScaleFactor { physScaleFactor }; 12 | constexpr float speedScaleFactor { lengthScaleFactor }; 13 | constexpr float accelerationScaleFactor { speedScaleFactor }; 14 | constexpr float massScaleFactor { physScaleFactor * physScaleFactor * physScaleFactor }; 15 | /* F = m*a so F_scaled = forceFactor * F = (massFactor * m) * (accFactor * a) */ 16 | const float forceScaleFactor { massScaleFactor * accelerationScaleFactor }; 17 | constexpr float gravitationConstant { 9.82f }; 18 | } 19 | 20 | void PhysicsWorld::assertDimensions(float unscaledLength) 21 | { 22 | assert(minWidthObject <= unscaledLength && unscaledLength <= maxWidthObject); 23 | } 24 | 25 | float PhysicsWorld::scaleLength(float unscaledLength) 26 | { 27 | assertDimensions(unscaledLength); 28 | return unscaledLength * lengthScaleFactor; 29 | } 30 | 31 | float PhysicsWorld::scaleLengthNoAssert(float unscaledLength) 32 | { 33 | assert(unscaledLength >= 0.0f); 34 | return unscaledLength * lengthScaleFactor; 35 | } 36 | 37 | float PhysicsWorld::unscaleLength(float scaledLength) 38 | { 39 | const float unscaledLength = scaledLength / lengthScaleFactor; 40 | assertDimensions(unscaledLength); 41 | return unscaledLength; 42 | } 43 | 44 | float PhysicsWorld::scaleRadius(float unscaledRadius) 45 | { 46 | return scaleLength(unscaledRadius * 2.0f) / 2.0f; 47 | } 48 | 49 | float PhysicsWorld::scalePosition(float unscaledPosition) 50 | { 51 | /* No dimension limitation for position x or y */ 52 | return unscaledPosition * lengthScaleFactor; 53 | } 54 | 55 | glm::vec2 PhysicsWorld::scalePosition(const glm::vec2 &unscaledPosition) 56 | { 57 | return { scalePosition(unscaledPosition.x), scalePosition(unscaledPosition.y) }; 58 | } 59 | 60 | float PhysicsWorld::unscalePosition(const float scaledPosition) 61 | { 62 | return scaledPosition / lengthScaleFactor; 63 | } 64 | 65 | glm::vec2 PhysicsWorld::unscalePosition(const glm::vec2 &scaledPosition) 66 | { 67 | return { unscalePosition(scaledPosition.x), unscalePosition(scaledPosition.y) }; 68 | } 69 | 70 | float PhysicsWorld::scaleSpeed(float unscaledSpeed) 71 | { 72 | return unscaledSpeed * speedScaleFactor; 73 | } 74 | 75 | float PhysicsWorld::unscaleSpeed(float scaledSpeed) 76 | { 77 | return scaledSpeed / speedScaleFactor; 78 | } 79 | 80 | float PhysicsWorld::scaleAcceleration(float unscaledAcceleration) 81 | { 82 | return unscaledAcceleration * accelerationScaleFactor; 83 | } 84 | 85 | float PhysicsWorld::scaleMass(float unscaledMass) 86 | { 87 | assert(unscaledMass >= 0.0f); 88 | return unscaledMass * massScaleFactor; 89 | } 90 | 91 | float PhysicsWorld::unscaleMass(float scaledMass) 92 | { 93 | assert(scaledMass >= 0.0f); 94 | return scaledMass / massScaleFactor; 95 | } 96 | 97 | float PhysicsWorld::scaleForce(float unscaledForce) 98 | { 99 | return unscaledForce * forceScaleFactor; 100 | } 101 | 102 | float PhysicsWorld::unscaleForce(float scaledForce) 103 | { 104 | return scaledForce / forceScaleFactor; 105 | } 106 | 107 | float PhysicsWorld::normalForce(float unscaledMass) 108 | { 109 | assert(unscaledMass >= 0.0f); 110 | return unscaledMass * gravitationConstant * forceScaleFactor; 111 | } 112 | 113 | void PhysicsWorld::init() 114 | { 115 | m_contactListener = std::make_unique(); 116 | m_world->SetContactListener(m_contactListener.get()); 117 | } 118 | 119 | PhysicsWorld::PhysicsWorld(Gravity gravity) : 120 | m_gravityType(gravity) 121 | { 122 | switch (gravity) 123 | { 124 | case Gravity::SideView: 125 | /* The typical Box2D physics */ 126 | m_world = std::make_unique(b2Vec2(0, -gravitationConstant)); 127 | break; 128 | case Gravity::TopView: 129 | /* Box2D is not designed for top view physics, but we 130 | can work around this by setting gravity to 0 */ 131 | m_world = std::make_unique(b2Vec2(0, 0)); 132 | break; 133 | case Gravity::Custom: 134 | /* If you end up here, you have used the wrong constructor, use the other one */ 135 | assert(false); 136 | } 137 | m_gravityType = gravity; 138 | init(); 139 | } 140 | 141 | PhysicsWorld::PhysicsWorld(float gravityX, float gravityY) : 142 | m_world(std::make_unique(b2Vec2(gravityX, gravityY))), 143 | m_gravityType(Gravity::Custom) 144 | { 145 | init(); 146 | } 147 | 148 | PhysicsWorld::~PhysicsWorld() 149 | { 150 | } 151 | 152 | void PhysicsWorld::step(float stepTime) 153 | { 154 | /* The iteration values 6 and 2 are recommended value taken from elsewhere. 155 | They matter when calculating collision. */ 156 | m_world->Step(stepTime, 6, 2); 157 | } 158 | -------------------------------------------------------------------------------- /src/physics/PhysicsWorld.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS_WORLD_H_ 2 | #define PHYSICS_WORLD_H_ 3 | 4 | #include 5 | #include 6 | 7 | class b2World; 8 | class ContactListener; 9 | 10 | /** 11 | * Wrapper class around Box2D b2World. Only one instance should exist at a time. 12 | * 13 | * Box2D uses metric units, and its documentation recommends sizing objects between 14 | * 0.1-10 m. For this simulator, it's more appropriate with a smaller range. Therefore, 15 | * we use a scale factor of 10. This class provides the necessary scaling 16 | * functions, which should only be called by classes inheriting PhysicsComponent. 17 | * That is, physics components work in the range 0.1-10 m while the other classes 18 | * work in the range 0.01-1 m. 19 | */ 20 | class PhysicsWorld 21 | { 22 | public: 23 | /** 24 | * PhysicsWorld supports different gravity modes. SideView is the same as the 25 | * regular 2D-game with a side view (Box2D default) 9.82 m/s^2 gravity constant. 26 | * TopView represents gravity when looked at from above. Custom mode can be used 27 | * to set specific gravity constant for X and Y axis. 28 | */ 29 | enum class Gravity { SideView, TopView, Custom }; 30 | PhysicsWorld(Gravity gravity); 31 | PhysicsWorld(float gravityX, float gravityY); 32 | ~PhysicsWorld(); 33 | void init(); 34 | 35 | void step(float stepTime); 36 | inline Gravity getGravityType() const { return m_gravityType; } 37 | 38 | static void assertDimensions(float unscaledLength); 39 | static float scaleLength(float unscaledLength); 40 | static float scaleLengthNoAssert(float unscaledLength); 41 | static float unscaleLength(float scaledLength); 42 | static float scaleRadius(float unscaledRadius); 43 | static float scalePosition(float unscaledPosition); 44 | static glm::vec2 scalePosition(const glm::vec2 &unscaledPosition); 45 | static float unscalePosition(float scaledPosition); 46 | static glm::vec2 unscalePosition(const glm::vec2 &scaledPosition); 47 | static float scaleSpeed(float unscaledSpeed); 48 | static float unscaleSpeed(float scaledSpeed); 49 | static float scaleAcceleration(float unscaledAcceleration); 50 | static float scaleMass(float unscaledMass); 51 | static float unscaleMass(float scaledMass); 52 | static float scaleForce(float unscaledForce); 53 | static float unscaleForce(float scaledForce); 54 | static float normalForce(float unscaledMass); 55 | 56 | /** To give physics components to access to b2World */ 57 | friend class PhysicsComponent; 58 | private: 59 | std::unique_ptr m_world; 60 | std::unique_ptr m_contactListener; 61 | Gravity m_gravityType = Gravity::SideView; 62 | }; 63 | 64 | #endif /* PHYSICS_WORLD_H_ */ 65 | -------------------------------------------------------------------------------- /src/physics/components/Body2D.h: -------------------------------------------------------------------------------- 1 | #ifndef BODY_2D_H_ 2 | #define BODY_2D_H_ 3 | 4 | #include "PhysicsComponent.h" 5 | #include 6 | 7 | class b2Body; 8 | class b2Joint; 9 | class RectTransform; 10 | class CircleTransform; 11 | class HollowCircleTransform; 12 | class QuadTransform; 13 | struct Body2DUserData; 14 | class b2FrictionJoint; 15 | struct QuadCoords; 16 | 17 | /** 18 | * Basic physics body class which creates a b2Body based on a transform. It synchronizes 19 | * the Box2D changes (position+rotation) with the transform data every fixed update. 20 | * 21 | * Many of the functions are just simple wrappers around b2Body. 22 | */ 23 | class Body2D : public PhysicsComponent 24 | { 25 | public: 26 | struct Specification { 27 | Specification() {} 28 | Specification(bool dynamic, bool collision, float mass, float frictionCoefficient, float angularDamping) : 29 | dynamic(dynamic), collision(collision), mass(mass), frictionCoefficient(frictionCoefficient), 30 | angularDamping(angularDamping) {} 31 | Specification(bool dynamic, bool collision, float mass) : 32 | dynamic(dynamic), collision(collision), mass(mass) {} 33 | /** Set body as dynamic (moveable) or static (still) */ 34 | bool dynamic = false; 35 | /** Enable or disable collision */ 36 | bool collision = true; 37 | float mass = 1.0f; 38 | /** This determines how easy it is to push the body (ONLY for top view) */ 39 | float frictionCoefficient = 0.0f; 40 | /** Damps the angular velocity (note, for values above 1.0f, the damping 41 | becomes sensitive to time step according to box2d docs) **/ 42 | float angularDamping = 0.0f; 43 | }; 44 | 45 | Body2D(const PhysicsWorld &world, const glm::vec2 &startPosition, float rotation, float radius, 46 | const Body2D::Specification &spec); 47 | Body2D(const PhysicsWorld &world, CircleTransform *transform, const Specification &spec); 48 | Body2D(const PhysicsWorld &world, RectTransform *transform, const Specification &spec); 49 | Body2D(const PhysicsWorld &world, HollowCircleTransform *transform, const Specification &spec); 50 | Body2D(const PhysicsWorld &world, QuadTransform *transform, const Specification &spec); 51 | ~Body2D(); 52 | void onFixedUpdate(float stepTime) override; 53 | void setUserData(Body2DUserData *userData); 54 | void attachBodyWithRevoluteJoint(const glm::vec2 &attachPos, const Body2D *body); 55 | void attachBodyWithWeldJoint(const glm::vec2 &attachPos, const Body2D *body); 56 | void setPositionAndRotation(const glm::vec2 &position, float rotation); 57 | void setForce(const glm::vec2 &vec, float magnitude); 58 | void setLinearImpulse(const glm::vec2 &vec); 59 | float getForwardSpeed() const; 60 | float getAngularSpeed() const; 61 | glm::vec2 getLateralVelocity() const; 62 | glm::vec2 getForwardNormal() const; 63 | glm::vec2 getPosition() const; 64 | float getRotation() const; 65 | float getMass() const; 66 | void setMass(float mass); 67 | void setFrictionCoefficient(float frictionCoefficient); 68 | void setAngularDamping(float angularDamping); 69 | float getAngularDamping() const; 70 | 71 | private: 72 | /** 73 | * In top view gravity mode, the gravity is set to 0. 74 | * We use b2FrictionJoint as a workaround for friction. 75 | */ 76 | void addTopViewFriction(float normalForce, float frictionCoefficient); 77 | float getTopViewFrictionForce(float stepTime) const; 78 | 79 | float m_topViewFrictionCoefficient = 0.0f; 80 | b2FrictionJoint *m_topViewFrictionJoint = nullptr; 81 | b2Body *m_body = nullptr; 82 | b2Body *m_frictionBody = nullptr; 83 | }; 84 | 85 | #endif /* BODY_2D_H_ */ 86 | -------------------------------------------------------------------------------- /src/physics/components/LineDetector.cpp: -------------------------------------------------------------------------------- 1 | #include "components/LineDetector.h" 2 | #include "components/Body2D.h" 3 | #include "components/Transforms.h" 4 | 5 | namespace { 6 | constexpr float drawRadiusUndetected = 0.001f; 7 | constexpr float drawRadiusDetected = 10 * drawRadiusUndetected; 8 | } 9 | 10 | LineDetector::LineDetector(const PhysicsWorld &world, CircleTransform *transform, const glm::vec2 &startPosition, 11 | float updateRateSeconds) : 12 | PhysicsComponent(world), 13 | m_transform(transform), 14 | m_updateRateSeconds(updateRateSeconds) 15 | { 16 | /* Represent as tiny body */ 17 | const Body2D::Specification bodySpec { true, false, 0.001f }; 18 | m_body2D = std::make_unique(world, startPosition, 0.0f, drawRadiusUndetected, bodySpec); 19 | m_body2D->setUserData(&m_userData); 20 | } 21 | 22 | LineDetector::~LineDetector() 23 | { 24 | } 25 | 26 | Body2D *LineDetector::getBody() const 27 | { 28 | return m_body2D.get(); 29 | } 30 | 31 | float *LineDetector::getVoltageLine() 32 | { 33 | return &m_detectVoltage; 34 | } 35 | 36 | void LineDetector::onFixedUpdate(float stepTime) 37 | { 38 | const bool detected = m_userData.contactCount.load() > 0; 39 | if (m_updateRateSeconds && m_timeSinceLastUpdate < m_updateRateSeconds) { 40 | m_timeSinceLastUpdate += stepTime; 41 | } else { 42 | m_detectVoltage = detected ? 3.3f : 0.0f; 43 | m_timeSinceLastUpdate = 0.0f; 44 | } 45 | const bool debugDraw = m_transform != nullptr; 46 | if (debugDraw) { 47 | m_transform->position = m_body2D->getPosition(); 48 | m_transform->radius = detected ? drawRadiusDetected : drawRadiusUndetected; 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /src/physics/components/LineDetector.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_DETECTOR_H_ 2 | #define LINE_DETECTOR_H_ 3 | 4 | #include "PhysicsComponent.h" 5 | #include "Body2DUserData.h" 6 | 7 | class Body2D; 8 | class b2Body; 9 | class CircleTransform; 10 | 11 | /** 12 | * A perfect line detector sensor, which detects physics bodies with BodyId set to 13 | * "Detectable" (see Body2DUserData). 14 | */ 15 | class LineDetector : public PhysicsComponent 16 | { 17 | public: 18 | LineDetector(const PhysicsWorld &world, CircleTransform *transform, const glm::vec2 &startPosition, 19 | float updateRateSeconds = 0.0f); 20 | ~LineDetector(); 21 | void onFixedUpdate(float stepTime) override; 22 | Body2D *getBody() const; 23 | /** Retrieve a pointer to the voltage line where the value is > 0 when detected and 0 when not detected. */ 24 | float *getVoltageLine(); 25 | 26 | private: 27 | std::unique_ptr m_body2D; 28 | Body2DUserData m_userData = { 0, 0, BodyId::LineDetector }; 29 | CircleTransform * const m_transform = nullptr; 30 | float m_detectVoltage = 0.0f; 31 | float m_updateRateSeconds = 0.0f; 32 | float m_timeSinceLastUpdate = 0.0f; 33 | }; 34 | 35 | #endif /* LINE_DETECTOR_H_ */ 36 | -------------------------------------------------------------------------------- /src/physics/components/PhysicsComponent.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS_COMPONENT_H 2 | #define PHYSICS_COMPONENT_H 3 | 4 | #include "PhysicsWorld.h" 5 | #include "Component.h" 6 | 7 | /** 8 | * Base class for translating from physics data to transform data. 9 | * Physics component classes should use inherited classes of this 10 | * when they update transforms. 11 | */ 12 | class PhysicsToTransformTranslator 13 | { 14 | public: 15 | virtual ~PhysicsToTransformTranslator() {} 16 | virtual void translate() = 0; 17 | }; 18 | 19 | /** 20 | * Base class for components that implement physics behaviour. 21 | * 22 | * It can't be used directly in a Scene, instead it must be assigned to 23 | * a Scene object to be updated each simulation iteration. 24 | */ 25 | class PhysicsComponent : public Component 26 | { 27 | public: 28 | PhysicsComponent(const PhysicsWorld &world) : 29 | m_world(world.m_world.get()) {} 30 | virtual ~PhysicsComponent() { 31 | } 32 | virtual void onFixedUpdate(float stepTime) = 0; 33 | 34 | protected: 35 | b2World *m_world; 36 | std::unique_ptr m_translator; 37 | }; 38 | 39 | #endif /* PHYSICS_COMPONENT_H */ 40 | -------------------------------------------------------------------------------- /src/physics/components/RangeSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "components/RangeSensor.h" 2 | 3 | #include 4 | 5 | #include "components/Body2D.h" 6 | #include "components/Transforms.h" 7 | 8 | namespace { 9 | constexpr float debugDrawWidth = 0.001f; 10 | } 11 | 12 | RangeSensor::RangeSensor(const PhysicsWorld &world, LineTransform *transform, 13 | const glm::vec2 &startPosition, float angle, 14 | float minDistance, float maxDistance, 15 | float updateRateSecond) 16 | : PhysicsComponent(world), 17 | m_lineTransform(transform), 18 | m_relativeAngle(angle), 19 | m_minDistance(minDistance), 20 | m_maxDistance(maxDistance), 21 | m_detectedDistance(m_maxDistance), 22 | m_updateRateSecond(updateRateSecond) { 23 | /* Create tiny body for attaching and keeping track of ray cast start 24 | * position */ 25 | const Body2D::Specification bodySpec{true, false, 0.001f}; 26 | m_body2D = 27 | std::make_unique(world, startPosition, 0.0f, 0.0005f, bodySpec); 28 | } 29 | 30 | RangeSensor::~RangeSensor() {} 31 | 32 | Body2D *RangeSensor::getBody() const { return m_body2D.get(); } 33 | 34 | void RangeSensor::onFixedUpdate(float stepTime) { 35 | const float rayAngleEnd = m_relativeAngle - m_body2D->getRotation(); 36 | const glm::vec2 bodyPosition = m_body2D->getPosition(); 37 | 38 | /* Recalculate the casted ray */ 39 | const glm::vec2 rayStartPosition = bodyPosition; 40 | const glm::vec2 rayEndPosition = 41 | rayStartPosition + 42 | glm::vec2{sinf(rayAngleEnd), cosf(rayAngleEnd)} * m_maxDistance; 43 | updateDetectedDistance(rayStartPosition, rayEndPosition); 44 | const glm::vec2 detectedEndPosition = 45 | rayStartPosition + 46 | glm::vec2{sinf(rayAngleEnd), cosf(rayAngleEnd)} * m_detectedDistance; 47 | const bool debugDraw = m_lineTransform != nullptr; 48 | 49 | if (debugDraw) { 50 | m_lineTransform->width = debugDrawWidth; 51 | m_lineTransform->start = {rayStartPosition.x, rayStartPosition.y}; 52 | m_lineTransform->end = {detectedEndPosition.x, detectedEndPosition.y}; 53 | } 54 | 55 | if (m_updateRateSecond && (m_timeSinceLastUpdate < m_updateRateSecond)) { 56 | m_timeSinceLastUpdate += stepTime; 57 | } else { 58 | m_timeSinceLastUpdate = 0.0f; 59 | } 60 | updateVoltage(); 61 | } 62 | 63 | void RangeSensor::updateDetectedDistance(const glm::vec2 &start, 64 | const glm::vec2 &end) { 65 | const glm::vec2 scaledStart = PhysicsWorld::scalePosition(start); 66 | const glm::vec2 scaledEnd = PhysicsWorld::scalePosition(end); 67 | b2RayCastInput rayInput; 68 | b2RayCastOutput rayOutput; 69 | rayInput.p1.x = scaledStart.x; 70 | rayInput.p1.y = scaledStart.y; 71 | rayInput.p2.x = scaledEnd.x; 72 | rayInput.p2.y = scaledEnd.y; 73 | rayInput.maxFraction = 1.0f; 74 | 75 | float closestFraction = rayInput.maxFraction; 76 | b2Vec2 intersectionNormal(0, 0); 77 | for (b2Body *b = m_world->GetBodyList(); b; b = b->GetNext()) { 78 | if (b->GetFixtureList() && b->GetFixtureList()[0].IsSensor()) { 79 | /* Don't detect non-collidable objects */ 80 | continue; 81 | } 82 | for (b2Fixture *f = b->GetFixtureList(); f; f = f->GetNext()) { 83 | if (!f->RayCast(&rayOutput, rayInput, 1)) { 84 | continue; 85 | } 86 | if (rayOutput.fraction < closestFraction) { 87 | closestFraction = rayOutput.fraction; 88 | intersectionNormal = rayOutput.normal; 89 | } 90 | } 91 | } 92 | 93 | m_detectedDistance = closestFraction * m_maxDistance; 94 | } 95 | 96 | float RangeSensor::getDistance() const { return m_detectedDistance; } 97 | 98 | void RangeSensor::updateVoltage() { 99 | m_distanceVoltage = m_detectedDistance / m_maxDistance; 100 | } 101 | 102 | float *RangeSensor::getVoltageLine() { return &m_distanceVoltage; } 103 | -------------------------------------------------------------------------------- /src/physics/components/RangeSensor.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGE_SENSOR_H_ 2 | #define RANGE_SENSOR_H_ 3 | 4 | #include "PhysicsComponent.h" 5 | 6 | class Body2D; 7 | class LineTransform; 8 | 9 | /** 10 | * Implements a perfect range sensor. Min and max distance are adjustable. 11 | */ 12 | class RangeSensor : public PhysicsComponent 13 | { 14 | public: 15 | RangeSensor(const PhysicsWorld &world, LineTransform *transform, 16 | const glm::vec2 &startPosition, float angle, float minDistance, float maxDistance, 17 | float updateRateSecond = 0.0f); 18 | ~RangeSensor(); 19 | void onFixedUpdate(float stepTime) override; 20 | /** Retrieve distance in metrics */ 21 | float getDistance() const; 22 | Body2D *getBody() const; 23 | /** Retrieve a pointer to a voltage value which corresponds to the detected distance where max 24 | * distance is 3.3 V. */ 25 | float *getVoltageLine(); 26 | 27 | private: 28 | void updateVoltage(); 29 | void updateDetectedDistance(const glm::vec2 &start, const glm::vec2 &end); 30 | 31 | LineTransform *const m_lineTransform = nullptr; 32 | const float m_relativeAngle = 0.0f; 33 | const float m_minDistance = 0.0f; 34 | const float m_maxDistance = 0.0f; 35 | std::unique_ptr m_body2D; 36 | 37 | float m_distanceVoltage = 0.0f; 38 | float m_detectedDistance = 0.0f; 39 | float m_timeSinceLastUpdate = 0.0f; 40 | float m_updateRateSecond = 0.0f; 41 | }; 42 | 43 | #endif /* RANGE_SENSOR_H_ */ 44 | -------------------------------------------------------------------------------- /src/renderer/AssetsHelper.cpp: -------------------------------------------------------------------------------- 1 | #include "AssetsHelper.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace { 9 | std::string assetsPath; 10 | const std::array allowedParentDirs {"", 11 | "external" 12 | "bots2d" 13 | "libs/bots2d" 14 | "external/bots2d"}; 15 | } 16 | 17 | void AssetsHelper::init() 18 | { 19 | struct stat info; 20 | const auto nestedLevelsToCheck = 5; 21 | 22 | /* Give some leeway for where the program is executed from. This 23 | * should work for both Linux and Windows */ 24 | std::string prefixPath = ""; 25 | for (int level = 0; level <= nestedLevelsToCheck; level++) { 26 | for (const auto &parentDir : allowedParentDirs) { 27 | const auto path = prefixPath + parentDir + "/assets/"; 28 | if (stat(path.c_str(), &info) == 0) { 29 | assetsPath = path; 30 | return; 31 | } 32 | } 33 | prefixPath = "../" + prefixPath; 34 | } 35 | 36 | std::cout << "Can't find the assets/ folder" 37 | "Checked for common parent dirs and several folder levels up" 38 | "You can include AssetsHelper in your application and " 39 | "set the path manually with AssetsHelper::setAssetsPath()" << std::endl; 40 | assert(false); 41 | } 42 | 43 | void AssetsHelper::setAssetsPath(const std::string relativePath) 44 | { 45 | assetsPath = relativePath; 46 | } 47 | 48 | std::string AssetsHelper::getAssetsPath() 49 | { 50 | assert(!assetsPath.empty()); 51 | return assetsPath; 52 | } 53 | 54 | std::string AssetsHelper::getTexturesPath() 55 | { 56 | return getAssetsPath() + "textures/"; 57 | } 58 | 59 | std::string AssetsHelper::getTexturePath(std::string textureName) 60 | { 61 | return getTexturesPath() + textureName; 62 | } 63 | -------------------------------------------------------------------------------- /src/renderer/AssetsHelper.h: -------------------------------------------------------------------------------- 1 | #ifndef ASSETS_HELPER_H_ 2 | #define ASSETS_HELPER_H_ 3 | 4 | #include 5 | 6 | /** 7 | * Finds the path to the resources folder. It enables the simulator binary to 8 | * be executed from different locations. 9 | */ 10 | class AssetsHelper 11 | { 12 | public: 13 | static void init(); 14 | void setAssetsPath(const std::string relativePath); 15 | static std::string getAssetsPath(); 16 | static std::string getTexturesPath(); 17 | static std::string getTexturePath(std::string textureName); 18 | }; 19 | 20 | #endif /* ASSETS__HELPER_H_ */ 21 | -------------------------------------------------------------------------------- /src/renderer/Camera.cpp: -------------------------------------------------------------------------------- 1 | #include "Camera.h" 2 | #include "Renderer.h" 3 | #include "Texture.h" 4 | #include "TexCoords.h" 5 | 6 | namespace { 7 | const float positionStepSize = 20.0f; 8 | const float zoomStepSize = 1.25f; 9 | const float minZoomFactor = 0.1f; 10 | const float maxZoomFactor = 4.0f; 11 | 12 | glm::vec2 cameraPosition = {0.0f, 0.0f}; 13 | float zoomFactor = 1.0f; 14 | int windowWidth = 800; 15 | int windowHeight = 600; 16 | } 17 | 18 | float Camera::getZoomFactor() 19 | { 20 | return zoomFactor; 21 | } 22 | 23 | glm::vec2 Camera::getPosition() 24 | { 25 | return cameraPosition; 26 | } 27 | 28 | glm::vec2 Camera::getWindowSize() 29 | { 30 | return { windowWidth, windowHeight }; 31 | } 32 | 33 | void Camera::reset() 34 | { 35 | cameraPosition.x = windowWidth / 2.0f; 36 | cameraPosition.y = windowHeight / 2.0f; 37 | zoomFactor = 1.0f / (zoomStepSize * zoomStepSize); 38 | Renderer::setCameraPosition(cameraPosition, zoomFactor); 39 | } 40 | 41 | bool Camera::onKeyEvent(const Event::Key &keyEvent) 42 | { 43 | if (keyEvent.action == Event::KeyAction::Press || keyEvent.action == Event::KeyAction::Repeat) { 44 | switch (keyEvent.code) 45 | { 46 | case Event::KeyCode::A: 47 | cameraPosition.x += positionStepSize; 48 | break; 49 | case Event::KeyCode::D: 50 | cameraPosition.x -= positionStepSize; 51 | break; 52 | case Event::KeyCode::S: 53 | cameraPosition.y += positionStepSize; 54 | break; 55 | case Event::KeyCode::W: 56 | cameraPosition.y -= positionStepSize; 57 | break; 58 | case Event::KeyCode::R: 59 | Camera::reset(); 60 | return true; 61 | default: 62 | return false; 63 | } 64 | Renderer::setCameraPosition(cameraPosition, zoomFactor); 65 | return true; 66 | } 67 | return false; 68 | } 69 | 70 | void Camera::onScrollEvent(const Event::Scroll &scrollEvent) 71 | { 72 | if (scrollEvent.yoffset > 0) { 73 | /* Zoom in */ 74 | zoomFactor *= zoomStepSize; 75 | zoomFactor = zoomFactor > maxZoomFactor ? maxZoomFactor : zoomFactor; 76 | } else if (scrollEvent.yoffset < 0) { 77 | /* Zoom out */ 78 | zoomFactor *= (1 / zoomStepSize); 79 | zoomFactor = zoomFactor < minZoomFactor ? minZoomFactor : zoomFactor; 80 | } 81 | Renderer::setCameraPosition(cameraPosition, zoomFactor); 82 | } 83 | 84 | void Camera::onWindowEvent(const Event::Window &windowEvent) 85 | { 86 | Renderer::setViewport(0, 0, windowEvent.width, windowEvent.height); 87 | windowWidth = windowEvent.width; 88 | windowHeight = windowEvent.height; 89 | reset(); 90 | } 91 | -------------------------------------------------------------------------------- /src/renderer/Camera.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_H_ 2 | #define CAMERA_H_ 3 | 4 | #include "Event.h" 5 | #include 6 | 7 | /** 8 | * Camera position and zoom factor. 9 | */ 10 | class Camera 11 | { 12 | public: 13 | static bool onKeyEvent(const Event::Key &keyEvent); 14 | static void onScrollEvent(const Event::Scroll &scrollEvent); 15 | static void onWindowEvent(const Event::Window &windowEvent); 16 | static float getZoomFactor(); 17 | static glm::vec2 getPosition(); 18 | static glm::vec2 getWindowSize(); 19 | static void reset(); 20 | }; 21 | 22 | #endif /* CAMERA_H_ */ 23 | -------------------------------------------------------------------------------- /src/renderer/GLError.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | void GLClearError() 6 | { 7 | while (glGetError() != GL_NO_ERROR); 8 | } 9 | 10 | bool GLLogCall(const char* function, const char* file, int line) 11 | { 12 | while (GLenum error = glGetError()) 13 | { 14 | std::cout << file << " " << line << " " << function << " [OpenGL errer]: " << error << std::endl; 15 | assert(false); 16 | } 17 | return true; 18 | } 19 | -------------------------------------------------------------------------------- /src/renderer/GLError.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Declares a macro for OpenGL calls to check for errors and report the 3 | * the failing line. This is a remnant from earlier OpenGL versions. In 4 | * later OpenGL a callback can be used instead. 5 | * 6 | * TODO: Replace with callback 7 | */ 8 | 9 | #define GLCall(x) GLClearError();\ 10 | x;\ 11 | GLLogCall(#x, __FILE__, __LINE__); 12 | 13 | void GLClearError(); 14 | bool GLLogCall(const char* function, const char* file, int line); 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/renderer/ImGuiMenu.cpp: -------------------------------------------------------------------------------- 1 | #include "ImGuiMenu.h" 2 | #include "ImGuiOverlay.h" 3 | #include "Scene.h" 4 | #include 5 | 6 | 7 | namespace { 8 | struct MenuLabel : ImGuiMenu::Item 9 | { 10 | MenuLabel(std::string text) : 11 | Item(Item::Type::Label), text(text) {} 12 | ~MenuLabel() = default; 13 | void setText(std::string newText) { text = newText; } 14 | std::string text; 15 | }; 16 | 17 | struct MenuButton : ImGuiMenu::Item 18 | { 19 | MenuButton(std::string text, std::function onClick) : 20 | Item(Item::Type::Button), text(text), onClick(onClick) {} 21 | ~MenuButton() = default; 22 | const std::string text; 23 | const std::function onClick; 24 | }; 25 | 26 | struct MenuSlider : ImGuiMenu::Item 27 | { 28 | MenuSlider(std::string name, float min, float max, float startValue, std::function onChanged) : 29 | Item(Item::Type::Slider), name(name), min(min), 30 | max(max), currentValue(startValue), onChanged(onChanged) {} 31 | ~MenuSlider() = default; 32 | bool hasValueChanged() const { return currentValue != oldValue; } 33 | const std::string name; 34 | float min = 0.0f; 35 | float max = 1.0f; 36 | float currentValue = 0.0f; 37 | float oldValue = 0.0f; 38 | const std::function onChanged; 39 | }; 40 | } 41 | 42 | ImGuiMenu::ImGuiMenu(Scene *scene, std::string name, float x, float y, float width, float height) : 43 | m_name(name), m_posX(x), m_posY(y), m_width(width), m_height(height) 44 | { 45 | assert(scene != nullptr); 46 | scene->addMenu(this); 47 | } 48 | 49 | void ImGuiMenu::render() 50 | { 51 | ImGuiOverlay::begin(m_name.c_str(), m_posX, m_posY, m_width, m_height); 52 | for (const auto &item : m_menuItems) { 53 | switch (item->type) { 54 | case Item::Type::Label: 55 | { 56 | const auto label = static_cast(item.get()); 57 | ImGuiOverlay::text(label->text); 58 | break; 59 | } 60 | case Item::Type::Button: 61 | { 62 | const auto button = static_cast(item.get()); 63 | if (ImGuiOverlay::button(button->text)) { 64 | if (button->onClick) { 65 | button->onClick(); 66 | } 67 | } 68 | break; 69 | } 70 | case Item::Type::Slider: 71 | { 72 | const auto slider = static_cast(item.get()); 73 | ImGuiOverlay::sliderFloat(slider->name, &slider->currentValue, slider->min, slider->max); 74 | if (slider->hasValueChanged()) { 75 | if (slider->onChanged) { 76 | slider->onChanged(slider->currentValue); 77 | } 78 | slider->oldValue = slider->currentValue; 79 | } 80 | break; 81 | } 82 | } 83 | } 84 | 85 | ImGuiOverlay::end(); 86 | } 87 | 88 | void ImGuiMenu::addButton(std::string text, std::function onClicked) 89 | { 90 | m_menuItems.push_back(std::make_unique(text, onClicked)); 91 | } 92 | 93 | void ImGuiMenu::addLabel(std::string text, std::function *setText) 94 | { 95 | auto menuLabel = std::make_unique(text); 96 | if (setText) { 97 | auto menuLabelPtr = menuLabel.get(); 98 | *setText = [menuLabelPtr](std::string text) 99 | { 100 | menuLabelPtr->setText(text); 101 | }; 102 | } 103 | m_menuItems.push_back(std::move(menuLabel)); 104 | } 105 | 106 | void ImGuiMenu::addSlider(std::string name, float min, float max, float startValue, std::function onChanged) 107 | { 108 | assert(min <= startValue && startValue <= max); 109 | m_menuItems.push_back(std::make_unique(name, min, max, startValue, onChanged)); 110 | } 111 | -------------------------------------------------------------------------------- /src/renderer/ImGuiMenu.h: -------------------------------------------------------------------------------- 1 | #ifndef IMGUI_MENU_H_ 2 | #define IMGUI_MENU_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class Scene; 10 | 11 | /** 12 | * A custom GUI menu, which can be added to a scene. Useful for tweaking 13 | * parameters of scene objects or showing information about a simulated scene. 14 | * 15 | * The menu items are placed in the order they are added (from top to bottom). 16 | */ 17 | class ImGuiMenu 18 | { 19 | public: 20 | struct Item 21 | { 22 | enum class Type { Label, Button, Slider }; 23 | Item(Type type) : type(type) {}; 24 | const Type type = Type::Label; 25 | }; 26 | 27 | ImGuiMenu(Scene *scene, std::string name, float x, float y, float width, float height); 28 | virtual ~ImGuiMenu() = default; 29 | void render(); 30 | /** 31 | * \param setText gives a function pointer for changing the text after the 32 | * label has been created. 33 | */ 34 | void addLabel(std::string text, std::function *setText = nullptr); 35 | void addButton(std::string text, std::function onClicked); 36 | void addSlider(std::string name, float min, float max, float startValue, std::function onChanged); 37 | 38 | private: 39 | std::vector> m_menuItems; 40 | const std::string m_name = ""; 41 | const float m_posX = 0.0f; 42 | const float m_posY = 0.0f; 43 | const float m_width = 100.0f; 44 | const float m_height = 100.0f; 45 | }; 46 | 47 | #endif /* IMGUI_MENU_H_ */ 48 | -------------------------------------------------------------------------------- /src/renderer/ImGuiOverlay.cpp: -------------------------------------------------------------------------------- 1 | #include "ImGuiOverlay.h" 2 | #include 3 | #include "backends/imgui_impl_glfw.h" 4 | #include "backends/imgui_impl_opengl3.h" 5 | 6 | void ImGuiOverlay::init(GLFWwindow *window) 7 | { 8 | const char* glsl_version = "#version 130"; 9 | glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); 10 | glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0); 11 | 12 | /* Copied from example_glfw_opengl3 */ 13 | IMGUI_CHECKVERSION(); 14 | ImGui::CreateContext(); 15 | ImGuiIO& io = ImGui::GetIO(); 16 | (void)io; 17 | ImGui::StyleColorsDark(); 18 | ImGui_ImplGlfw_InitForOpenGL(window, true); 19 | ImGui_ImplOpenGL3_Init(glsl_version); 20 | } 21 | 22 | void ImGuiOverlay::destroy() 23 | { 24 | ImGui_ImplOpenGL3_Shutdown(); 25 | ImGui_ImplGlfw_Shutdown(); 26 | ImGui::DestroyContext(); 27 | } 28 | 29 | void ImGuiOverlay::newFrame() 30 | { 31 | ImGui_ImplOpenGL3_NewFrame(); 32 | ImGui_ImplGlfw_NewFrame(); 33 | ImGui::NewFrame(); 34 | } 35 | 36 | void ImGuiOverlay::render() 37 | { 38 | ImGui::Render(); 39 | ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData()); 40 | } 41 | 42 | void ImGuiOverlay::begin(std::string name, float x, float y, float width, float height) 43 | { 44 | ImGui::SetNextWindowPos({x, y}); 45 | ImGui::SetNextWindowSize(ImVec2(width, height), ImGuiCond_Once); 46 | ImGui::Begin(name.c_str(), NULL, ImGuiWindowFlags_AlwaysAutoResize); 47 | } 48 | 49 | void ImGuiOverlay::end() 50 | { 51 | ImGui::End(); 52 | } 53 | 54 | bool ImGuiOverlay::button(std::string name) 55 | { 56 | return ImGui::Button(name.c_str()); 57 | } 58 | 59 | void ImGuiOverlay::text(std::string text) 60 | { 61 | ImGui::PushTextWrapPos(ImGui::GetCursorPos().x + 230); 62 | ImGui::Text("%s", text.c_str()); 63 | } 64 | 65 | void ImGuiOverlay::checkbox(std::string name, bool *set) 66 | { 67 | ImGui::Checkbox(name.c_str(), set); 68 | } 69 | 70 | void ImGuiOverlay::sliderFloat(std::string name, float *value, float min, float max) 71 | { 72 | ImGui::SliderFloat(name.c_str(), value, min, max); 73 | } 74 | 75 | /* TODO: Colored text: ImGui::TextColored(ImVec4(1,1,0,1), "Important Stuff"); */ 76 | -------------------------------------------------------------------------------- /src/renderer/ImGuiOverlay.h: -------------------------------------------------------------------------------- 1 | #ifndef IMGUI_OVERLAY_H_ 2 | #define IMGUI_OVERLAY_H_ 3 | 4 | #include 5 | 6 | struct GLFWwindow; 7 | 8 | /** 9 | * Wrapper around ImGui (lightweight GUI library). 10 | */ 11 | class ImGuiOverlay 12 | { 13 | public: 14 | static void init(GLFWwindow *window); 15 | static void destroy(); 16 | 17 | static void render(); 18 | static void newFrame(); 19 | static void begin(std::string name, float x, float y, float width, float height); 20 | static void end(); 21 | static bool button(std::string name); 22 | static void text(std::string text); 23 | static void checkbox(std::string name, bool *set); 24 | static void sliderFloat(std::string name, float *value, float min, float max); 25 | }; 26 | 27 | #endif /* IMGUI_OVERLAY_ */ 28 | -------------------------------------------------------------------------------- /src/renderer/IndexBuffer.cpp: -------------------------------------------------------------------------------- 1 | #include "IndexBuffer.h" 2 | #include 3 | #include "GLError.h" 4 | #include 5 | 6 | IndexBuffer::IndexBuffer(const unsigned int *data, unsigned int count) 7 | : m_count(count) 8 | { 9 | GLCall(glGenBuffers(1, &m_id)); 10 | GLCall(glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_id)); 11 | assert(sizeof(GLuint) == sizeof(unsigned int)); 12 | GLCall(glBufferData(GL_ELEMENT_ARRAY_BUFFER, count * sizeof(unsigned int), data, GL_STATIC_DRAW)); 13 | } 14 | 15 | IndexBuffer::~IndexBuffer() 16 | { 17 | GLCall(glDeleteBuffers(1, &m_id)); 18 | } 19 | 20 | void IndexBuffer::bind() const 21 | { 22 | GLCall(glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_id)); 23 | } 24 | 25 | void IndexBuffer::unbind() const 26 | { 27 | GLCall(glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0)); 28 | } 29 | -------------------------------------------------------------------------------- /src/renderer/IndexBuffer.h: -------------------------------------------------------------------------------- 1 | #ifndef INDEX_BUFFER_H_ 2 | #define INDEX_BUFFER_H_ 3 | 4 | /** 5 | * Wrapper around OpenGL index buffer. An index buffer indexes into 6 | * a vertex buffer, and prevents storage duplication when using the 7 | * same vertex multiple times. 8 | */ 9 | class IndexBuffer 10 | { 11 | public: 12 | IndexBuffer(const unsigned int *data, unsigned int count); 13 | ~IndexBuffer(); 14 | 15 | void bind() const; 16 | void unbind() const; 17 | 18 | inline unsigned int getCount() const { return m_count; } 19 | 20 | private: 21 | unsigned int m_id; 22 | unsigned int m_count; 23 | }; 24 | 25 | #endif /* INDEX_BUFFER_H_ */ 26 | -------------------------------------------------------------------------------- /src/renderer/QuadCoords.h: -------------------------------------------------------------------------------- 1 | #ifndef QUAD_COORDS_H_ 2 | #define QUAD_COORDS_H_ 3 | #include 4 | 5 | /** 6 | * Location coordinates for the four corner points of a quad. 7 | */ 8 | struct QuadCoords { 9 | glm::vec2 BottomLeft = {-0.5f, -0.5f}; 10 | glm::vec2 BottomRight = {0.5, -0.5f}; 11 | glm::vec2 TopRight = {0.5f, 0.5f}; 12 | glm::vec2 TopLeft = {-0.5f, 0.5f}; 13 | 14 | QuadCoords() {} 15 | QuadCoords(glm::vec2 bottomLeft, glm::vec2 bottomRight, glm::vec2 topRight, glm::vec2 topLeft) : 16 | BottomLeft(bottomLeft), BottomRight(bottomRight), TopRight(topRight), TopLeft(topLeft) {} 17 | QuadCoords(const QuadCoords& quadCoords) : 18 | BottomLeft(quadCoords.BottomLeft), BottomRight(quadCoords.BottomRight), 19 | TopRight(quadCoords.TopRight), TopLeft(quadCoords.TopLeft) {} 20 | 21 | void operator*=(const float multiplier) 22 | { 23 | this->BottomLeft *= multiplier; 24 | this->BottomRight *= multiplier; 25 | this->TopRight *= multiplier; 26 | this->TopLeft *= multiplier; 27 | } 28 | 29 | QuadCoords operator*(const float multiplier) 30 | { 31 | QuadCoords quadCoords = 32 | { 33 | this->BottomLeft * multiplier, 34 | this->BottomRight * multiplier, 35 | this->TopRight * multiplier, 36 | this->TopLeft * multiplier 37 | }; 38 | return quadCoords; 39 | } 40 | }; 41 | 42 | #endif /* QUAD_COORDS_H_ */ 43 | -------------------------------------------------------------------------------- /src/renderer/Renderer.h: -------------------------------------------------------------------------------- 1 | #ifndef RENDERER_H_ 2 | #define RENDERER_H_ 3 | 4 | #include 5 | 6 | class VertexArray; 7 | class IndexBuffer; 8 | class Shader; 9 | class Texture; 10 | struct TexCoords; 11 | struct QuadCoords; 12 | 13 | /** 14 | * Main renderer class, which brings together all the other OpenGL wrappers to 15 | * produce OpenGL draw calls from simple arguments (position, size, rotation, etc.) 16 | */ 17 | class Renderer 18 | { 19 | public: 20 | /** Must be called before calling any other function */ 21 | static void init(); 22 | static void destroy(); 23 | static void clear(const glm::vec4 &color); 24 | static void setViewport(int x, int y, int width, int height); 25 | static void setCameraPosition(const glm::vec2 &position, float zoomFactor); 26 | static float getPixelScaleFactor(); 27 | static void drawLine(const glm::vec2 &start, const glm::vec2 &end, float width, const glm::vec4 &color); 28 | static void drawRect(const glm::vec2 &position, const glm::vec2 &size, float rotation, const glm::vec4 &color); 29 | static void drawRect(const glm::vec2 &position, const glm::vec2 &size, float rotation, const Texture &texture, 30 | const TexCoords *texCoords = nullptr); 31 | static void drawQuad(const QuadCoords &quadCoords, const glm::vec4 &color); 32 | static void drawQuad(glm::vec2 start, glm::vec2 end); 33 | static void drawCircle(const glm::vec2 &position, float radius, const glm::vec4 &color); 34 | }; 35 | 36 | #endif /* RENDERER_H_ */ 37 | -------------------------------------------------------------------------------- /src/renderer/Scalebar.cpp: -------------------------------------------------------------------------------- 1 | #include "Scalebar.h" 2 | #include "Renderer.h" 3 | #include "Camera.h" 4 | 5 | 6 | Scalebar::ScalebarRenderable::ScalebarRenderable(std::string textureName, float width, float height) : 7 | texture(textureName), 8 | width(width), 9 | height(height) 10 | { 11 | } 12 | 13 | Scalebar::Scalebar() : 14 | m_scalebar4m("scalebar4m.png", 4.0f, 0.18f), 15 | m_scalebar2m("scalebar2m.png", 2.0f, 0.09f), 16 | m_scalebar1m("scalebar1m.png", 1.0f, 0.045f), 17 | m_scalebar50cm("scalebar50cm.png", 0.50f, 0.0225f), 18 | m_scalebar25cm("scalebar25cm.png", 0.25f, 0.01125f) 19 | { 20 | } 21 | 22 | Scalebar::~Scalebar() 23 | { 24 | } 25 | 26 | void Scalebar::render() 27 | { 28 | const float pixelScaleFactor = Renderer::getPixelScaleFactor(); 29 | const glm::vec2 cameraPosition = Camera::getPosition(); 30 | const float zoomFactor = Camera::getZoomFactor(); 31 | const glm::vec2 windowSize = Camera::getWindowSize(); 32 | 33 | const ScalebarRenderable *scalebar = nullptr; 34 | if (zoomFactor < 0.25f) { 35 | scalebar = &m_scalebar4m; 36 | } else if (zoomFactor < 0.5f) { 37 | scalebar = &m_scalebar2m; 38 | } else if (zoomFactor < 1.0f) { 39 | scalebar = &m_scalebar1m; 40 | } else if (zoomFactor < 2.0f) { 41 | scalebar = &m_scalebar50cm; 42 | } else { 43 | scalebar = &m_scalebar25cm; 44 | } 45 | const float extraBottomMargin = 0.1f * scalebar->height; 46 | const float bottomMargin = (pixelScaleFactor * zoomFactor) * ((scalebar->height / 2.0f) + extraBottomMargin); 47 | const float positionY = bottomMargin - cameraPosition.y; 48 | const float positionX = (windowSize.x / 2.0f) - cameraPosition.x; 49 | 50 | glm::vec2 drawPosition = glm::vec2{positionX, positionY} / (pixelScaleFactor * zoomFactor); 51 | glm::vec2 drawSize { scalebar->width, scalebar->height }; 52 | 53 | if ((drawSize.x * zoomFactor * pixelScaleFactor) > windowSize.x) { 54 | /* Hide if scalebar is larger than window */ 55 | return; 56 | } 57 | 58 | Renderer::drawRect(drawPosition, drawSize, 0.0f, scalebar->texture); 59 | } 60 | -------------------------------------------------------------------------------- /src/renderer/Scalebar.h: -------------------------------------------------------------------------------- 1 | #ifndef SCALEBAR_H_ 2 | #define SCALEBAR_H_ 3 | 4 | #include "Texture.h" 5 | 6 | /** 7 | * GUI-overlay at the bottom, which displays the scale. It adjusts to 8 | * the camera zoom factor. 9 | */ 10 | class Scalebar 11 | { 12 | public: 13 | Scalebar(); 14 | ~Scalebar(); 15 | void render(); 16 | 17 | private: 18 | struct ScalebarRenderable 19 | { 20 | ScalebarRenderable(std::string textureName, float width, float height); 21 | Texture texture; 22 | float width; 23 | float height; 24 | }; 25 | 26 | const ScalebarRenderable m_scalebar4m; 27 | const ScalebarRenderable m_scalebar2m; 28 | const ScalebarRenderable m_scalebar1m; 29 | const ScalebarRenderable m_scalebar50cm; 30 | const ScalebarRenderable m_scalebar25cm; 31 | }; 32 | 33 | #endif /* SCALEBAR_H */ 34 | -------------------------------------------------------------------------------- /src/renderer/Shader.cpp: -------------------------------------------------------------------------------- 1 | #include "Shader.h" 2 | #include 3 | #include "GLError.h" 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | /** 12 | * Store the shaders in the executable instead of separate files to avoid 13 | * locating them at runtime. 14 | */ 15 | namespace { 16 | const char textureVertexShader[] = R"glsl( 17 | #version 330 core 18 | 19 | layout(location = 0) in vec4 position; 20 | layout(location = 1) in vec2 texCoord; 21 | 22 | out vec2 v_texCoord; 23 | 24 | uniform mat4 u_mvpMatrix; 25 | 26 | void main() 27 | { 28 | gl_Position = u_mvpMatrix * position; 29 | v_texCoord = texCoord; 30 | }; 31 | )glsl"; 32 | 33 | const char textureFragmentShader[] = R"glsl( 34 | #version 330 core 35 | 36 | layout(location = 0) out vec4 color; 37 | 38 | in vec2 v_texCoord; 39 | 40 | uniform sampler2D u_Texture; 41 | 42 | void main() 43 | { 44 | vec4 texColor = texture(u_Texture, v_texCoord); 45 | color = texColor; 46 | }; 47 | )glsl"; 48 | 49 | const char solidColorVertexShader[] = R"glsl( 50 | #version 330 core 51 | 52 | layout(location = 0) in vec4 position; 53 | 54 | uniform mat4 u_mvpMatrix; 55 | 56 | void main() 57 | { 58 | gl_Position = u_mvpMatrix * position; 59 | }; 60 | )glsl"; 61 | 62 | const char solidColorFragmentShader[] = R"glsl( 63 | #version 330 core 64 | 65 | layout(location = 0) out vec4 color; 66 | 67 | uniform vec4 u_Color; 68 | 69 | void main() 70 | { 71 | color = u_Color; 72 | }; 73 | )glsl"; 74 | } 75 | 76 | struct ShaderProgramSource 77 | { 78 | std::string vertexSource; 79 | std::string fragmentSource; 80 | }; 81 | 82 | Shader::Shader(Shader::Program shaderProgram) 83 | { 84 | ShaderProgramSource source; 85 | switch (shaderProgram) 86 | { 87 | case Shader::Program::SolidColor: 88 | source = { solidColorVertexShader, solidColorFragmentShader }; 89 | break; 90 | case Shader::Program::Texture: 91 | source = { textureVertexShader, textureFragmentShader }; 92 | break; 93 | } 94 | m_id = create(source.vertexSource, source.fragmentSource); 95 | } 96 | 97 | Shader::~Shader() 98 | { 99 | GLCall(glDeleteProgram(m_id)); 100 | } 101 | 102 | void Shader::bind() const 103 | { 104 | GLCall(glUseProgram(m_id)); 105 | } 106 | 107 | void Shader::unbind() const 108 | { 109 | GLCall(glUseProgram(0)); 110 | } 111 | 112 | void Shader::setUniform1i(const std::string &name, int value) 113 | { 114 | GLCall(glUniform1i(getUniformLocation(name), value)); 115 | } 116 | 117 | void Shader::setUniform4f(const std::string &name, const glm::vec4 &color) 118 | { 119 | GLCall(glUniform4f(getUniformLocation(name), color[0], color[1], color[2], color[3])); 120 | } 121 | 122 | void Shader::setUniformMat4f(const std::string &name, const glm::mat4 &matrix) 123 | { 124 | GLCall(glUniformMatrix4fv(getUniformLocation(name), 1, GL_FALSE, &matrix[0][0])); 125 | } 126 | 127 | int Shader::getUniformLocation(const std::string &name) 128 | { 129 | if (m_uniformLocationCache.find(name) != m_uniformLocationCache.end()) { 130 | return m_uniformLocationCache[name]; 131 | } 132 | GLCall(int location = glGetUniformLocation(m_id, name.c_str())); 133 | if (location == -1) { 134 | std::cout << "Warning: Uniform doesn't exist (" << name << ")" << std::endl; 135 | } 136 | m_uniformLocationCache[name] = location; 137 | return location; 138 | } 139 | 140 | unsigned int Shader::compile(unsigned int type, const std::string &source) 141 | { 142 | unsigned int id = glCreateShader(type); 143 | const char* src = source.c_str(); 144 | glShaderSource(id, 1, &src, nullptr); 145 | glCompileShader(id); 146 | 147 | int result; 148 | glGetShaderiv(id, GL_COMPILE_STATUS, &result); 149 | if (result == GL_FALSE) { 150 | int length; 151 | glGetShaderiv(id, GL_INFO_LOG_LENGTH, &length); 152 | char* message = (char*)alloca(length * sizeof(char)); 153 | glGetShaderInfoLog(id, length, &length, message); 154 | std::cout << "Failed to compile " << (type == GL_VERTEX_SHADER ? "vertex" : "fragment") << std::endl; 155 | std::cout << message << std::endl; 156 | glDeleteShader(id); 157 | return 0; 158 | } 159 | 160 | return id; 161 | } 162 | 163 | unsigned int Shader::create(const std::string &vertexShader, const std::string &fragmentShader) 164 | { 165 | unsigned int program = glCreateProgram(); 166 | /* Vertex shader specifiy where the vertex should be rendered (runs for each vertex) */ 167 | unsigned int vs = compile(GL_VERTEX_SHADER, vertexShader); 168 | /* Fragment shader runs for each pixel */ 169 | unsigned int fs = compile(GL_FRAGMENT_SHADER, fragmentShader); 170 | 171 | GLCall(glAttachShader(program, vs)); 172 | GLCall(glAttachShader(program, fs)); 173 | GLCall(glLinkProgram(program)); 174 | GLCall(glValidateProgram(program)); 175 | 176 | GLCall(glDeleteShader(vs)); 177 | GLCall(glDeleteShader(fs)); 178 | return program; 179 | } 180 | -------------------------------------------------------------------------------- /src/renderer/Shader.h: -------------------------------------------------------------------------------- 1 | #ifndef SHADER_H_ 2 | #define SHADER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | struct ShaderProgramSource; 9 | 10 | /** 11 | * Wrapper around OpenGL shader handling. 12 | * Loads and compiles a shader (GPU) program. 13 | */ 14 | class Shader 15 | { 16 | public: 17 | enum class Program { SolidColor, Texture }; 18 | Shader(Program shaderProgram); 19 | ~Shader(); 20 | void bind() const; 21 | void unbind() const; 22 | void setUniform1i(const std::string& name, int value); 23 | void setUniform4f(const std::string& name, const glm::vec4& color); 24 | void setUniformMat4f(const std::string& name, const glm::mat4& matrix); 25 | 26 | private: 27 | std::string m_filepath; 28 | unsigned int m_id; 29 | std::unordered_map m_uniformLocationCache; 30 | 31 | unsigned int compile(unsigned int type, const std::string& source); 32 | unsigned int create(const std::string& vertexShader, const std::string& fragmentShader); 33 | int getUniformLocation(const std::string& name); 34 | }; 35 | #endif /* SHADER_H_ */ 36 | -------------------------------------------------------------------------------- /src/renderer/SpriteAnimation.cpp: -------------------------------------------------------------------------------- 1 | #include "SpriteAnimation.h" 2 | 3 | namespace { 4 | float maxFloatRoundingError = 0.001f; 5 | } 6 | 7 | SpriteAnimation::SpriteAnimation(unsigned int spriteSheetWidth, unsigned int spriteSheetHeight, unsigned int spriteCount, 8 | unsigned int framesBetweenUpdates, SpriteAnimation::Direction animationDirection) : 9 | m_spriteSheetWidth(spriteSheetWidth), 10 | m_spriteSheetHeight(spriteSheetHeight), 11 | m_spriteCount(spriteCount), 12 | m_framesBetweenUpdates(framesBetweenUpdates), 13 | m_spriteWidth(1.0f / m_spriteSheetWidth), 14 | m_spriteHeight(1.0f / m_spriteSheetHeight), 15 | m_animationDirection(animationDirection) 16 | { 17 | if (animationDirection == SpriteAnimation::Direction::Backward) { 18 | m_currentSpriteIndex = m_spriteCount - 1; 19 | } 20 | } 21 | 22 | SpriteAnimation::SpriteAnimation(const SpriteAnimation::Params ¶ms) : 23 | SpriteAnimation(params.spriteSheetWidth, params.spriteSheetHeight, params.spriteCount, 24 | params.framesBetweenUpdates, params.direction) 25 | { 26 | } 27 | 28 | SpriteAnimation::~SpriteAnimation() 29 | { 30 | } 31 | 32 | void SpriteAnimation::stop() 33 | { 34 | m_stopped = true; 35 | } 36 | 37 | void SpriteAnimation::setDirection(Direction animationDirection) 38 | { 39 | m_animationDirection = animationDirection; 40 | } 41 | 42 | void SpriteAnimation::setFramesBetweenUpdates(unsigned int framesBetweenUpdates) 43 | { 44 | m_stopped = false; 45 | m_framesBetweenUpdates = framesBetweenUpdates; 46 | } 47 | 48 | void SpriteAnimation::onFixedUpdate() 49 | { 50 | if (m_stopped) { 51 | return; 52 | } 53 | if (m_framesSinceLastUpdate != m_framesBetweenUpdates) { 54 | m_framesSinceLastUpdate++; 55 | return; 56 | } 57 | if (m_animationDirection == SpriteAnimation::Direction::Forward) { 58 | m_currentSpriteIndex = (m_currentSpriteIndex + 1) % m_spriteCount; 59 | } else { 60 | if (m_currentSpriteIndex == 0) { 61 | m_currentSpriteIndex = m_spriteCount - 1; 62 | } else { 63 | m_currentSpriteIndex--; 64 | } 65 | } 66 | m_framesSinceLastUpdate = 0; 67 | } 68 | 69 | void SpriteAnimation::computeTexCoords(TexCoords &texCoords) const 70 | { 71 | texCoords.TopLeft.x = (m_currentSpriteIndex % m_spriteSheetWidth) * m_spriteWidth; 72 | texCoords.TopLeft.y = 1.0f - (m_currentSpriteIndex / m_spriteSheetWidth) * m_spriteHeight; 73 | if (texCoords.TopLeft.y < 0.0f) 74 | { 75 | assert(texCoords.TopLeft.y >= -maxFloatRoundingError); 76 | texCoords.TopLeft.y = 0.0f; 77 | } 78 | texCoords.TopRight.x = texCoords.TopLeft.x + m_spriteWidth; 79 | texCoords.TopRight.y = texCoords.TopLeft.y; 80 | texCoords.BottomLeft.x = texCoords.TopLeft.x; 81 | texCoords.BottomLeft.y = texCoords.TopLeft.y - m_spriteHeight; 82 | if (texCoords.BottomLeft.y < 0.0f) 83 | { 84 | assert(texCoords.BottomLeft.y >= -maxFloatRoundingError); 85 | texCoords.BottomLeft.y = 0.0f; 86 | } 87 | texCoords.BottomRight.x = texCoords.TopLeft.x + m_spriteWidth; 88 | texCoords.BottomRight.y = texCoords.BottomLeft.y; 89 | } 90 | -------------------------------------------------------------------------------- /src/renderer/SpriteAnimation.h: -------------------------------------------------------------------------------- 1 | #ifndef SPRITE_ANIMATION_H_ 2 | #define SPRITE_ANIMATION_H_ 3 | 4 | #include "TexCoords.h" 5 | 6 | /** 7 | * Provides animation based on a spritesheet texture. It animates by showing a portion 8 | * of the spritesheet (a single sprite) at a time and changing the texture coordinates 9 | * based on the specified update frequency. 10 | */ 11 | class SpriteAnimation { 12 | public: 13 | enum class Direction { Forward, Backward }; 14 | struct Params { 15 | unsigned int spriteSheetWidth = 0; 16 | unsigned int spriteSheetHeight = 0; 17 | unsigned int spriteCount = 0; 18 | unsigned int framesBetweenUpdates = 0; 19 | Direction direction = Direction::Forward; 20 | }; 21 | /** 22 | * NOTE: Certain values of spriteCountAxis (spritesheet row/column) lead to small rounding 23 | * errors of the texture coordinates, which results in a slighty miss-aligned sprite. */ 24 | SpriteAnimation(unsigned int spriteSheetWidth, unsigned int spriteSheetHeight, unsigned int spriteCount, 25 | unsigned int framesBetweenUpdates, Direction animationDirection = Direction::Forward); 26 | SpriteAnimation(const Params ¶ms); 27 | ~SpriteAnimation(); 28 | void setFramesBetweenUpdates(unsigned int framesBetweenUpdates); 29 | void setDirection(Direction animationDirection); 30 | void stop(); 31 | void onFixedUpdate(); 32 | void getTexCoords(TexCoords &texCoords) const; 33 | void computeTexCoords(TexCoords &texCoords) const; 34 | 35 | private: 36 | unsigned int m_spriteSheetWidth = 1; 37 | unsigned int m_spriteSheetHeight = 1; 38 | const unsigned int m_spriteCount = 1; 39 | unsigned int m_currentSpriteIndex = 1; 40 | unsigned int m_framesBetweenUpdates = 10; 41 | unsigned int m_framesSinceLastUpdate = 0; 42 | const float m_spriteWidth = 1.0f; 43 | const float m_spriteHeight = 1.0f; 44 | TexCoords m_texCoords; 45 | Direction m_animationDirection = Direction::Forward; 46 | bool m_stopped = false; 47 | }; 48 | 49 | #endif /* SPRITE_ANIMATION_H_ */ 50 | -------------------------------------------------------------------------------- /src/renderer/TexCoords.h: -------------------------------------------------------------------------------- 1 | #ifndef TEX_COORDS_H_ 2 | #define TEX_COORDS_H_ 3 | #include 4 | 5 | /** 6 | * Texture coordinates determine how a texture is rendered relative to 7 | * its corresponding location coordinates in the vertex buffer. 8 | */ 9 | struct TexCoords { 10 | static constexpr float texCoordMax = 1.0f; 11 | static constexpr float texCoordMin = 0.0f; 12 | 13 | glm::vec2 BottomLeft = {0.0f, 0.0f}; 14 | glm::vec2 BottomRight = {1.0f, 0.0f}; 15 | glm::vec2 TopRight = {1.0f, 1.0f}; 16 | glm::vec2 TopLeft = {0.0f, 1.0f}; 17 | 18 | void assertLimits() const { 19 | assertLimit(BottomLeft); 20 | assertLimit(BottomRight); 21 | assertLimit(TopRight); 22 | assertLimit(TopLeft); 23 | } 24 | 25 | static void assertLimit(const glm::vec2 &vec) { 26 | assert(texCoordMin <= vec.x && vec.x <= texCoordMax); 27 | assert(texCoordMin <= vec.y && vec.y <= texCoordMax); 28 | } 29 | 30 | void operator=(const TexCoords &texCoords) { 31 | this->BottomLeft = texCoords.BottomLeft; 32 | this->BottomRight = texCoords.BottomRight; 33 | this->TopRight = texCoords.TopRight; 34 | this->TopLeft = texCoords.TopLeft; 35 | this->assertLimits(); 36 | } 37 | }; 38 | 39 | #endif /* TEX_COORDS_H_ */ 40 | -------------------------------------------------------------------------------- /src/renderer/Texture.cpp: -------------------------------------------------------------------------------- 1 | #include "Texture.h" 2 | #include "GLError.h" 3 | #include "AssetsHelper.h" 4 | 5 | #include 6 | #include "stb_image.h" 7 | #include 8 | #include 9 | 10 | Texture::Texture(const std::string& textureName) : 11 | m_filepath(AssetsHelper::getTexturePath(textureName)) 12 | { 13 | /* Flips texture upside down because OpenGl expects pixels to 14 | start at the bottom left */ 15 | stbi_set_flip_vertically_on_load(1); 16 | /* 4 channels since RBGA */ 17 | m_localBuffer = stbi_load(m_filepath.c_str(), &m_width, &m_height, &m_bpp, 4); 18 | 19 | if (m_localBuffer == nullptr) { 20 | std::cout << "Could not find texture: " << m_filepath << std::endl; 21 | assert(m_localBuffer != nullptr); 22 | } 23 | 24 | GLCall(glGenTextures(1, &m_id)); 25 | GLCall(glBindTexture(GL_TEXTURE_2D, m_id)); 26 | 27 | /* Resampling/scaling paramters filter etc... 28 | * You MUST specify this otherwise you get black texture.. */ 29 | GLCall(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR)); 30 | GLCall(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR)); 31 | GLCall(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE)); 32 | GLCall(glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE)); 33 | 34 | /* Send the texture to OpenGl (allocate space on GPU) 35 | * First format is how OpenGL stores it (GL_RGBA8) 36 | * Second format is the format of the data we supply (GL_RGBA) */ 37 | GLCall(glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, m_width, m_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, m_localBuffer)); 38 | unbind(); 39 | 40 | if (m_localBuffer) { 41 | stbi_image_free(m_localBuffer); 42 | } 43 | } 44 | 45 | Texture::~Texture() 46 | { 47 | GLCall(glDeleteTextures(1, &m_id)); 48 | } 49 | 50 | void Texture::bind(unsigned int slot) const 51 | { 52 | /* Select which texture slot to use */ 53 | GLCall(glActiveTexture(GL_TEXTURE0 + slot)); 54 | /* Bind texture to that slot */ 55 | GLCall(glBindTexture(GL_TEXTURE_2D, m_id)); 56 | } 57 | 58 | void Texture::unbind() const 59 | { 60 | GLCall(glBindTexture(GL_TEXTURE_2D, 0)); 61 | } 62 | -------------------------------------------------------------------------------- /src/renderer/Texture.h: -------------------------------------------------------------------------------- 1 | #ifndef TEXTURE_H_ 2 | #define TEXTURE_H_ 3 | #include 4 | 5 | /** 6 | * Loads a texture (image file) from a given path. 7 | * It depends on stb_image. 8 | */ 9 | class Texture 10 | { 11 | public: 12 | Texture(const std::string& textureName); 13 | ~Texture(); 14 | 15 | void bind(unsigned int slot = 0) const; 16 | void unbind() const; 17 | 18 | inline int getWidth() const { return m_width; } 19 | inline int getHeight() const { return m_height; } 20 | 21 | private: 22 | unsigned int m_id = 0; 23 | std::string m_filepath = nullptr; 24 | unsigned char* m_localBuffer = nullptr; 25 | int m_width = 0; 26 | int m_height = 0; 27 | int m_bpp = 0; 28 | 29 | }; 30 | #endif /* TEXTURE_H_ */ 31 | -------------------------------------------------------------------------------- /src/renderer/VertexArray.cpp: -------------------------------------------------------------------------------- 1 | #include "VertexArray.h" 2 | #include 3 | #include "GLError.h" 4 | #include "VertexBuffer.h" 5 | #include "VertexBufferLayout.h" 6 | 7 | VertexArray::VertexArray() 8 | { 9 | GLCall(glGenVertexArrays(1, &m_id)); 10 | } 11 | 12 | VertexArray::~VertexArray() 13 | { 14 | GLCall(glDeleteVertexArrays(1, &m_id)); 15 | } 16 | 17 | /** 18 | * There are two main ways to store vertex data: 19 | * Separate: Spread the vertex across data specific buffers (e.g. position, normal, texcoord) 20 | * Interleave: Store the vertex data in a single buffer. 21 | * Here it's done the interleaving way. */ 22 | void VertexArray::addBuffer(const VertexBuffer& vertexBuffer, const VertexBufferLayout& layout) 23 | { 24 | bind(); 25 | vertexBuffer.bind(); 26 | const auto& elements = layout.getElements(); 27 | size_t offset = 0; 28 | 29 | for (unsigned int i = 0; i < elements.size(); i++) 30 | { 31 | const auto& element = elements[i]; 32 | glEnableVertexAttribArray(i); 33 | GLCall(glVertexAttribPointer(i, element.count, element.type, element.normalized, 34 | layout.getStride(), reinterpret_cast(offset))); 35 | offset += element.count * VertexBufferElement::getSizeOfType(element.type); 36 | } 37 | } 38 | 39 | void VertexArray::bind() const 40 | { 41 | GLCall(glBindVertexArray(m_id)); 42 | } 43 | 44 | void VertexArray::unbind() const 45 | { 46 | GLCall(glBindVertexArray(0)); 47 | } 48 | -------------------------------------------------------------------------------- /src/renderer/VertexArray.h: -------------------------------------------------------------------------------- 1 | #ifndef VERTEX_ARRAY_H_ 2 | #define VERTEX_ARRAY_H_ 3 | 4 | class VertexBuffer; 5 | class VertexBufferLayout; 6 | 7 | /** 8 | * Wrapper around OpenGL vertex array object (VAO). A VAO ties a VertexBuffer to 9 | * a vertex buffer layout. So instead of having to separately bind a buffer and 10 | * its layout each time, we can just bind the VAO instead. 11 | */ 12 | class VertexArray 13 | { 14 | public: 15 | VertexArray(); 16 | ~VertexArray(); 17 | 18 | void addBuffer(const VertexBuffer& vb, const VertexBufferLayout& layout); 19 | 20 | void bind() const; 21 | void unbind() const; 22 | private: 23 | unsigned int m_id; 24 | }; 25 | #endif /* VERTEX_ARRAY_H_ */ 26 | -------------------------------------------------------------------------------- /src/renderer/VertexBuffer.cpp: -------------------------------------------------------------------------------- 1 | #include "VertexBuffer.h" 2 | #include "GLError.h" 3 | #include 4 | #include 5 | 6 | VertexBuffer::VertexBuffer(const void* data, size_t size, VertexBuffer::DrawType drawType) : 7 | m_size(size), m_glDrawType(glDrawType(drawType)) 8 | { 9 | GLCall(glGenBuffers(1, &m_id)); 10 | GLCall(glBindBuffer(GL_ARRAY_BUFFER, m_id)); 11 | GLCall(glBufferData(GL_ARRAY_BUFFER, size, data, m_glDrawType)); 12 | } 13 | 14 | VertexBuffer::~VertexBuffer() 15 | { 16 | GLCall(glDeleteBuffers(1, &m_id)); 17 | } 18 | 19 | int VertexBuffer::glDrawType(VertexBuffer::DrawType drawType) 20 | { 21 | switch (drawType) 22 | { 23 | case VertexBuffer::DrawType::Static: return GL_STATIC_DRAW; 24 | case VertexBuffer::DrawType::Dynamic: return GL_DYNAMIC_DRAW; 25 | } 26 | return 0; 27 | } 28 | 29 | void VertexBuffer::updateData(const void *data, size_t size) 30 | { 31 | assert(size == m_size); 32 | bind(); 33 | GLCall(glBufferData(GL_ARRAY_BUFFER, size, data, m_glDrawType)); 34 | unbind(); 35 | } 36 | 37 | void VertexBuffer::bind() const 38 | { 39 | GLCall(glBindBuffer(GL_ARRAY_BUFFER, m_id)); 40 | } 41 | 42 | void VertexBuffer::unbind() const 43 | { 44 | GLCall(glBindBuffer(GL_ARRAY_BUFFER, 0)); 45 | } 46 | -------------------------------------------------------------------------------- /src/renderer/VertexBuffer.h: -------------------------------------------------------------------------------- 1 | #ifndef VERTEX_BUFFER_H_ 2 | #define VERTEX_BUFFER_H_ 3 | 4 | #include 5 | /** 6 | * Wrapper around OpenGL vertex buffer object (VBO). A VBO holds vertex data such as 7 | * position and texture coordinates. 8 | */ 9 | class VertexBuffer 10 | { 11 | public: 12 | enum class DrawType { Static, Dynamic }; 13 | VertexBuffer(const void* data, size_t size, DrawType drawType); 14 | ~VertexBuffer(); 15 | void updateData(const void *data, size_t size); 16 | 17 | void bind() const; 18 | void unbind() const; 19 | 20 | private: 21 | int glDrawType(DrawType drawType); 22 | 23 | unsigned int m_id; 24 | const size_t m_size; 25 | const int m_glDrawType; 26 | }; 27 | 28 | #endif /* VERTEX_BUFFER_H_ */ 29 | -------------------------------------------------------------------------------- /src/renderer/VertexBufferLayout.h: -------------------------------------------------------------------------------- 1 | #ifndef VERTEX_BUFFER_LAYOUT_H_ 2 | #define VERTEX_BUFFER_LAYOUT_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | struct VertexBufferElement 9 | { 10 | unsigned int type; 11 | unsigned int count; 12 | bool normalized; 13 | 14 | static unsigned int getSizeOfType(unsigned int type) 15 | { 16 | switch (type) 17 | { 18 | case GL_FLOAT: return 4; 19 | case GL_UNSIGNED_INT: return 4; 20 | case GL_UNSIGNED_BYTE: return 1; 21 | } 22 | assert(false); 23 | return 0; 24 | } 25 | }; 26 | 27 | /** 28 | * Wrapper around OpenGL vertex buffer layout, which specifies to OpenGL what kind of 29 | * data the vertex buffer contains. 30 | */ 31 | class VertexBufferLayout 32 | { 33 | public: 34 | VertexBufferLayout() : 35 | m_stride(0) 36 | { 37 | } 38 | 39 | template 40 | void push(unsigned int count) 41 | { 42 | static_assert(true); 43 | } 44 | 45 | inline const std::vector getElements() const { return m_elements; } 46 | inline unsigned int getStride() const { return m_stride; } 47 | 48 | private: 49 | std::vector m_elements; 50 | unsigned int m_stride; 51 | }; 52 | 53 | template<> 54 | inline void VertexBufferLayout::push(unsigned int count) 55 | { 56 | m_elements.push_back({ GL_FLOAT, count, GL_FALSE }); 57 | m_stride += count * VertexBufferElement::getSizeOfType(GL_FLOAT); 58 | } 59 | 60 | template<> 61 | inline void VertexBufferLayout::push(unsigned int count) 62 | { 63 | m_elements.push_back({ GL_UNSIGNED_BYTE, count, GL_TRUE }); 64 | m_stride += count * VertexBufferElement::getSizeOfType(GL_UNSIGNED_BYTE); 65 | } 66 | 67 | template<> 68 | inline void VertexBufferLayout::push(unsigned int count) 69 | { 70 | m_elements.push_back({ GL_UNSIGNED_INT, count, GL_FALSE }); 71 | m_stride += count * VertexBufferElement::getSizeOfType(GL_UNSIGNED_INT); 72 | } 73 | 74 | #endif /* VERTEX_BUFFER_LAYOUT_H_ */ 75 | -------------------------------------------------------------------------------- /src/renderer/components/CircleComponent.h: -------------------------------------------------------------------------------- 1 | #ifndef CIRCLE_COMPONENT_H_ 2 | #define CIRCLE_COMPONENT_H_ 3 | 4 | #include "Renderer.h" 5 | #include "components/RenderableComponent.h" 6 | #include "components/Transforms.h" 7 | 8 | #include 9 | 10 | /** 11 | * Renders a circle filled with a single color. 12 | * 13 | * It can't be used directly in a Scene, instead it must be assigned to 14 | * a Scene object to be updated each simulation iteration. 15 | */ 16 | class CircleComponent : public RenderableComponent 17 | { 18 | public: 19 | CircleComponent(const CircleTransform *transform, const glm::vec4& color) : 20 | m_transform(transform), m_color(color) { 21 | assert(transform != nullptr); 22 | } 23 | 24 | void onFixedUpdate() override { 25 | if (m_enabled == false) { 26 | return; 27 | } 28 | Renderer::drawCircle(m_transform->position, m_transform->radius, m_color); 29 | } 30 | private: 31 | const CircleTransform *const m_transform = nullptr; 32 | glm::vec4 m_color; 33 | }; 34 | 35 | #endif /* CIRCLE_COMPONENT_H_ */ 36 | -------------------------------------------------------------------------------- /src/renderer/components/HollowCircleComponent.h: -------------------------------------------------------------------------------- 1 | #ifndef HOLLOW_CIRCLE_COMPONENT_H_ 2 | #define HOLLOW_CIRCLE_COMPONENT_H_ 3 | 4 | #include "Renderer.h" 5 | #include "components/RenderableComponent.h" 6 | #include "components/Transforms.h" 7 | 8 | #include 9 | 10 | /** 11 | * Renders a filled circle with a border color. 12 | * 13 | * It can't be used directly in a Scene, instead it must be assigned to 14 | * a Scene object to be updated each simulation iteration. 15 | */ 16 | class HollowCircleComponent : public RenderableComponent 17 | { 18 | public: 19 | HollowCircleComponent(const HollowCircleTransform *transform, const glm::vec4 &fillColor, const glm::vec4 &borderColor) : 20 | m_transform(transform), m_fillColor(fillColor), m_borderColor(borderColor) { 21 | assert(transform != nullptr); 22 | } 23 | 24 | void onFixedUpdate() override { 25 | if (m_enabled == false) { 26 | return; 27 | } 28 | Renderer::drawCircle(m_transform->position, m_transform->outerRadius, m_borderColor); 29 | Renderer::drawCircle(m_transform->position, m_transform->innerRadius, m_fillColor); 30 | } 31 | private: 32 | const HollowCircleTransform *const m_transform = nullptr; 33 | const glm::vec4 m_fillColor; 34 | const glm::vec4 m_borderColor; 35 | }; 36 | 37 | #endif /* HOLLOW_CIRCLE_COMPONENT_H_ */ 38 | -------------------------------------------------------------------------------- /src/renderer/components/LineComponent.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_COMPONENT_H_ 2 | #define LINE_COMPONENT_H_ 3 | 4 | #include "Renderer.h" 5 | #include "components/RenderableComponent.h" 6 | #include "components/Transforms.h" 7 | 8 | #include 9 | 10 | /** 11 | * Renders a line from a start to an end position with a specified width. 12 | * 13 | * It can't used directly in a Scene, instead it must be assigned to 14 | * a Scene object to be updated each simulation iteration. 15 | */ 16 | class LineComponent : public RenderableComponent 17 | { 18 | public: 19 | LineComponent(const LineTransform *transform, const glm::vec4& color) : 20 | m_transform(transform), 21 | m_color(color) {} 22 | /** 23 | * Called every simulation iteration (if assigned to a Scene Object). 24 | */ 25 | void onFixedUpdate() override { 26 | if (m_enabled == false) { 27 | return; 28 | } 29 | assert(m_transform != nullptr); 30 | Renderer::drawLine(m_transform->start, m_transform->end, m_transform->width, m_color); 31 | } 32 | private: 33 | const LineTransform *const m_transform = nullptr; 34 | glm::vec4 m_color; 35 | }; 36 | 37 | #endif /* LINE_COMPONENT_H_ */ 38 | -------------------------------------------------------------------------------- /src/renderer/components/QuadComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "components/Transforms.h" 2 | #include "components/QuadComponent.h" 3 | #include "Renderer.h" 4 | 5 | QuadComponent::QuadComponent(const QuadTransform *transform, const glm::vec4 &color) : 6 | m_transform(transform), 7 | m_color(color) 8 | { 9 | assert(transform != nullptr); 10 | } 11 | 12 | QuadComponent::~QuadComponent() 13 | { 14 | } 15 | 16 | void QuadComponent::onFixedUpdate() 17 | { 18 | if (m_enabled == false) { 19 | return; 20 | } 21 | 22 | Renderer::drawQuad(m_transform->quadCoords, m_color); 23 | } 24 | -------------------------------------------------------------------------------- /src/renderer/components/QuadComponent.h: -------------------------------------------------------------------------------- 1 | #ifndef QUAD_COMPONENT_H_ 2 | #define QUAD_COMPONENT_H_ 3 | 4 | #include "components/RenderableComponent.h" 5 | #include "QuadCoords.h" 6 | 7 | #include 8 | 9 | class QuadTransform; 10 | 11 | /** 12 | * Renders a quad from four arbitrary points (QuadCoords). 13 | * 14 | * It can't be used directly in a Scene, instead it must be assigned to 15 | * a Scene object to be updated each simulation iteration. 16 | */ 17 | class QuadComponent : public RenderableComponent 18 | { 19 | public: 20 | QuadComponent(const QuadTransform *transform, const glm::vec4 &color); 21 | ~QuadComponent(); 22 | 23 | void onFixedUpdate() override; 24 | private: 25 | const QuadTransform *const m_transform = nullptr; 26 | glm::vec4 m_color; 27 | }; 28 | 29 | #endif /* QUAD_COMPONENT_H_ */ 30 | -------------------------------------------------------------------------------- /src/renderer/components/RectComponent.cpp: -------------------------------------------------------------------------------- 1 | #include "components/RectComponent.h" 2 | #include "Renderer.h" 3 | #include "components/Transforms.h" 4 | #include "TexCoords.h" 5 | #include "Texture.h" 6 | #include "SpriteAnimation.h" 7 | 8 | RectComponent::RectComponent(const RectTransform *transform, const glm::vec4& color) : 9 | m_quadTransform(transform), 10 | m_color(color) 11 | { 12 | } 13 | 14 | RectComponent::RectComponent(const RectTransform *transform, const std::string &textureName, SpriteAnimation *spriteAnimation) : 15 | m_quadTransform(transform), 16 | m_texture(std::make_unique(textureName)), 17 | m_texCoords(std::make_unique()), 18 | m_spriteAnimation(spriteAnimation) 19 | { 20 | } 21 | 22 | RectComponent::RectComponent(const CircleTransform *transform, const std::string &textureName, SpriteAnimation *spriteAnimation) : 23 | m_circleTransform(transform), 24 | m_texture(std::make_unique(textureName)), 25 | m_texCoords(std::make_unique()), 26 | m_spriteAnimation(spriteAnimation) 27 | { 28 | } 29 | 30 | RectComponent::~RectComponent() 31 | { 32 | } 33 | 34 | void RectComponent::onFixedUpdate() 35 | { 36 | if (m_enabled == false) { 37 | return; 38 | } 39 | 40 | if (m_spriteAnimation != nullptr) { 41 | m_spriteAnimation->onFixedUpdate(); 42 | m_spriteAnimation->computeTexCoords(*m_texCoords); 43 | } 44 | 45 | glm::vec2 size; 46 | glm::vec2 position; 47 | float rotation = 0.0f; 48 | if (m_quadTransform != nullptr) { 49 | size = m_quadTransform->size; 50 | position = m_quadTransform->position; 51 | rotation = m_quadTransform->rotation; 52 | } else if (m_circleTransform != nullptr) { 53 | size = glm::vec2{ 2 * m_circleTransform->radius, 2 * m_circleTransform->radius }; 54 | position = m_circleTransform->position; 55 | rotation = m_circleTransform->rotation; 56 | } else { 57 | assert(0); 58 | } 59 | 60 | if (m_texture) { 61 | Renderer::drawRect(position, size, rotation, *m_texture.get(), m_texCoords.get()); 62 | } else { 63 | Renderer::drawRect(position, size, rotation, m_color); 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /src/renderer/components/RectComponent.h: -------------------------------------------------------------------------------- 1 | #ifndef RECT_COMPONENT_H_ 2 | #define RECT_COMPONENT_H_ 3 | 4 | #include "components/RenderableComponent.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | struct TexCoords; 11 | class Texture; 12 | class SpriteAnimation; 13 | class RectTransform; 14 | class CircleTransform; 15 | 16 | /** 17 | * Renders a rectangle with single color or texture (and animation). 18 | * 19 | * It can't be used directly in a Scene, instead it must be assigned to 20 | * a Scene object to be updated each simulation iteration. 21 | */ 22 | class RectComponent : public RenderableComponent 23 | { 24 | public: 25 | RectComponent(const RectTransform *transform, const glm::vec4& color); 26 | RectComponent(const RectTransform *transform, const std::string &textureName, SpriteAnimation *spriteAnimation = nullptr); 27 | RectComponent(const CircleTransform *transform, const std::string &textureName, SpriteAnimation *spriteAnimation = nullptr); 28 | ~RectComponent(); 29 | 30 | void onFixedUpdate() override; 31 | private: 32 | const RectTransform *const m_quadTransform = nullptr; 33 | const CircleTransform *const m_circleTransform = nullptr; 34 | glm::vec4 m_color; 35 | const std::unique_ptr m_texture; 36 | const std::unique_ptr m_texCoords; 37 | SpriteAnimation *const m_spriteAnimation = nullptr; 38 | }; 39 | 40 | #endif /* RECT_COMPONENT_H_ */ 41 | -------------------------------------------------------------------------------- /src/renderer/components/RenderableComponent.h: -------------------------------------------------------------------------------- 1 | #ifndef RENDERABLE_COMPONENT_H_ 2 | #define RENDERABLE_COMPONENT_H_ 3 | 4 | #include "Component.h" 5 | #include 6 | 7 | /** 8 | * Base class for components that renders. 9 | */ 10 | class RenderableComponent : public Component 11 | { 12 | public: 13 | RenderableComponent() {} 14 | virtual ~RenderableComponent() {} 15 | /** 16 | * Called every simulation iteration (if assigned to a Scene Object). 17 | */ 18 | virtual void onFixedUpdate() = 0; 19 | /** 20 | * Enable or disable rendering. 21 | */ 22 | void setEnabled(float enabled) 23 | { 24 | m_enabled = enabled; 25 | } 26 | 27 | protected: 28 | bool m_enabled = true; 29 | }; 30 | 31 | #endif /* RENDERABLE_COMPONENT_H_ */ 32 | -------------------------------------------------------------------------------- /src/renderer/stb_image.cpp: -------------------------------------------------------------------------------- 1 | #define STB_IMAGE_IMPLEMENTATION 2 | #include "stb_image.h" 3 | -------------------------------------------------------------------------------- /src/scene/Scene.cpp: -------------------------------------------------------------------------------- 1 | #include "Scene.h" 2 | #include "SceneObject.h" 3 | #include "ImGuiMenu.h" 4 | 5 | Scene::Scene(std::string description) : 6 | m_description(description) 7 | { 8 | } 9 | 10 | Scene::Scene(std::string description, PhysicsWorld::Gravity gravity, float physicsStepTime) : 11 | m_physicsWorld(std::make_unique(gravity)), 12 | m_description(description), 13 | m_physicsStepTime(physicsStepTime), 14 | m_startTime(std::chrono::system_clock::now()) 15 | { 16 | } 17 | 18 | Scene::~Scene() 19 | { 20 | } 21 | 22 | PhysicsWorld *Scene::getPhysicsWorld() const 23 | { 24 | return m_physicsWorld.get(); 25 | } 26 | 27 | void Scene::onKeyEvent(const Event::Key &keyEvent) 28 | { 29 | for (auto obj : m_objects) { 30 | obj->onKeyEvent(keyEvent); 31 | } 32 | } 33 | 34 | void Scene::updatePhysics(float stepTime) 35 | { 36 | if (m_physicsWorld) { 37 | m_physicsWorld->step(stepTime); 38 | for (auto obj : m_objects) { 39 | obj->updatePhysics(stepTime); 40 | } 41 | } 42 | } 43 | 44 | void Scene::updateControllers(float stepTime) 45 | { 46 | for (auto obj : m_objects) { 47 | obj->updateController(stepTime); 48 | } 49 | } 50 | 51 | void Scene::sceneObjectsOnFixedUpdate() 52 | { 53 | for (auto obj : m_objects) { 54 | obj->onFixedUpdate(); 55 | } 56 | } 57 | 58 | void Scene::render() 59 | { 60 | for (auto menu : m_menus) { 61 | menu->render(); 62 | } 63 | for (auto obj : m_objects) { 64 | obj->updateRenderable(); 65 | } 66 | } 67 | 68 | void Scene::addObject(SceneObject *sceneObject) 69 | { 70 | assert(sceneObject != nullptr); 71 | assert(sceneObject->getScene() == nullptr); 72 | m_objects.push_back(sceneObject); 73 | } 74 | 75 | void Scene::removeObject(SceneObject *sceneObject) 76 | { 77 | auto itr = m_objects.begin(); 78 | while (itr != m_objects.end()) 79 | { 80 | if (sceneObject == *itr) { 81 | itr = m_objects.erase(itr); 82 | break; 83 | } 84 | itr++; 85 | } 86 | } 87 | 88 | void Scene::addMenu(ImGuiMenu *menu) 89 | { 90 | assert(menu != nullptr); 91 | m_menus.push_back(menu); 92 | } 93 | 94 | unsigned int Scene::getSecondsSinceStart() const 95 | { 96 | const auto timeNow = std::chrono::system_clock::now(); 97 | return static_cast(std::chrono::duration(timeNow - m_startTime).count()); 98 | } 99 | 100 | unsigned int Scene::getMillisecondsSinceStart() const 101 | { 102 | const auto timeNow = std::chrono::system_clock::now(); 103 | return static_cast(std::chrono::duration(timeNow - m_startTime).count()); 104 | } 105 | -------------------------------------------------------------------------------- /src/scene/Scene.h: -------------------------------------------------------------------------------- 1 | #ifndef SCENE_H_ 2 | #define SCENE_H_ 3 | 4 | #include "Event.h" 5 | #include "PhysicsWorld.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class SceneObject; 12 | class ImGuiMenu; 13 | 14 | /** 15 | * Base class for scenes. All scenes must inherit this class. A Scene provides the stage 16 | * for the simulation and has a PhysicsWorld and a list of SceneObject. Only one Scene 17 | * can be active at a time. 18 | */ 19 | class Scene 20 | { 21 | public: 22 | Scene(std::string description); 23 | /** 24 | * \param physicsStepTime Determines how many times per second the physics and logic are updated. 25 | * Note, to avoid jittery behaviour, make this at least twice as large as the rendering rate 26 | * (monitor's refresh rate). 27 | */ 28 | Scene(std::string description, PhysicsWorld::Gravity gravity, float physicsStepTime = 0.001f); 29 | virtual ~Scene(); 30 | PhysicsWorld *getPhysicsWorld() const; 31 | void updatePhysics(float stepTime); 32 | void updateControllers(float stepTime); 33 | void sceneObjectsOnFixedUpdate(); 34 | void render(); 35 | void onKeyEvent(const Event::Key &keyEvent); 36 | void addObject(SceneObject *sceneObject); 37 | void removeObject(SceneObject *sceneObject); 38 | virtual void onFixedUpdate() {}; 39 | void addMenu(ImGuiMenu *menu); 40 | std::string getDescription() const { return m_description; } 41 | unsigned int getSecondsSinceStart() const; 42 | unsigned int getMillisecondsSinceStart() const; 43 | float getPhysicsStepTime() const { return m_physicsStepTime; } 44 | 45 | protected: 46 | std::unique_ptr m_physicsWorld; 47 | 48 | private: 49 | std::vector m_objects; 50 | std::vector m_menus; 51 | std::string m_description; 52 | float m_physicsStepTime = 0.001f; 53 | const std::chrono::time_point m_startTime; 54 | }; 55 | 56 | #endif /* SCENE_H_ */ 57 | -------------------------------------------------------------------------------- /src/scene/SceneMenu.cpp: -------------------------------------------------------------------------------- 1 | #include "SceneMenu.h" 2 | #include "ImGuiOverlay.h" 3 | #include "Camera.h" 4 | #include "Application.h" 5 | 6 | SceneMenu::SceneMenu(Scene*& scene) : 7 | m_currentScene(scene) 8 | { 9 | } 10 | 11 | SceneMenu::~SceneMenu() 12 | { 13 | if (m_currentScene) { 14 | delete m_currentScene; 15 | } 16 | } 17 | 18 | void SceneMenu::setFps(unsigned int fps) 19 | { 20 | m_fps = fps; 21 | } 22 | 23 | void SceneMenu::setAvgPhysicsSteps(unsigned int avgPhysicsSteps) 24 | { 25 | m_avgPhysicsSteps = avgPhysicsSteps; 26 | } 27 | 28 | void SceneMenu::setWarningMessage(std::string message) 29 | { 30 | m_warningMessage = message; 31 | } 32 | 33 | void SceneMenu::setCurrentScene(std::string sceneName) 34 | { 35 | for (auto &scene : m_scenes) { 36 | if (scene.first == sceneName) { 37 | m_currentScene = scene.second(); 38 | Camera::reset(); 39 | } 40 | } 41 | } 42 | 43 | void SceneMenu::render() 44 | { 45 | ImGuiOverlay::begin("Scene menu", 15.0f, 15.0f, 230.0f, 450.0f); 46 | for (auto& scene : m_scenes) 47 | { 48 | if (ImGuiOverlay::button(scene.first.c_str())) { 49 | delete m_currentScene; 50 | m_currentScene = scene.second(); 51 | Camera::reset(); 52 | } 53 | } 54 | ImGuiOverlay::text(""); 55 | if (m_currentScene != nullptr) { 56 | ImGuiOverlay::text("Scene: " + m_currentScene->getDescription()); 57 | } 58 | if (!m_warningMessage.empty()) 59 | { 60 | ImGuiOverlay::text("Warning: " + m_warningMessage); 61 | } 62 | ImGuiOverlay::text("FPS: " + std::to_string(m_fps) + " Hz"); 63 | if (m_avgPhysicsSteps) 64 | { 65 | ImGuiOverlay::text("Physics step rate: " + std::to_string(m_avgPhysicsSteps) + " Hz"); 66 | } else { 67 | ImGuiOverlay::text("Physics step rate: "); 68 | } 69 | ImGuiOverlay::text(""); 70 | ImGuiOverlay::text("Move camera up "); 71 | ImGuiOverlay::text("Move camera left "); 72 | ImGuiOverlay::text("Move camera down "); 73 | ImGuiOverlay::text("Move camera right "); 74 | ImGuiOverlay::text("Zoom camera "); 75 | ImGuiOverlay::text("Reset camera "); 76 | ImGuiOverlay::end(); 77 | } 78 | -------------------------------------------------------------------------------- /src/scene/SceneMenu.h: -------------------------------------------------------------------------------- 1 | #ifndef SCENE_MENU_H_ 2 | #define SCENE_MENU_H_ 3 | 4 | #include "Scene.h" 5 | #include 6 | #include 7 | 8 | class Application; 9 | 10 | /** 11 | * A GUI sidebar menu that provides a list of selectable scenes and keeps 12 | * track of the current Scene. It also displays useful simulation info and statistics. 13 | */ 14 | class SceneMenu 15 | { 16 | public: 17 | SceneMenu(Scene*& scene); 18 | ~SceneMenu(); 19 | 20 | void render(); 21 | 22 | template 23 | void registerScene(const std::string& name) 24 | { 25 | m_scenes.push_back(std::make_pair(name, []() { return new T(); })); 26 | } 27 | void setCurrentScene(std::string sceneName); 28 | void setFps(unsigned int fps); 29 | void setAvgPhysicsSteps(unsigned int avgPhysicsSteps); 30 | void setWarningMessage(std::string message); 31 | private: 32 | Scene*& m_currentScene; 33 | std::vector>> m_scenes; 34 | unsigned int m_fps = 0; 35 | unsigned int m_avgPhysicsSteps = 0; 36 | std::string m_warningMessage; 37 | }; 38 | 39 | #endif /* SCENE_MENU_H_ */ 40 | -------------------------------------------------------------------------------- /src/scene/SceneObject.cpp: -------------------------------------------------------------------------------- 1 | #include "SceneObject.h" 2 | #include "Scene.h" 3 | #include "components/RenderableComponent.h" 4 | #include "components/PhysicsComponent.h" 5 | #include "components/ControllerComponent.h" 6 | #include "components/Transforms.h" 7 | 8 | #include 9 | 10 | SceneObject::SceneObject(Scene *scene) 11 | { 12 | assert(scene != nullptr); 13 | scene->addObject(this); 14 | m_scene = scene; 15 | m_physicsWorld = scene->getPhysicsWorld(); 16 | } 17 | 18 | SceneObject::~SceneObject() 19 | { 20 | m_scene->removeObject(this); 21 | } 22 | 23 | void SceneObject::setController(ControllerComponent *controller) 24 | { 25 | assert(controller != nullptr); 26 | m_controllerComponent = controller; 27 | } 28 | 29 | void SceneObject::updateRenderable() 30 | { 31 | if (m_renderableComponent) { 32 | m_renderableComponent->onFixedUpdate(); 33 | } 34 | } 35 | 36 | void SceneObject::updatePhysics(float stepTime) 37 | { 38 | if (m_physicsComponent) { 39 | m_physicsComponent->onFixedUpdate(stepTime); 40 | } 41 | } 42 | 43 | void SceneObject::updateController(float stepTime) 44 | { 45 | if (m_controllerComponent) { 46 | m_controllerComponent->onFixedUpdate(stepTime); 47 | } 48 | } 49 | 50 | void SceneObject::onKeyEvent(const Event::Key &keyEvent) 51 | { 52 | if (m_controllerComponent) { 53 | m_controllerComponent->onKeyEvent(keyEvent); 54 | } 55 | } 56 | 57 | void SceneObject::onFixedUpdate() 58 | { 59 | /* Do nothing */ 60 | } 61 | -------------------------------------------------------------------------------- /src/scene/SceneObject.h: -------------------------------------------------------------------------------- 1 | #ifndef SCENE_OBJECT_H_ 2 | #define SCENE_OBJECT_H_ 3 | 4 | #include "Event.h" 5 | #include 6 | 7 | class TransformComponent; 8 | class RenderableComponent; 9 | class PhysicsComponent; 10 | class ControllerComponent; 11 | class Scene; 12 | class PhysicsWorld; 13 | 14 | /** 15 | * Base class that scene objects inherit from. A scene object is a general purpose object. It's 16 | * similar to the "entity" in an Entity-component-system (ECS) pattern. It's an object composed 17 | * of multiple components such as rendering and physics components. In contrast to a real 18 | * ECS, there is no memory optimization, e.g. similar components are not stored next to each 19 | * other for cache-friendliness. Neither are there any systems, instead the data and behaviour 20 | * is contained inside the components. 21 | * 22 | * A scene object must be part of a scene. 23 | */ 24 | class SceneObject 25 | { 26 | public: 27 | SceneObject(Scene *scene); 28 | virtual ~SceneObject(); 29 | Scene *getScene() const { return m_scene; }; 30 | void setController(ControllerComponent *controller); 31 | void updateRenderable(); 32 | void updatePhysics(float stepTime); 33 | void updateController(float stepTime); 34 | virtual void onFixedUpdate(); 35 | virtual void onKeyEvent(const Event::Key &keyEvent); 36 | 37 | protected: 38 | Scene *m_scene = nullptr; 39 | PhysicsWorld *m_physicsWorld = nullptr; 40 | std::unique_ptr m_transformComponent; 41 | std::unique_ptr m_renderableComponent; 42 | std::unique_ptr m_physicsComponent; 43 | /** Make controller a raw pointer because it's unflexible to have the scene object 44 | * own the controller. */ 45 | ControllerComponent *m_controllerComponent = nullptr; 46 | }; 47 | 48 | #endif /* SCENE_OBJECT_H_ */ 49 | -------------------------------------------------------------------------------- /src/transforms/components/Transforms.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORMS_H_ 2 | #define TRANSFORMS_H_ 3 | 4 | #include "Component.h" 5 | #include "QuadCoords.h" 6 | 7 | #include 8 | 9 | /** 10 | * The base class for Transform-components. A transform holds the data 11 | * shared between physics and rendering and can thus be seen as the 12 | * link between them. 13 | */ 14 | class TransformComponent : public Component 15 | { 16 | public: 17 | /* Add virtual destructor for polymorphism */ 18 | virtual ~TransformComponent() {}; 19 | }; 20 | 21 | class LineTransform : public TransformComponent 22 | { 23 | public: 24 | LineTransform() {} 25 | LineTransform(const glm::vec2 &start, const glm::vec2 &end, float width) : 26 | start(start), end(end), width(width) {} 27 | glm::vec2 start = {0, 0}; 28 | glm::vec2 end = {0, 0}; 29 | float width = 0.0f; 30 | }; 31 | 32 | class RectTransform : public TransformComponent 33 | { 34 | public: 35 | RectTransform(const glm::vec2 &position, const glm::vec2 &size, float rotation = 0.0f) : 36 | position(position), size(size), rotation(rotation) {} 37 | ~RectTransform() {} 38 | glm::vec2 position; 39 | glm::vec2 size; 40 | float rotation = 0.0f; 41 | }; 42 | 43 | class QuadTransform : public TransformComponent 44 | { 45 | public: 46 | QuadTransform(const QuadCoords &quadCoords) : 47 | quadCoords(quadCoords) {} 48 | ~QuadTransform() {} 49 | const QuadCoords quadCoords; 50 | }; 51 | 52 | class CircleTransform : public TransformComponent 53 | { 54 | public: 55 | CircleTransform() {} 56 | CircleTransform(const glm::vec2 &position, float radius, float rotation) : 57 | position(position), radius(radius), rotation(rotation) {} 58 | ~CircleTransform() {} 59 | glm::vec2 position { 0, 0 }; 60 | float radius = 0.0f; 61 | float rotation = 0.0f; 62 | }; 63 | 64 | class HollowCircleTransform : public TransformComponent 65 | { 66 | public: 67 | HollowCircleTransform(const glm::vec2 &position, float innerRadius, float outerRadius) : 68 | position(position), innerRadius(innerRadius), outerRadius(outerRadius) {} 69 | ~HollowCircleTransform() {} 70 | glm::vec2 position; 71 | float innerRadius = 0.0f; 72 | float outerRadius = 0.0f; 73 | }; 74 | 75 | #endif /* TRANSFORMS_H_ */ 76 | -------------------------------------------------------------------------------- /testapp/Bots2DTestApp.cpp: -------------------------------------------------------------------------------- 1 | #include "Bots2DTestApp.h" 2 | #include "SceneMenu.h" 3 | #include "DrawTestScene.h" 4 | #include "SpriteAnimationTestScene.h" 5 | #include "PhysicsTestScene.h" 6 | #include "WheelMotorTestScene.h" 7 | #include "PhysicsBotTestScene.h" 8 | #include "SumobotTestScene.h" 9 | #include "LineFollowerTestScene.h" 10 | 11 | Bots2DTestApp::Bots2DTestApp() : 12 | Application("bots2dtest") 13 | { 14 | m_sceneMenu->registerScene("DrawTest"); 15 | m_sceneMenu->registerScene("SpriteAnimationTest"); 16 | m_sceneMenu->registerScene("PhysicsTest"); 17 | m_sceneMenu->registerScene("WheelMotorTest"); 18 | m_sceneMenu->registerScene("PhysicsBotTest"); 19 | m_sceneMenu->registerScene("SumobotTest"); 20 | m_sceneMenu->registerScene("LineFollowerTest"); 21 | m_sceneMenu->setCurrentScene("SumobotTest"); 22 | } 23 | 24 | Bots2DTestApp::~Bots2DTestApp() 25 | { 26 | } 27 | 28 | void Bots2DTestApp::onKeyEvent(const Event::Key &keyEvent) 29 | { 30 | (void)keyEvent; 31 | } 32 | 33 | void Bots2DTestApp::onFixedUpdate() 34 | { 35 | } 36 | -------------------------------------------------------------------------------- /testapp/Bots2DTestApp.h: -------------------------------------------------------------------------------- 1 | #ifndef BOTS2D_TEST_APP_H_ 2 | #define BOTS2D_TEST_APP_H_ 3 | 4 | #include "Application.h" 5 | 6 | /** 7 | * Test application with test scenes. 8 | */ 9 | class Bots2DTestApp : public Application 10 | { 11 | public: 12 | Bots2DTestApp(); 13 | ~Bots2DTestApp(); 14 | void onKeyEvent(const Event::Key &keyEvent) override; 15 | void onFixedUpdate() override; 16 | }; 17 | 18 | #endif /* BOTS2D_TEST_APP_H_ */ 19 | -------------------------------------------------------------------------------- /testapp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(bots2dtest VERSION 1.0) 3 | 4 | # Modify to where the bots2d folder is stored 5 | set (BOTS2D_RELATIVE_PATH ../) 6 | 7 | set (BOTS2D_ASSETS 8 | ${BOTS2D_RELATIVE_PATH}/assets/objects/shapes/RectObject.cpp 9 | ${BOTS2D_RELATIVE_PATH}/assets/objects/shapes/QuadObject.cpp 10 | ${BOTS2D_RELATIVE_PATH}/assets/objects/shapes/CircleObject.cpp 11 | ${BOTS2D_RELATIVE_PATH}/assets/objects/shapes/LineObject.cpp 12 | ${BOTS2D_RELATIVE_PATH}/assets/objects/actuators/WheelMotor.cpp 13 | ${BOTS2D_RELATIVE_PATH}/assets/objects/bodies/SimpleBotBody.cpp 14 | ${BOTS2D_RELATIVE_PATH}/assets/objects/robots/BaseBot.cpp 15 | ${BOTS2D_RELATIVE_PATH}/assets/objects/robots/PhysicsBot.cpp 16 | ${BOTS2D_RELATIVE_PATH}/assets/objects/robots/Sumobot.cpp 17 | ${BOTS2D_RELATIVE_PATH}/assets/objects/robots/LineFollower.cpp 18 | ${BOTS2D_RELATIVE_PATH}/assets/objects/sensors/RangeSensorObject.cpp 19 | ${BOTS2D_RELATIVE_PATH}/assets/objects/sensors/LineDetectorObject.cpp 20 | ${BOTS2D_RELATIVE_PATH}/assets/objects/playgrounds/Dohyo.cpp 21 | ${BOTS2D_RELATIVE_PATH}/assets/objects/playgrounds/LineFollowerPath.cpp 22 | ) 23 | 24 | set (BOTS2D_TEST_SCENES 25 | scenes/DrawTestScene.cpp 26 | scenes/SpriteAnimationTestScene.cpp 27 | scenes/PhysicsTestScene.cpp 28 | scenes/WheelMotorTestScene.cpp 29 | scenes/PhysicsBotTestScene.cpp 30 | scenes/SumobotTestScene.cpp 31 | scenes/LineFollowerTestScene.cpp 32 | ) 33 | 34 | set (BOTS2D_TEST_CONTROLLERS 35 | controllers/NsumoController/main.c 36 | controllers/NsumoController/NsumoMicrocontroller.cpp 37 | controllers/NsumoController/motor.c 38 | controllers/NsumoController/nsumo/drive.c 39 | controllers/NsumoController/nsumo/line_detection.c 40 | controllers/NsumoController/nsumo/detection_history.c 41 | controllers/NsumoController/nsumo/state_machine/state_machine.c 42 | controllers/NsumoController/nsumo/state_machine/state_retreat.c 43 | controllers/NsumoController/nsumo/state_machine/state_attack.c 44 | controllers/NsumoController/nsumo/state_machine/state_search.c 45 | controllers/NsumoController/nsumo/enemy_detection.c 46 | controllers/NsumoController/nsumo/trace.c 47 | controllers/NsumoController/nsumo/test.c 48 | controllers/NsumoController/nsumo/timer.c 49 | ) 50 | 51 | set (BOTS2D_TEST_FILES 52 | ${BOTS2D_ASSETS} 53 | ${BOTS2D_TEST_SCENES} 54 | ${BOTS2D_TEST_CONTROLLERS} 55 | Bots2DTestApp.cpp 56 | main.cpp 57 | ) 58 | 59 | add_executable(bots2dtest 60 | ${BOTS2D_TEST_FILES} 61 | ) 62 | 63 | target_include_directories(bots2dtest PRIVATE scenes) 64 | target_include_directories(bots2dtest PRIVATE controllers/NsumoController/nsumo/) 65 | target_include_directories(bots2dtest PRIVATE controllers/NsumoController/nsumo/drivers) 66 | target_include_directories(bots2dtest PRIVATE controllers/NsumoController/nsumo/state_machine) 67 | target_include_directories(bots2dtest PRIVATE controllers) 68 | target_include_directories(bots2dtest PRIVATE ${BOTS2D_RELATIVE_PATH}/assets/objects) 69 | target_include_directories(bots2dtest PRIVATE ${BOTS2D_RELATIVE_PATH}/src/core) 70 | target_include_directories(bots2dtest PRIVATE ${BOTS2D_RELATIVE_PATH}/src/renderer) 71 | target_include_directories(bots2dtest PRIVATE ${BOTS2D_RELATIVE_PATH}/src/transforms) 72 | target_include_directories(bots2dtest PRIVATE ${BOTS2D_RELATIVE_PATH}/src/scene) 73 | target_include_directories(bots2dtest PRIVATE ${BOTS2D_RELATIVE_PATH}/src/physics) 74 | target_include_directories(bots2dtest PRIVATE ${BOTS2D_RELATIVE_PATH}/src/controllers) 75 | target_include_directories(bots2dtest PRIVATE ${BOTS2D_RELATIVE_PATH}/src/controllers/components) 76 | target_include_directories(bots2dtest PRIVATE ${BOTS2D_RELATIVE_PATH}/external/glm) 77 | 78 | # Include bots2d and build it as a static lib 79 | add_subdirectory(${BOTS2D_RELATIVE_PATH} build) 80 | target_link_libraries(bots2dtest PRIVATE bots2d) 81 | 82 | target_compile_definitions(bots2dtest PRIVATE BUILD_TEST) 83 | 84 | set_target_properties(bots2dtest PROPERTIES 85 | CXX_STANDARD 17 86 | CXX_STANDARD_REQUIRED YES 87 | CXX_EXTENSIONS NO 88 | ) 89 | 90 | if(MSVC) 91 | target_compile_options(bots2dtest PRIVATE /W4 /WX) 92 | else() 93 | target_compile_options(bots2dtest PRIVATE -Wall -Wextra -pedantic -Werror) 94 | endif() 95 | -------------------------------------------------------------------------------- /testapp/README.md: -------------------------------------------------------------------------------- 1 | # Description 2 | This folder contains a test application, which uses the Bots2D framework and its 3 | assets to simulate multiple test scenes. 4 | 5 | # Build on Linux 6 | 7 | ``` 8 | mkdir build 9 | cd build 10 | cmake .. 11 | cmake --build . 12 | run build/bots2d_testapp 13 | ``` 14 | 15 | # Build on Windows 16 | Tested with Visual Studio 2019: 17 | 18 | ``` 19 | 1. Open local folder and select bots2d/testapp 20 | 2. After VS has loaded the project, right-click on CMakeLists.txt in the solution 21 | explorer and set it as startup item. 22 | 3. Start Debugging (F5) 23 | ``` 24 | -------------------------------------------------------------------------------- /testapp/controllers/NsumoController/NsumoMicrocontroller.cpp: -------------------------------------------------------------------------------- 1 | #include "NsumoController/NsumoMicrocontroller.h" 2 | 3 | extern "C" { 4 | #include "NsumoController/main_function.h" 5 | } 6 | 7 | NsumoMicrocontroller::NsumoMicrocontroller(Microcontroller::VoltageLines &voltageLines) : 8 | CMicrocontroller(voltageLines, _main) 9 | { 10 | } 11 | -------------------------------------------------------------------------------- /testapp/controllers/NsumoController/NsumoMicrocontroller.h: -------------------------------------------------------------------------------- 1 | #ifndef NSUMO_MICROCONTROLLER_H_ 2 | #define NSUMO_MICROCONTROLLER_H_ 3 | 4 | #include "components/CMicrocontroller.h" 5 | 6 | class NsumoMicrocontroller : public CMicrocontroller 7 | { 8 | public: 9 | NsumoMicrocontroller(Microcontroller::VoltageLines &voltageLines); 10 | ~NsumoMicrocontroller() {} 11 | }; 12 | 13 | #endif /* NSUMO_MICROCONTROLLER_H_ */ 14 | -------------------------------------------------------------------------------- /testapp/controllers/NsumoController/README.md: -------------------------------------------------------------------------------- 1 | This is the controller code for simulating Nsumo, a mini-class sumobot with 2 | four-wheel drive, four line-sensors and five range sensors. The real controller 3 | code is written in C and runs on an MSP430 microcontroller, so we must use the 4 | C-bindings for simulating it. The code for the real microcontroller is included 5 | as a submodule (directory nsumo), and the simulated code uses the same high-level 6 | functions (e.g. state_machine, line_detection), which makes it very easy to 7 | switch between the simulator and the real sumobot. 8 | 9 | -------------------------------------------------------------------------------- /testapp/controllers/NsumoController/main.c: -------------------------------------------------------------------------------- 1 | #include "NsumoController/main_function.h" 2 | #include "NsumoController/nsumo/trace.h" 3 | #include "NsumoController/nsumo/state_machine/state_machine.h" 4 | #include "NsumoController/nsumo/test.h" 5 | 6 | void _main() 7 | { 8 | trace_init(); 9 | //test_enemy_detection_print(); 10 | state_machine_run(); 11 | //test_drive_duty_cycles(); 12 | //test_drive_and_line_detect(); 13 | //test_rotate_and_enemy_detect(); 14 | } 15 | -------------------------------------------------------------------------------- /testapp/controllers/NsumoController/main_function.h: -------------------------------------------------------------------------------- 1 | void _main(); 2 | -------------------------------------------------------------------------------- /testapp/controllers/NsumoController/motor.c: -------------------------------------------------------------------------------- 1 | #include "NsumoController/nsumo/drivers/motor.h" 2 | #include "microcontroller_c_bindings.h" 3 | #include "NsumoController/voltage_lines.h" 4 | #include 5 | 6 | #define VOLTAGE_PER_BATTERY_FULL_CHARGE (4.2f) 7 | #define VOLTAGE_BATTERIES_FULL_CHARGE (2 * VOLTAGE_PER_BATTERY_FULL_CHARGE) 8 | #define MAX_VOLTAGE_ALLOWED (6.0f) 9 | #define LOSS_FACTOR (0.938f) 10 | #define SCALE_FACTOR (MAX_VOLTAGE_ALLOWED / VOLTAGE_BATTERIES_FULL_CHARGE) 11 | #define MAX_VOLTAGE (SCALE_FACTOR * LOSS_FACTOR * VOLTAGE_BATTERIES_FULL_CHARGE) 12 | 13 | void motor_init() { } 14 | 15 | void motor_set_duty_cycle(motors_t motors, int16_t duty_cycle) 16 | { 17 | assert(-100 <= duty_cycle && duty_cycle <= 100); 18 | float motor_voltage = ((float)duty_cycle / 100.0f) * MAX_VOLTAGE; 19 | switch (motors) { 20 | case MOTORS_LEFT: 21 | set_voltage(VOLTAGE_FRONT_LEFT_MOTOR, motor_voltage); 22 | set_voltage(VOLTAGE_BACK_LEFT_MOTOR, motor_voltage); 23 | break; 24 | case MOTORS_RIGHT: 25 | set_voltage(VOLTAGE_FRONT_RIGHT_MOTOR, motor_voltage); 26 | set_voltage(VOLTAGE_BACK_RIGHT_MOTOR, motor_voltage); 27 | break; 28 | } 29 | } 30 | 31 | void motor_stop_safely() { } 32 | -------------------------------------------------------------------------------- /testapp/controllers/NsumoController/voltage_lines.h: -------------------------------------------------------------------------------- 1 | #ifndef VOTLAGE_LINES_H 2 | #define VOLTAGE_LINES_H 3 | 4 | /* Must match with how lines are assigned in the C++ microcontroller class */ 5 | typedef enum voltage_idx { 6 | VOLTAGE_FRONT_LEFT_MOTOR = 0, 7 | VOLTAGE_BACK_LEFT_MOTOR = 1, 8 | VOLTAGE_FRONT_RIGHT_MOTOR = 2, 9 | VOLTAGE_BACK_RIGHT_MOTOR = 3, 10 | VOLTAGE_FRONT_LEFT_LINE_DETECTOR = 4, 11 | VOLTAGE_BACK_LEFT_LINE_DETECTOR = 5, 12 | VOLTAGE_FRONT_RIGHT_LINE_DETECTOR = 6, 13 | VOLTAGE_BACK_RIGHT_LINE_DETECTOR = 7, 14 | VOLTAGE_LEFT_RANGE_SENSOR = 8, 15 | VOLTAGE_FRONT_LEFT_RANGE_SENSOR = 9, 16 | VOLTAGE_FRONT_RANGE_SENSOR = 10, 17 | VOLTAGE_FRONT_RIGHT_RANGE_SENSOR = 11, 18 | VOLTAGE_RIGHT_RANGE_SENSOR = 12, 19 | } voltage_idx_t; 20 | 21 | #endif /* VOLTAGE_LINES_H */ 22 | -------------------------------------------------------------------------------- /testapp/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Bots2DTestApp.h" 2 | 3 | int main() 4 | { 5 | Bots2DTestApp app; 6 | app.run(); 7 | } 8 | -------------------------------------------------------------------------------- /testapp/scenes/DrawTestScene.cpp: -------------------------------------------------------------------------------- 1 | #include "DrawTestScene.h" 2 | #include "shapes/RectObject.h" 3 | #include "shapes/QuadObject.h" 4 | #include "shapes/CircleObject.h" 5 | #include "shapes/LineObject.h" 6 | #include "QuadCoords.h" 7 | 8 | #include 9 | 10 | #include "Renderer.h" 11 | 12 | DrawTestScene::DrawTestScene() : 13 | Scene("Test draw basic shapes") 14 | { 15 | m_rectObject = std::make_unique(this, glm::vec4{ 1.0f, 0.0f, 0.0f, 1.0f }, nullptr, 16 | glm::vec2{ 0.0f, 0.7f }, glm::vec2{ 0.25f, 0.25f }, 0.0f); 17 | const QuadCoords quadCoords 18 | { 19 | { -0.15f, 0.20f }, 20 | { 0.10f, 0.20f }, 21 | { 0.15f, 0.45f }, 22 | { -0.10f, 0.45f }, 23 | }; 24 | m_quadObject = std::make_unique(this, quadCoords, glm::vec4{1.0f, 1.0f, 0.0f, 1.0f}, nullptr, false); 25 | m_circleObject = std::make_unique(this, glm::vec4{ 0.0f, 1.0f, 0.0f, 1.0f }, nullptr, 26 | glm::vec2{ 0.0f, 0.0f }, 0.125f); 27 | m_lineObject = std::make_unique(this, glm::vec4{ 0.0f, 0.0f, 1.0f, 1.0f }, glm::vec2{ -0.125f, -0.5f }, 28 | glm::vec2{ 0.125f, -0.25f }, 0.005f); 29 | } 30 | -------------------------------------------------------------------------------- /testapp/scenes/DrawTestScene.h: -------------------------------------------------------------------------------- 1 | #ifndef DRAW_TEST_SCENE_H_ 2 | #define DRAW_TEST_SCENE_H_ 3 | 4 | #include "Scene.h" 5 | 6 | class RectObject; 7 | class QuadObject; 8 | class CircleObject; 9 | class LineObject; 10 | 11 | class DrawTestScene : public Scene 12 | { 13 | public: 14 | DrawTestScene(); 15 | 16 | private: 17 | std::unique_ptr m_rectObject; 18 | std::unique_ptr m_quadObject; 19 | std::unique_ptr m_circleObject; 20 | std::unique_ptr m_lineObject; 21 | }; 22 | 23 | #endif /* DRAW_TEST_SCENE_H_ */ 24 | -------------------------------------------------------------------------------- /testapp/scenes/LineFollowerTestScene.cpp: -------------------------------------------------------------------------------- 1 | #include "LineFollowerTestScene.h" 2 | #include "components/KeyboardController.h" 3 | #include "robots/LineFollower.h" 4 | #include "shapes/RectObject.h" 5 | #include "playgrounds/LineFollowerPath.h" 6 | 7 | namespace { 8 | class LineFollowerController : public KeyboardController 9 | { 10 | public: 11 | enum class Drive { Stop, Forward, Backward, Left, Right }; 12 | const float maxVoltage = 3.0f; 13 | 14 | void setDrive(Drive drive) { 15 | float leftVoltage = 0.0f; 16 | float rightVoltage = 0.0f; 17 | switch (drive) { 18 | case Drive::Stop: 19 | leftVoltage = rightVoltage = 0.0f; 20 | break; 21 | case Drive::Forward: 22 | leftVoltage = rightVoltage = maxVoltage; 23 | break; 24 | case Drive::Backward: 25 | leftVoltage = rightVoltage = -maxVoltage; 26 | break; 27 | case Drive::Left: 28 | leftVoltage = -maxVoltage; 29 | rightVoltage = maxVoltage; 30 | break; 31 | case Drive::Right: 32 | leftVoltage = maxVoltage; 33 | rightVoltage = -maxVoltage; 34 | break; 35 | } 36 | 37 | *m_lineFollower->getVoltageLine(LineFollower::WheelMotorIndex::Left) = leftVoltage; 38 | *m_lineFollower->getVoltageLine(LineFollower::WheelMotorIndex::Right) = rightVoltage; 39 | } 40 | LineFollowerController(LineFollower *lineFollower) : m_lineFollower(lineFollower) 41 | { 42 | assert(lineFollower != nullptr); 43 | } 44 | void onKeyEvent(const Event::Key &keyEvent) override 45 | { 46 | if (keyEvent.code == Event::KeyCode::Up) { 47 | if (keyEvent.action == Event::KeyAction::Press) { 48 | setDrive(Drive::Forward); 49 | } else if (keyEvent.action == Event::KeyAction::Release) { 50 | setDrive(Drive::Stop); 51 | } 52 | } else if (keyEvent.code == Event::KeyCode::Down) { 53 | if (keyEvent.action == Event::KeyAction::Press) { 54 | setDrive(Drive::Backward); 55 | } else if (keyEvent.action == Event::KeyAction::Release) { 56 | setDrive(Drive::Stop); 57 | } 58 | } else if (keyEvent.code == Event::KeyCode::Left) { 59 | if (keyEvent.action == Event::KeyAction::Press) { 60 | setDrive(Drive::Left); 61 | } else if (keyEvent.action == Event::KeyAction::Release) { 62 | setDrive(Drive::Stop); 63 | } 64 | } else if (keyEvent.code == Event::KeyCode::Right) { 65 | if (keyEvent.action == Event::KeyAction::Press) { 66 | setDrive(Drive::Right); 67 | } else if (keyEvent.action == Event::KeyAction::Release) { 68 | setDrive(Drive::Stop); 69 | } 70 | } 71 | } 72 | void onFixedUpdate(float stepTime) override 73 | { 74 | (void)stepTime; 75 | } 76 | private: 77 | LineFollower *m_lineFollower = nullptr; 78 | }; 79 | } 80 | 81 | LineFollowerTestScene::LineFollowerTestScene() : 82 | Scene("Test keyboard-controlled line follower", PhysicsWorld::Gravity::TopView) 83 | { 84 | const float bgWidth = 3.0f; 85 | const float bgHeight = bgWidth; 86 | const glm::vec4 bgColor(1.0f, 1.0f, 1.0f, 1.0f); 87 | const glm::vec4 lineColor(0.0f, 0.0f, 0.0f, 1.0f); 88 | const float lineWidth = 0.01f; 89 | m_background = std::make_unique(this, bgColor, nullptr, glm::vec2{ 0.0f, 0.0f }, 90 | glm::vec2{ bgWidth, bgHeight }, 0.0f); 91 | 92 | m_lineFollowerPath = std::make_unique(this, lineColor, lineWidth, 93 | LineFollowerPath::getBlueprintPathPoints(LineFollowerPath::Blueprint::Mshaped)); 94 | m_lineFollower = std::make_unique(this, LineFollower::getBlueprintSpec(LineFollower::Blueprint::FourFrontSensors), 95 | glm::vec2{-0.25f, 0.0f}, 1.5f); 96 | m_lineFollower->setController(new LineFollowerController(m_lineFollower.get())); 97 | m_lineFollower->setDebug(true); 98 | } 99 | 100 | LineFollowerTestScene::~LineFollowerTestScene() 101 | { 102 | } 103 | -------------------------------------------------------------------------------- /testapp/scenes/LineFollowerTestScene.h: -------------------------------------------------------------------------------- 1 | #ifndef LINE_FOLLOWER_TEST_SCENE_H_ 2 | #define LINE_FOLLOWER_TEST_SCENE_H_ 3 | 4 | #include "Scene.h" 5 | 6 | #include "shapes/RectObject.h" 7 | 8 | class RectObject; 9 | class LineFollower; 10 | class LineFollowerPath; 11 | 12 | class LineFollowerTestScene : public Scene 13 | { 14 | public: 15 | LineFollowerTestScene(); 16 | ~LineFollowerTestScene(); 17 | 18 | private: 19 | std::unique_ptr m_background; 20 | std::unique_ptr m_lineFollowerPath; 21 | std::unique_ptr m_lineFollower; 22 | }; 23 | 24 | #endif /* LINE_FOLLOWER_TEST_SCENE_H_ */ 25 | -------------------------------------------------------------------------------- /testapp/scenes/PhysicsBotTestScene.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS_BOT_TEST_SCENE_H_ 2 | #define PHYSICS_BOT_TEST_SCENE_H_ 3 | 4 | #include "Scene.h" 5 | 6 | class KeyboardController; 7 | class PhysicsBot; 8 | 9 | class PhysicsBotTestScene : public Scene 10 | { 11 | public: 12 | PhysicsBotTestScene(); 13 | ~PhysicsBotTestScene(); 14 | 15 | private: 16 | void createTuningMenu(); 17 | std::unique_ptr m_tuningMenu; 18 | std::unique_ptr m_keyboardController; 19 | std::unique_ptr m_physicsBot; 20 | }; 21 | 22 | #endif /* PHYSICS_BOT_TEST_SCENE_H_ */ 23 | -------------------------------------------------------------------------------- /testapp/scenes/PhysicsTestScene.cpp: -------------------------------------------------------------------------------- 1 | #include "PhysicsTestScene.h" 2 | #include "shapes/RectObject.h" 3 | #include "shapes/CircleObject.h" 4 | 5 | #include 6 | 7 | PhysicsTestScene::PhysicsTestScene() : 8 | Scene("Test physics with 5x5cm@1kg box and 5cm@1kg ball", PhysicsWorld::Gravity::SideView) 9 | { 10 | const glm::vec4 color(1.0f, 1.0f, 1.0f, 1.0f); 11 | const Body2D::Specification groundBodySpec = { false, true, 1.0f }; 12 | m_ground = std::make_unique(this, color, &groundBodySpec, 13 | glm::vec2{ 0.0f, -0.2f }, glm::vec2{ 0.4f, 0.025f}, 0.0f); 14 | 15 | const Body2D::Specification fallingBoxBodySpec = { true, true, 1.0f }; 16 | m_fallingBox = std::make_unique(this, color, &fallingBoxBodySpec, 17 | glm::vec2{ 0.0f, 0.5f }, glm::vec2{ 0.05f, 0.05f }, 0.2f); 18 | const Body2D::Specification fallingBallBodySpec = { true, true, 1.0f }; 19 | m_fallingBall = std::make_unique(this, color, &fallingBallBodySpec, glm::vec2{ 0.04f, 0.7f }, 0.025f); 20 | } 21 | -------------------------------------------------------------------------------- /testapp/scenes/PhysicsTestScene.h: -------------------------------------------------------------------------------- 1 | #ifndef PHYSICS_TEST_SCENE_H_ 2 | #define PHYSICS_TEST_SCENE_H_ 3 | 4 | #include "Scene.h" 5 | 6 | class RectObject; 7 | class CircleObject; 8 | 9 | class PhysicsTestScene : public Scene 10 | { 11 | public: 12 | PhysicsTestScene(); 13 | 14 | private: 15 | std::unique_ptr m_ground; 16 | std::unique_ptr m_fallingBox; 17 | std::unique_ptr m_fallingBall; 18 | }; 19 | 20 | #endif /* PHYSICS_TEST_SCENE_H_ */ 21 | -------------------------------------------------------------------------------- /testapp/scenes/SpriteAnimationTestScene.cpp: -------------------------------------------------------------------------------- 1 | #include "SpriteAnimationTestScene.h" 2 | #include "SpriteAnimation.h" 3 | #include "shapes/RectObject.h" 4 | 5 | #include 6 | 7 | SpriteAnimationTestScene::SpriteAnimationTestScene() : 8 | Scene("Simple animation sprite sheet test") 9 | { 10 | const SpriteAnimation::Params animationParams = { 1, 2, 2, 30 }; 11 | m_rectObject = std::make_unique(this, "AnimationTestSpriteSheet.png", &animationParams, 12 | nullptr, glm::vec2{ 0.0f, 0.0f }, glm::vec2{ 0.25f, 0.25f }, 0.0f); 13 | } 14 | -------------------------------------------------------------------------------- /testapp/scenes/SpriteAnimationTestScene.h: -------------------------------------------------------------------------------- 1 | #ifndef SPRITE_ANIMATION_TEST_SCENE_H_ 2 | #define SPRITE_ANIMATION_TEST_SCENE_H_ 3 | 4 | #include "Scene.h" 5 | 6 | class RectObject; 7 | 8 | class SpriteAnimationTestScene : public Scene 9 | { 10 | public: 11 | SpriteAnimationTestScene(); 12 | private: 13 | std::unique_ptr m_rectObject; 14 | }; 15 | 16 | #endif /* SPRITE_ANIMATION_TEST_SCENE_H_ */ 17 | -------------------------------------------------------------------------------- /testapp/scenes/SumobotTestScene.h: -------------------------------------------------------------------------------- 1 | #ifndef SUMOBOT_TEST_SCENE_H_ 2 | #define SUMOBOT_TEST_SCENE_H_ 3 | 4 | #include "Scene.h" 5 | 6 | class RectObject; 7 | class Dohyo; 8 | class Sumobot; 9 | class NsumoMicrocontroller; 10 | class KeyboardController; 11 | 12 | class SumobotTestScene : public Scene 13 | { 14 | public: 15 | SumobotTestScene(); 16 | ~SumobotTestScene(); 17 | virtual void onFixedUpdate() override; 18 | 19 | private: 20 | struct Background { 21 | std::unique_ptr leftSide; 22 | std::unique_ptr middleStripe; 23 | std::unique_ptr rightSide; 24 | }; 25 | void createBackground(); 26 | void createTuningMenu(); 27 | std::unique_ptr m_tuningMenu; 28 | std::unique_ptr m_background = std::make_unique(); 29 | std::unique_ptr m_dohyo; 30 | std::unique_ptr m_fourWheelBot; 31 | std::unique_ptr m_fourWheelBotOpponent; 32 | std::unique_ptr m_twoWheelRectangleBot; 33 | std::unique_ptr m_twoWheelRoundBlackBot; 34 | std::unique_ptr m_twoWheelRoundRedBot; 35 | std::unique_ptr m_microcontroller; 36 | std::unique_ptr m_keyboardController; 37 | }; 38 | 39 | #endif /* SUMOBOT_TEST_SCENE_H_ */ 40 | -------------------------------------------------------------------------------- /testapp/scenes/WheelMotorTestScene.cpp: -------------------------------------------------------------------------------- 1 | #include "WheelMotorTestScene.h" 2 | #include "actuators/WheelMotor.h" 3 | #include "components/Transforms.h" 4 | #include "components/KeyboardController.h" 5 | 6 | namespace { 7 | const float voltageToApply = 2.0f; 8 | 9 | class WheelMotorController : public KeyboardController 10 | { 11 | public: 12 | WheelMotorController(WheelMotor *wheelMotor) : m_wheelMotor(wheelMotor) 13 | { 14 | assert(wheelMotor != nullptr); 15 | } 16 | void onKeyEvent(const Event::Key &keyEvent) override 17 | { 18 | if (keyEvent.code == Event::KeyCode::Up) { 19 | if (keyEvent.action == Event::KeyAction::Press) { 20 | *m_wheelMotor->getVoltageLine() = voltageToApply; 21 | } else if (keyEvent.action == Event::KeyAction::Release) { 22 | *m_wheelMotor->getVoltageLine() = 0.0f; 23 | } 24 | } else if (keyEvent.code == Event::KeyCode::Down) { 25 | if (keyEvent.action == Event::KeyAction::Press) { 26 | *m_wheelMotor->getVoltageLine() = -voltageToApply; 27 | } else if (keyEvent.action == Event::KeyAction::Release) { 28 | *m_wheelMotor->getVoltageLine() = 0.0f; 29 | } 30 | } 31 | } 32 | private: 33 | WheelMotor *m_wheelMotor = nullptr; 34 | }; 35 | } 36 | 37 | WheelMotorTestScene::WheelMotorTestScene() : 38 | Scene("Test wheel motor object", PhysicsWorld::Gravity::TopView) 39 | { 40 | const WheelMotor::Specification spec(0.03f, 0.06f, 0.06f, WheelMotor::TextureType::Green); 41 | m_wheelMotor = std::make_unique(this, spec, WheelMotor::Orientation::Left, glm::vec2{ 0.0f, 0.0f }, 0.0f); 42 | m_wheelMotorController = std::make_unique(m_wheelMotor.get()); 43 | m_wheelMotor->setController(m_wheelMotorController.get()); 44 | } 45 | -------------------------------------------------------------------------------- /testapp/scenes/WheelMotorTestScene.h: -------------------------------------------------------------------------------- 1 | #ifndef WHEEL_MOTOR_TEST_SCENE_H_ 2 | #define WHEEL_MOTOR_TEST_SCENE_H_ 3 | 4 | #include "Scene.h" 5 | 6 | class WheelMotor; 7 | class ControllerComponent; 8 | 9 | class WheelMotorTestScene : public Scene 10 | { 11 | public: 12 | WheelMotorTestScene(); 13 | 14 | private: 15 | std::unique_ptr m_wheelMotor; 16 | std::unique_ptr m_wheelMotorController; 17 | }; 18 | 19 | #endif /* WHEEL_MOTOR_TEST_SCENE_H_ */ 20 | -------------------------------------------------------------------------------- /tools/README.md: -------------------------------------------------------------------------------- 1 | This folder contains helper tools/scripts. 2 | -------------------------------------------------------------------------------- /tools/dc_motor_plot.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import matplotlib.style as style 3 | 4 | VOLTAGE_IN_CONSTANT = 0.00628 5 | ANGULAR_SPEED_CONSTANT = 0.00178 6 | TIME_STEP = 0.01 # seconds 7 | WHEEL_RADIUS = 0.015 # m 8 | MASS = 0.5 # kg 9 | WHEEL_COUNT = 4 10 | GRAVITY_CONSTANT = 9.82 11 | FRICTION_COEFFICIENT = 0.1 12 | NORMAL_FORCE = MASS * GRAVITY_CONSTANT 13 | MAX_FRICTION_FORCE = NORMAL_FORCE * FRICTION_COEFFICIENT 14 | 15 | 16 | # Mimics coulomb friction model (model used in b2FrictionJoint) 17 | def get_friction_force(force): 18 | if force < 0: # No friction force if no force applied 19 | return 0 20 | if force > MAX_FRICTION_FORCE: 21 | return MAX_FRICTION_FORCE 22 | else: 23 | return force 24 | 25 | 26 | def calc_next_value(voltage_in, graph_values): 27 | if len(graph_values["acceleration"]): 28 | prev_acceleration = graph_values["acceleration"][-1] 29 | ang_speed = graph_values["angular_speed"][-1] 30 | else: 31 | prev_acceleration = 0 32 | ang_speed = 0 33 | torque = VOLTAGE_IN_CONSTANT * voltage_in - ANGULAR_SPEED_CONSTANT * ang_speed 34 | force = WHEEL_COUNT * (torque / WHEEL_RADIUS) 35 | force_minus_friction = force - get_friction_force(force) 36 | acceleration = force_minus_friction / MASS 37 | 38 | # Integrate 39 | speed_increase = (abs(acceleration - prev_acceleration) * TIME_STEP) / 2.0 + \ 40 | (min(acceleration, prev_acceleration) * TIME_STEP) 41 | new_ang_speed = ang_speed + (speed_increase / (2 * 3.14 * WHEEL_RADIUS)) 42 | 43 | graph_values["acceleration"].append(acceleration) 44 | graph_values["force"].append(force) 45 | graph_values["torque"].append(torque) 46 | graph_values["angular_speed"].append(new_ang_speed) 47 | graph_values["speed"].append(new_ang_speed * 2 * 3.14 * WHEEL_RADIUS) 48 | 49 | 50 | def plot_single_graph(name, xvals, yvals, xlabel, ylabel, color, show=True): 51 | plt.rcParams['font.family'] = "serif" 52 | plt.rc('font', **{'size': 22}) 53 | style.use("seaborn-white") 54 | plt.figure(num=None, figsize=(20, 6), dpi=150) 55 | plt.xlabel(xlabel) 56 | plt.ylabel(ylabel) 57 | plt.gca().spines['right'].set_visible(False) 58 | plt.gca().spines['top'].set_visible(False) 59 | plt.plot(xvals, yvals, color=color) 60 | plt.savefig(name+'.svg', bbox_inches='tight', transparent=True) 61 | if show: 62 | plt.show() 63 | 64 | 65 | def calc_and_show_graphs(scaled=False): 66 | graph_values = {"time": [], 67 | "angular_speed": [], 68 | "speed": [], 69 | "torque": [], 70 | "force": [], 71 | "acceleration": []} 72 | duration = 5 73 | voltage_in = 6 74 | voltage_applied_duration = 3 75 | time_passed = 0 76 | while time_passed < duration: 77 | if voltage_in and (time_passed > voltage_applied_duration): 78 | voltage_in = 0 79 | calc_next_value(voltage_in, graph_values) 80 | graph_values["time"].append(time_passed) 81 | time_passed += TIME_STEP 82 | 83 | if scaled: 84 | graph_values["torque"] = [torque for torque in graph_values["torque"]] 85 | graph_values["speed"] = [speed for speed in graph_values["speed"]] 86 | graph_values["force"] = [force for force in graph_values["force"]] 87 | 88 | plot_single_graph("torque", graph_values["time"], graph_values["torque"], "Time (sec)", "Torque (Nm)", "blue") 89 | plot_single_graph("speed", graph_values["time"], graph_values["speed"], "Time (sec)", "Speed (m/s)", "red") 90 | plot_single_graph("force", graph_values["time"], graph_values["force"], "Time (sec)", "Force (N)", "green") 91 | 92 | 93 | calc_and_show_graphs(True) 94 | print("done") 95 | --------------------------------------------------------------------------------