animation.c

#include <SDL2/SDL.h>
#include <stdbool.h>
#include <math.h>

#define WINDOW_WIDTH 800
#define WINDOW_HEIGHT 600

typedef struct {
    double x, y;
    double dx, dy;
} Point;

typedef struct {
	Point p;
	Point q;
} Line;

void updatePoint(Point* point) {
    point->x += point->dx;
    point->y += point->dy;
}

void updateLine(Line* line) {
	updatePoint(&(line->p));
	updatePoint(&(line->q));
}

void renderPoint(SDL_Renderer *renderer, Point point, int r, int g, int b) {
    SDL_SetRenderDrawColor(renderer, r, g, b, SDL_ALPHA_OPAQUE);
    SDL_RenderDrawPoint(renderer, (int) point.x, (int) point.y);
}

void renderDot(SDL_Renderer *renderer, Point point, int thickness, int r, int g, int b) {
    SDL_SetRenderDrawColor(renderer, r, g, b, SDL_ALPHA_OPAQUE);
	SDL_Rect rect = {point.x - thickness / 2.0, point.y - thickness / 2.0, thickness, thickness};
	SDL_RenderFillRect(renderer, &rect);
}

void renderSegment(SDL_Renderer *renderer, Line line, int r, int g, int b) {
    SDL_SetRenderDrawColor(renderer, r, g, b, SDL_ALPHA_OPAQUE);
    SDL_RenderDrawLine(renderer, (int) line.p.x, (int) line.p.y, (int) line.q.x, (int) line.q.y);
}

void renderLine(SDL_Renderer *renderer, Line line, int r, int g, int b) {
    SDL_SetRenderDrawColor(renderer, r, g, b, SDL_ALPHA_OPAQUE);
	// vertical line
	if (line.p.x == line.q.x)
    	SDL_RenderDrawLine(renderer, (int) line.p.x, 0, (int) line.q.x, WINDOW_HEIGHT-1);
	// slope != infinity
	double slope = (line.q.y - line.p.y) / (line.q.x - line.p.x);
    SDL_RenderDrawLine(renderer, 0, (int) (line.p.y - slope * line.p.x), WINDOW_WIDTH-1, (int) (line.p.y + slope * (WINDOW_WIDTH - line.p.x)));
}

Point calculate_Qp(Point A, Point P, Point Q) {
	double a = P.x - A.x;
	double b = P.y - A.y;
	double c = Q.x - A.x;
	double d = Q.y - A.y;
	double det = -a*c -b*d;
	double x = (-b*b*c + a*b*d) / det;
	double y = (-b*b*d - a*a*d) / det;
	Point output = {x, y, 0, 0};
	return output;
}

Point math2graphics(Point A) {
	Point output;
	output.x = A.x + (WINDOW_WIDTH / 2);
	output.y = -1 * A.y + (WINDOW_HEIGHT * 3 / 4);
	return output;
}

