Create Hamiltonian-based simulation

.gitignore

@@ -0,0 +1 @@
+build

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)

main.c

@@ -0,0 +1,136 @@
+#include <SDL2/SDL.h>
+#include <assert.h>
+#include <math.h>
+
+#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;
+}