Add files via upload

game/math/bezier.c

@@ -0,0 +1,132 @@
+#include "bezier.h"
+#include "mathutils.h"
+#include "../motion.h"
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+BezierPath make_bezier_path(int n, point bezier[n]) {
+	BezierPath path = {
+		.arr = malloc(sizeof(point) * n),
+		.cap = n,
+		.len = n,
+	};
+
+	memcpy(path.arr, bezier, sizeof(point) * n);
+
+	return path;
+}
+
+void free_bezier(BezierPath *path) {
+	free(path->arr);
+	*path = (BezierPath) {
+		.arr      = NULL,
+		.cap      = 0,
+		.len      = 0,
+		.selected = -1,
+	};
+}
+
+double get_bez_distance(BezierPath *bezier, double res) {
+	double dist = 0;
+
+	for (int i = 0; i < bezier->len; i += 2) {
+		point prev = bezier->arr[i];
+
+		for (float j = 0; j < 1.f; j += res) {
+			point percent_a = lerp_point(bezier->arr[i    ], bezier->arr[i + 1], j);
+			point percent_b = lerp_point(bezier->arr[i + 1], bezier->arr[i + 2], j);
+
+			point curve = lerp_point(percent_a, percent_b, j);
+			dist += get_distance(prev, curve);
+
+			prev = curve;
+		}
+	}
+
+	return dist;
+}
+
+bool select_bez_point(int x, int y, BezierPath *path, Field *field) {
+	for (int i = 0; i < path->len; i++) {
+		point node = scale_to_screen(
+				path->arr[i].x,
+				path->arr[i].y,
+				FIELD_RESCALED);
+		rect handle = { node.x - 5, node.y - 5, 10, 10 };
+
+		if (within_box((point) { x, y }, handle)) {
+			path->selected = i;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void move_point(int x, int y, BezierPath *path, Field *field) {
+	if (path->selected == -1) return;
+	path->arr[path->selected] = scale_to_field(x, y, FIELD_RESCALED);
+}
+
+#define DIST    0.01
+#define DIST_SQ (DIST * DIST)
+
+/* fuck how do I do this shit?
+ * the issue is traveling a consistent distance along the curve
+ * instead of a percentage of each bezier.
+ *
+ * this is a very slow implementation of `arc length parameterization`
+ *
+ * I don't factor in ramping, which could lead to some problems */
+void save_bezier(BezierPath *path, const char *filename, double speed) {
+	if (speed < DIST) {
+		printf("bezier cannot be exported with speed %f\n", speed);
+		return;
+	}
+
+	FILE *file;
+	fopen_s(&file, filename, "w+");
+	fprintf(file, "%f,%f,%f,%f,%d\n", 0.f, 0.f, 0.f, 0.f, 0);
+
+	uint32_t time_ms = 0;
+
+	for (int i = 0; i < path->len-1; i += 2) {
+		point prev = path->arr[i];
+
+		for (float j = 0; j < 1.f; j += 0.001) {
+			point percent_a = lerp_point(path->arr[i    ], path->arr[i + 1], j);
+			point percent_b = lerp_point(path->arr[i + 1], path->arr[i + 2], j);
+
+			point curve = lerp_point(percent_a, percent_b, j);
+
+			double dist_sq =
+				(prev.x - curve.x) * (prev.x - curve.x) +
+				(prev.y - curve.y) * (prev.y - curve.y);
+
+			if (dist_sq > DIST_SQ) {
+				// are the component distances correct or should I apply normalization?
+				double dist   = sqrt(dist_sq);
+				double dist_x = curve.x - prev.x;
+				double dist_y = curve.y - prev.y;
+
+				// does this make sense? am i going insane?
+				double time_sec = dist / speed;
+				double left_x   = (dist_y / time_sec) / speed;
+				double left_y   = (dist_x / time_sec) / speed;
+
+				time_ms += (uint32_t) (time_sec * 1000);
+
+				fprintf(file, "%f,%f,%f,%f,%d\n",
+						left_x, left_y, 0.f, 0.f, time_ms);
+				prev = curve;
+			}
+		}
+	}
+
+	fclose(file);
+	printf("saved bezier path %s\n", filename);
+}

game/math/bezier.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "mathutils.h"
+#include "../motion.h"
+
+typedef point Bezier[3];
+
+typedef struct {
+	point *arr;
+	int    cap;
+	int    len;
+	int    selected;
+} BezierPath;
+
+#define BEZIER_TO_PATH(path) make_bezier_path(3, path)
+#define RES 0.025
+
+BezierPath make_bezier_path(int n, point bezier[n]);
+void       free_bezier(BezierPath *path);
+
+// is this actually needed?
+// maybe, depending on how I do playback and stuff
+double get_bez_distance(BezierPath *bezier, double res);
+bool   select_bez_point(int x, int y, BezierPath *path, Field *field);
+void   move_point(int x, int y, BezierPath *path, Field *field);
+
+// TODO: void deselect_bez_point(BezierPath *path);
+
+void save_bezier(BezierPath *path, const char *filename, double speed);

game/math/mathutils.c

@@ -0,0 +1,49 @@
+#include "mathutils.h"
+#include "../../gfx/gfx.h"
+#include <corecrt_math.h>
+
+point scale_to_screen(
+		double x,
+		double y,
+		double w_meters,
+		double h_meters,
+		double x_off,
+		double y_off)
+{
+	return (point) {
+		.x = x * ((double) SCREEN_WIDTH  / w_meters) + x_off,
+		.y = y * ((double) SCREEN_HEIGHT / h_meters) + y_off,
+	};
+}
+
+point scale_to_field(
+		double x,
+		double y,
+		double w_meters,
+		double h_meters,
+		double x_off,
+		double y_off)
+{
+	return (point) {
+		.x = (x - x_off) / ((double) SCREEN_WIDTH  / w_meters),
+		.y = (y - y_off) / ((double) SCREEN_HEIGHT / h_meters),
+	};
+}
+
+bool within_box(point p, rect r) {
+	return p.x >= r.x && p.y >= r.y &&
+		p.x <= r.x + r.w && p.y <= r.y + r.h;
+}
+
+point lerp_point(point a, point b, float p) {
+	return (point) {
+			a.x + (b.x - a.x) * p,
+			a.y + (b.y - a.y) * p
+	};
+}
+
+double get_distance(point a, point b) {
+	return sqrt(
+			(a.x - b.x) * (a.x - b.x) +
+			(a.y - b.y) * (a.y - b.y));
+}

game/math/mathutils.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <stdbool.h>
+
+typedef struct {
+	double x, y;
+} point;
+
+typedef struct {
+	double x, y, w, h;
+} rect;
+
+#define FIELD_RESCALED field->w_meters, field->h_meters, 0, 0
+
+// Rescales field coordinates to pixels
+point scale_to_screen(
+		double x,
+		double y,
+		double w_meters,
+		double h_meters,
+		double x_off,
+		double y_off);
+
+// Rescales pixel coordinates to meters
+point scale_to_field(
+		double x,
+		double y,
+		double w_meters,
+		double h_meters,
+		double x_off,
+		double y_off);
+
+bool within_box(point p, rect r);
+point lerp_point(point a, point b, float p);
+double get_distance(point a, point b);

game/motion.c

@@ -0,0 +1,178 @@
+#include "motion.h"
+#include "math/mathutils.h"
+#include "planner.h"
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+static bool add_point(MotionPath *path, double input[4], uint32_t time) {
+	if (path->len >= path->cap) {
+		path->cap *= 2;
+		printf("new cap %d\n", path->cap);
+
+		MotionPoint *tmp   = path->points;
+		const size_t psize = sizeof(MotionPoint) * path->cap;
+		path->points       = malloc(psize);
+		memset(path->points, 0, psize);
+
+		memcpy(path->points, tmp, sizeof(MotionPoint) * (path->len));
+		free(tmp);
+
+		if (path->points == NULL) {
+			printf("reallocation failed in add_point\n");
+			free(path->points);
+			return false;
+		}
+	}
+
+	path->points[path->len++] = (MotionPoint) {
+		.leftx	= input[0],
+		.lefty	= input[1],
+		.rightx	= input[2],
+		.righty	= input[3],
+		.time	= time,
+	};
+
+	return true;
+}
+
+MotionPath load_path(const char *filename, Field *field) {
+	FILE *file;
+	fopen_s(&file, filename, "r");
+
+	const int    filename_len = strlen(filename) + 1;
+	const size_t psize        = sizeof(MotionPoint) * PATH_INIT_SIZE;
+
+	MotionPath path = {
+		.name		  = malloc(sizeof(char) * filename_len),
+		.points		  = malloc(psize),
+		.cap		  = PATH_INIT_SIZE,
+		.len		  = 0,
+		.odo_path	  = NULL,
+		.bounding_box = { 0, 0, 0, 0 },
+	};
+
+	strcpy_s(path.name, filename_len, filename);
+	memset(path.points, 0, psize);
+
+	char num_buf[200];
+	int  num_top = 0;
+
+	double point_data[4];
+	int    point_top = 0;
+
+	char ch;
+	while ((ch = fgetc(file)) != EOF) {
+		switch (ch) {
+			case '-':
+			case '.':
+			case '0' ... '9':
+				num_buf[num_top++] = ch;
+				break;
+			case ',':
+				num_buf[num_top++] = '\0';
+				point_data[point_top++] = atof(num_buf);
+				num_top = 0;
+				break;
+			case '\n':
+				num_buf[num_top++] = '\0';
+				add_point(&path, point_data, atoi(num_buf));
+
+				num_top   = 0;
+				point_top = 0;
+				break;
+			default:
+				break;
+		}
+	}
+
+	fclose(file);
+
+	build_odo_mpath(&path, field);
+	return path;
+}
+
+// PERF: you know your data is well structured
+// when serialization is this easy
+void save_path(MotionPath *path) {
+	FILE *file;
+	fopen_s(&file, path->name, "w+");
+
+	for (int i = 0; i < path->len; i++) {
+		fprintf(file, "%f,%f,%f,%f,%d\n",
+				path->points[i].leftx,
+				path->points[i].lefty,
+				path->points[i].rightx,
+				path->points[i].righty,
+				path->points[i].time);
+	}
+
+	fclose(file);
+	printf("saved motion path %s\n", path->name);
+}
+
+bool free_path(MotionPath *path) {
+	if (path == NULL) return false;
+	free(path->name);
+	free(path->points);
+	path->name   = NULL;
+	path->points = NULL;
+	return true;
+}
+
+static inline double motion_curve(double i) {
+	return i;
+}
+
+void build_odo_mpath(MotionPath *path, Field *field) {
+	if (path->odo_path != NULL) return;
+
+	const size_t path_size = sizeof(point) * path->len;
+	path->odo_path         = malloc(path_size);
+	memset(path->odo_path, 0, path_size);
+
+	printf("building odo mpath\n");
+
+	double x = field->startx;
+	double y = field->starty;
+
+	path->odo_path[0] = (point) {
+		.x = field->startx,
+		.y = field->starty,
+	};
+
+	for (int i = 1; i < path->len; i++) {
+		// get the inputs (should represent the derivative of our auto)
+
+		double ix = path->points[i].leftx;
+		double iy = path->points[i].lefty;
+
+		// multiply by speed
+
+		double dx = iy * field->xy_speed;
+		double dy = ix * field->xy_speed;
+
+		// take the time difference (in seconds) and multiply the derivative by it
+
+		dx *= (path->points[i].time - path->points[i-1].time) / 1000.f;
+		dy *= (path->points[i].time - path->points[i-1].time) / 1000.f;
+
+		/* overall, for the x axis the full equation should look like this:
+		 * (leftx_input * speed) * ((time_ms1 - time_ms0) / 1000)
+		 *
+		 * a few other useful facts:
+		 *     input * speed = distance
+		 * */
+
+		path->odo_path[i] = (point) {x, y};
+
+		// change x & y by the derivative
+
+		x += dx;
+		y += dy;
+	}
+
+	printf("built odo mpath\n");
+}

game/motion.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include "math/mathutils.h"
+#include "../include/SDL.h"
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef enum {
+	RED_ONE,
+	RED_TWO,
+	RED_THREE,
+
+	BLUE_ONE,
+	BLUE_TWO,
+	BLUE_THREE,
+} StartPos;
+
+typedef struct {
+	double startx, starty;
+	double w_meters;
+	double h_meters;
+
+	double xy_speed; // meters per second
+	double rot_speed; // radians per second
+} Field;
+
+typedef struct {
+	double   leftx;
+	double   lefty;
+	double   rightx;
+	double   righty;
+	uint32_t time;
+} MotionPoint;
+
+#define PATH_INIT_SIZE 700
+typedef struct {
+	char        *name;
+
+	MotionPoint	*points;
+	uint32_t     cap;
+	uint32_t     len;
+	point		*odo_path;
+	SDL_Rect     bounding_box;
+} MotionPath;
+
+MotionPath load_path(const char *filename, Field *field);
+void       save_path(MotionPath *path);
+bool       free_path(MotionPath *path);
+
+void build_odo_mpath(MotionPath *path, Field *field);
+// void build_odo_bpath(MotionPath *path, Bezier *beziers);

game/planner.c

@@ -0,0 +1,316 @@
+#include "planner.h"
+#include "../gfx/gfx.h"
+#include "../include/SDL.h"
+#include "math/bezier.h"
+#include "math/mathutils.h"
+#include "motion.h"
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+
+#define SELECTED state.paths.arr[state.paths.selected]
+
+#define STATE_FIELD_RESCALED state.field.w_meters, state.field.h_meters, 0, 0
+
+static PlannerState state;
+static BezierPath bezier = {
+	.arr = NULL,
+	.cap = 51,
+	.len = 0,
+	.selected = -1,
+};
+
+const point START_POSITIONS[6] = {
+	[RED_ONE]    = {14.5, 3.6},
+	[RED_TWO]    = {14.5, 5.5},
+	[RED_THREE]  = {14.5, 7.0},
+
+	[BLUE_ONE]   = {2.0, 3.6},
+	[BLUE_TWO]   = {2.0, 5.5},
+	[BLUE_THREE] = {2.0, 7.0},
+};
+
+static void build_bounding_box(MultiPath *path) {
+	rect box = {
+		state.field.startx,
+		state.field.starty,
+		state.field.startx,
+		state.field.starty
+	};
+
+	if (path->type == PATH_MOTION) {
+		printf("motion.len: %d\n", path->motion.len);
+		for (int i = 0; i < path->motion.len; i++) {
+			double x = path->motion.odo_path[i].x;
+			double y = path->motion.odo_path[i].y;
+
+			if (x < box.x) box.x = x;
+			if (y < box.y) box.y = y;
+			if (x > box.w) box.w = x;
+			if (y > box.h) box.h = y;
+		}
+	} else {
+		for (int i = 0; i < path->bezier.len; i += 2) {
+			point prev = path->bezier.arr[i];
+
+			for (float j = 0; j < 1.f; j += RES) {
+				point percent_a = lerp_point(path->bezier.arr[i],
+						path->bezier.arr[i + 1], j);
+
+				point percent_b = lerp_point(path->bezier.arr[i + 1],
+						path->bezier.arr[i + 2], j);
+
+				point curve = lerp_point(percent_a, percent_b, j);
+				if (curve.x < box.x) box.x = curve.x;
+				if (curve.y < box.y) box.y = curve.y;
+				if (curve.x > box.w) box.w = curve.x;
+				if (curve.y > box.h) box.h = curve.y;
+
+				prev = curve;
+			}
+		}
+	}
+
+	point ll = scale_to_screen(box.x,         box.y,         STATE_FIELD_RESCALED);
+	point rr = scale_to_screen(box.w - box.x, box.h - box.y, STATE_FIELD_RESCALED);
+	path->bounding_box = (rect) {ll.x, ll.y, rr.x, rr.y};
+}
+
+void init_planner(StartPos start) {
+	const size_t path_arr_size = sizeof(MotionPath) * 10;
+
+	state = (PlannerState) {
+		.field = {
+			.startx    = START_POSITIONS[start].x, // 8.25,
+			.starty    = START_POSITIONS[start].y, // 4.05,
+			.w_meters  = 16.5,
+			.h_meters  = 8.1,
+			.xy_speed  = 0.8, //4.8,
+			.rot_speed = 0,
+		},
+		.zoom_level = 1,
+		.x_offset   = 0,
+		.y_offset   = 0,
+
+		.paths = {
+			.cap      = 4,
+			.len      = 0,
+			.arr      = malloc(path_arr_size),
+			.selected = 0,
+		},
+
+		.mode = MODE_EDIT,
+	};
+
+	memset(state.paths.arr, 0, path_arr_size);
+
+	if (state.paths.arr == NULL) {
+		printf("could not allocate MotionPaths");
+		exit(1);
+	}
+
+	const size_t bezier_size = sizeof(point) * bezier.cap;
+	bezier.arr               = malloc(bezier_size);
+	memset(bezier.arr, 0, bezier_size);
+
+	bezier.arr[bezier.len++] = (point) {
+		state.field.startx,
+		state.field.starty
+	};
+
+	bezier.arr[bezier.len++] = (point) {
+		state.field.startx + 1,
+		state.field.starty
+	};
+
+	bezier.arr[bezier.len++] = (point) {
+		state.field.startx + 2,
+		state.field.starty
+	};
+
+	state.paths.arr[state.paths.len++] = WRAP_BEZIER(bezier);
+	state.paths.arr[state.paths.len++] =
+		WRAP_MOTION(load_path("bezier.txt", &state.field));
+
+	for (int i = 0; i < state.paths.len; i++)
+		build_bounding_box(&state.paths.arr[i]);
+}
+
+static void path_playback(bool start, MotionPath *path) {
+	static clock_t start_time = 0;
+	static int     index      = 1;
+
+	if (start) {
+		start_time = clock();
+		index      = 1;
+	}
+
+	clock_t playback_time = clock() - start_time;
+
+	while (index < path->len && path->points[index].time < playback_time) {
+		index++;
+	}
+
+	if (index == path->len) {
+		state.mode = MODE_EDIT;
+		return;
+	}
+
+	double x_min = path->odo_path[index - 1].x;
+	double x_max = path->odo_path[index    ].x;
+
+	double y_min = path->odo_path[index - 1].y;
+	double y_max = path->odo_path[index    ].y;
+
+	double lerp_time =
+		((double) (path->points[index].time - path->points[index-1].time)) /
+		((double) playback_time);
+
+	point robot_pos = {
+		x_min + lerp_time * (x_max - x_min),
+		y_min + lerp_time * (y_max - y_min),
+	};
+
+	draw_robot(&state.field, robot_pos);
+}
+
+static void bezier_playback(bool start, BezierPath *path) {
+
+}
+
+// TODO: use BezierPath & port to bezier module
+static void deselect_bez_point(MultiPath *path) {
+	if (path->type == PATH_MOTION) return;
+	path->bezier.selected = -1;
+	build_bounding_box(path);
+}
+
+static void select_path(int x, int y) {
+	point mouse = { x, y };
+
+	for (int i = 0; i < state.paths.len; i++) {
+		if (within_box(mouse, state.paths.arr[i].bounding_box)) {
+			state.paths.selected = i;
+			return;
+		}
+	}
+}
+
+static void draw_correct_path(MultiPath *path, bool bold, bool select) {
+	if (path->type == PATH_MOTION)
+		draw_path(&path->motion, &state.field, bold, select);
+	else
+		draw_bezier(&path->bezier, &state.field, bold, select);
+}
+
+static void draw_paths(int x, int y) {
+	point mouse = { x, y };
+
+	for (int i = 0; i < state.paths.len && i < 2; i++) {
+		if (state.paths.selected == i) {
+			draw_correct_path(&state.paths.arr[i], true, true);
+			continue;
+		}
+
+		if (within_box(mouse, state.paths.arr[i].bounding_box))
+			draw_correct_path(&state.paths.arr[i], true, false);
+		else
+			draw_correct_path(&state.paths.arr[i], false, false);
+	}
+}
+
+void save_correct_format(MultiPath *path) {
+	if (path->type == PATH_MOTION)
+		save_path(&path->motion);
+	else
+		save_bezier(&path->bezier, "bezier.txt", state.field.xy_speed);
+}
+
+void planner_loop(SDL_Event *e) {
+	int x, y;
+	SDL_GetMouseState(&x, &y);
+
+	while (SDL_PollEvent(e)) {
+		switch (e->type) {
+			case SDL_QUIT:
+				// FIXME: crashing the program to exit is rather ungraceful.
+				// I should make a cleanup procedure at some point.
+				exit(0);
+				break;
+
+			case SDL_MOUSEBUTTONDOWN:
+				if (SELECTED.type == PATH_BEZIER &&
+						select_bez_point(x, y, &SELECTED.bezier, &state.field)) break;
+
+				select_path(x, y);
+				break;
+
+			case SDL_MOUSEBUTTONUP:
+				deselect_bez_point(&SELECTED);
+				break;
+
+			case SDL_KEYDOWN:
+				switch(e->key.keysym.scancode) {
+					case SDL_SCANCODE_P:
+						if (state.paths.selected >= 0 && SELECTED.type == PATH_MOTION) {
+							path_playback(true, &SELECTED.motion);
+							state.mode = MODE_PLAYBACK;
+						}
+						break;
+
+					case SDL_SCANCODE_A:
+						if (state.paths.selected >= 0 && SELECTED.type == PATH_BEZIER) {
+							if (bezier.len >= bezier.cap) break;
+
+							for (int i = 0; i < 2; i++) {
+								SELECTED.bezier.arr[SELECTED.bezier.len++] = (point) {
+									.x = state.field.startx,
+									.y = state.field.starty,
+								};
+							}
+						}
+						break;
+
+					case SDL_SCANCODE_S:
+						save_correct_format(&SELECTED);
+						break;
+
+					case SDL_SCANCODE_D:
+						printf("(%f, %f)\n", bezier.arr[0].x, bezier.arr[0].y);
+						break;
+
+					default:
+						break;
+				}
+				break;
+		}
+	}
+
+	draw_field(&state.field);
+
+	switch (state.mode) {
+		case MODE_EDIT:
+			if (SELECTED.type == PATH_BEZIER)
+				move_point(x, y, &SELECTED.bezier, &state.field);
+
+			draw_paths(x, y);
+			draw_robot(&state.field, (point) { state.field.startx, state.field.starty });
+			break;
+		case MODE_PLAYBACK:
+			DRAW_PATH_SELECT(&SELECTED.motion, &state.field);
+			path_playback(false, &SELECTED.motion);
+			break;
+		case MODE_RECORD:
+			DRAW_PATH_SELECT(&SELECTED.motion, &state.field);
+			draw_robot(&state.field, (point) {
+						SELECTED.motion.odo_path[SELECTED.motion.len - 1].x,
+						SELECTED.motion.odo_path[SELECTED.motion.len - 1].y,
+					});
+			break;
+	}
+
+	blit_screen();
+}

game/planner.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#include "motion.h"
+#include "math/bezier.h"
+#include "../include/SDL.h"
+
+typedef struct {
+	enum {
+		PATH_MOTION,
+		PATH_BEZIER
+	} type;
+
+	union {
+		MotionPath motion;
+		BezierPath bezier;
+	};
+
+	rect bounding_box;
+} MultiPath;
+
+#define WRAP_MOTION(path) (MultiPath) { PATH_MOTION, .motion = path };
+#define WRAP_BEZIER(path) (MultiPath) { PATH_BEZIER, .bezier = path };
+
+typedef struct {
+	Field field;
+	float zoom_level;
+	int	  x_offset;
+	int	  y_offset;
+
+	struct {
+		int        cap;
+		int        len;
+		MultiPath *arr;
+		int        selected;
+	} paths;
+
+	enum {
+		MODE_RECORD,
+		MODE_EDIT,
+		MODE_PLAYBACK,
+	} mode;
+} PlannerState;
+
+void init_planner(StartPos start);
+void path_playback(bool start, MotionPath *path);
+void save_correct_format(MultiPath *path);
+void planner_loop(SDL_Event *e);