int main(int argc, char *argv[]) {
    if (SDL_Init(SDL_INIT_VIDEO) != 0) {
        SDL_Log("Unable to initialize SDL: %s", SDL_GetError());
        return 1;
    }

    SDL_Window *window = SDL_CreateWindow("Distance Recognition Without LIDAR",
                                          SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED,
                                          WINDOW_WIDTH, WINDOW_HEIGHT,
                                          SDL_WINDOW_SHOWN);
    if (!window) {
        SDL_Log("Could not create window: %s", SDL_GetError());
        SDL_Quit();
        return 1;
    }

    SDL_Renderer *renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_ACCELERATED);
    if (!renderer) {
        SDL_Log("Could not create renderer: %s", SDL_GetError());
        SDL_DestroyWindow(window);
        SDL_Quit();
        return 1;
    }






    // Initialize points with random positions and velocities

	Point O	= {0, 0, 0, 0};
	Point A	= {-100, 0, 0, 0};
	Point B	= {100, 0, 0, 0};
	Point P	= {0, 200, 0, 0};	// unknown to the observer
	int trail_max = 100;
	Point Q_es[trail_max];
	double t = 0;
	double dt = 0.01;

    

	


	bool running = true;
    SDL_Event event;

    while (running) {
        while (SDL_PollEvent(&event)) {
            if (event.type == SDL_QUIT) {
                running = false;
            }
        }

		t += dt;
        updatePoint(&O);
        updatePoint(&A);
        updatePoint(&B);
        updatePoint(&P);
		Point Q	= {80 + 200*cos(t), 300 + 50*sin(2*t), 0, 0};	// unknown to the observer

		double w = B.x - A.x;
		double theta = atan2(P.y - A.y, P.x - A.x);
		double dist = (w/2) * tan(theta);	// d has to be obtained the hard way
		Point P_e	= {0, dist, 0, 0};
		Line AB 	= {A, B};
		Line AP_e 	= {A, P_e};
		Line BP_e 	= {B, P_e};
		Point Qp = calculate_Qp(A, P, Q);
		Point Qpp = calculate_Qp(B, P, Q);
		Line PQp 	= {P, Qp};
		Line PQpp 	= {P, Qpp};
		Line AQ 	= {A, Q};
		Line BQ 	= {B, Q};
		// Calculate Q_e
		double a = Qp.x - A.x;
		double b = Qp.y - A.y;
		double c = Qpp.x - B.x;
		double d = Qpp.y - B.y;
		double x = (w/2.)*(a*d+b*c)/(a*d-b*c);
		double y = b*d*w/(a*d-b*c);
		Point Q_e = {x, y, 0, 0};


		Point gO = math2graphics(O);
		Point gA = math2graphics(A);
		Point gB = math2graphics(B);
		Point gP = math2graphics(P);
		Point gQ = math2graphics(Q);
		Point gP_e = math2graphics(P_e);
		Point gQp = math2graphics(Qp);
		Point gQpp = math2graphics(Qpp);
		Line gAP_e = {math2graphics(AP_e.p), math2graphics(AP_e.q)};
		Line gBP_e = {math2graphics(BP_e.p), math2graphics(BP_e.q)};
		Line gPQp = {math2graphics(PQp.p), math2graphics(PQp.q)};
		Line gPQpp = {math2graphics(PQpp.p), math2graphics(PQpp.q)};
		Line gAQ = {math2graphics(AQ.p), math2graphics(AQ.q)};
		Line gBQ = {math2graphics(BQ.p), math2graphics(BQ.q)};
		Point gQ_e = math2graphics(Q_e);
		for (int i=trail_max-1; i>0; --i) {
			Q_es[i] = Q_es[i-1];
		}
		Q_es[0] = gQ_e;


        SDL_SetRenderDrawColor(renderer, 0, 0, 0, SDL_ALPHA_OPAQUE); // Black color
        SDL_RenderClear(renderer);

		renderSegment(renderer, gAP_e, 30, 30, 30);
		renderSegment(renderer, gBP_e, 30, 30, 30);
		renderLine(renderer, gPQp, 30, 30, 30);
		renderLine(renderer, gPQpp, 30, 30, 30);
		renderSegment(renderer, gAQ, 30, 30, 30);
		renderSegment(renderer, gBQ, 30, 30, 30);
        renderDot(renderer, gO, 3, 255, 255, 255);		// known
        renderDot(renderer, gA, 3, 255, 255, 255);		// known
        renderDot(renderer, gB, 3, 255, 255, 255);		// known
        renderDot(renderer, gP, 5, 255, 100, 100);		// unknown
        renderDot(renderer, gQ, 5, 255, 100, 100);		// unknown
        renderDot(renderer, gQp, 3, 255, 255, 100);		// observed
        renderDot(renderer, gQpp, 3, 255, 255, 100);	// observed
        renderDot(renderer, gP_e, 3, 100, 255, 255);	// calculated
		for (int i=trail_max-1; i>=0; --i)
        	renderPoint(renderer, Q_es[i], 100, 100, 100);
        renderDot(renderer, gQ_e, 3, 100, 255, 255);	// calculated

        SDL_RenderPresent(renderer);

        SDL_Delay(16); // ~60 FPS
    }







    SDL_DestroyRenderer(renderer);
    SDL_DestroyWindow(window);
    SDL_Quit();

    return 0;
}