From 08a0eff69bea1d903f2a7d9d368db5f237979436 Mon Sep 17 00:00:00 2001 From: Camden Dixie O'Brien Date: Sat, 24 May 2025 09:15:23 +0100 Subject: [PATCH] Create Hamiltonian-based simulation --- .gitignore | 1 + CMakeLists.txt | 11 ++++ main.c | 136 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 148 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 main.c diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..482bbba --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.13) + +project(cart-and-pole LANGUAGES C) + +find_package(SDL2 REQUIRED CONFIG REQUIRED COMPONENTS SDL2) + +add_executable(cart-and-pole main.c) +set_property(TARGET cart-and-pole PROPERTY C_STANDARD 11) +set_property(TARGET cart-and-pole PROPERTY C_EXTENSIONS OFF) +target_compile_options(cart-and-pole PRIVATE -Wall -Wextra -pedantic) +target_link_libraries(cart-and-pole PRIVATE m SDL2::SDL2) diff --git a/main.c b/main.c new file mode 100644 index 0000000..77eda05 --- /dev/null +++ b/main.c @@ -0,0 +1,136 @@ +#include +#include +#include + +#define WIN_WIDTH 800 +#define WIN_HEIGHT 800 +#define MARGIN_X 100 +#define MARGIN_Y 100 + +#define SCENE_WIDTH (WIN_WIDTH - 2 * MARGIN_X) +#define SCENE_HEIGHT (WIN_HEIGHT - 2 * MARGIN_Y) + +#define CART_WIDTH 50 +#define CART_HEIGHT 20 +#define MASS_RADIUS 10 + +typedef struct { + double s, ps; // Cart displacement + double a, pa; // Pendulum angle (from vertical) +} state_t; + +static const double mc = 2; // Cart mass +static const double mp = 2; // Pendulum mass +static const double l = 0.5; // Pendulum length +static const double g = 9.8; // Gravity + +static void update(state_t *s, double dt, double k) +{ + const double co = cos(s->a); + const double si = sin(s->a); + + const double s_dot + = (s->ps * l - s->pa * co) / (l * (mc + mp * pow(si, 2))); + const double ps_dot = k; + const double a_dot = (s->pa * (mc + mp) - s->ps * mp * l * co) + / (mp * pow(l, 2) * (mc + mp * pow(si, 2))); + const double pa_dot + = (co * si + * (pow(s->ps, 2) * mp * pow(l, 2) + - 2 * s->ps * s->pa * mp * l * co + pow(s->pa, 2) * (mc + mp))) + / (pow(l, 2) * pow(mc + mp * pow(si, 2), 2)) + - (s->ps * s->pa * si) / (l * (mc + mp * pow(si, 2))) + + mp * g * l * si; + + s->s += dt * s_dot; + s->ps += dt * ps_dot; + s->a += dt * a_dot; + s->pa += dt * pa_dot; +} + +static void render(SDL_Renderer *r, const state_t *s) +{ + static const double sf = SCENE_WIDTH; + static const double sc_l = sf * l; + static const double sc_vdisp = (SCENE_HEIGHT - sc_l) / 2; + static const SDL_Point sc_anchor = { + .x = WIN_WIDTH / 2, + .y = MARGIN_Y + (int)(sc_vdisp + sc_l), + }; + + // Cart centre + const SDL_Point sc_a = { + .x = sc_anchor.x + (int)(sf * s->s), + .y = sc_anchor.y, + }; + // Pendulum centre + const SDL_Point sc_b = { + .x = sc_a.x + (int)(sc_l * sin(s->a)), + .y = sc_a.y - (int)(sc_l * cos(s->a)), + }; + + // Cart + const SDL_Rect c = { + .x = sc_a.x - CART_WIDTH / 2, + .y = sc_a.y - CART_HEIGHT / 2, + .w = CART_WIDTH, + .h = CART_HEIGHT, + }; + SDL_RenderFillRect(r, &c); + + // Rod + SDL_RenderDrawLine(r, sc_a.x, sc_a.y, sc_b.x, sc_b.y); + + // Mass + const SDL_Rect m = { + .x = sc_b.x - MASS_RADIUS, + .y = sc_b.y - MASS_RADIUS, + .w = 2 * MASS_RADIUS, + .h = 2 * MASS_RADIUS, + }; + SDL_RenderFillRect(r, &m); +} + +int main(void) +{ + int err = SDL_Init(SDL_INIT_VIDEO); + assert(err == 0); + + SDL_Window *window = SDL_CreateWindow( + "Cart and Rod Simulation", SDL_WINDOWPOS_UNDEFINED, + SDL_WINDOWPOS_UNDEFINED, WIN_WIDTH, WIN_HEIGHT, 0); + assert(NULL != window); + + SDL_Renderer *renderer = SDL_CreateRenderer( + window, -1, SDL_RENDERER_ACCELERATED | SDL_RENDERER_PRESENTVSYNC); + assert(NULL != renderer); + + SDL_DisplayMode mode; + assert(SDL_GetWindowDisplayMode(window, &mode) == 0); + const double dt = 1.0 / mode.refresh_rate; + + state_t state = { .a = 0.1 }; + while (1) { + SDL_Event e; + while (SDL_PollEvent(&e)) { + if (e.type == SDL_QUIT) + goto quit; + } + + update(&state, dt, 0.0); + + SDL_SetRenderDrawColor(renderer, 255, 255, 255, 255); + SDL_RenderClear(renderer); + SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255); + render(renderer, &state); + + SDL_RenderPresent(renderer); + } + +quit: + SDL_DestroyRenderer(renderer); + SDL_DestroyWindow(window); + SDL_Quit(); + + return 0; +}