2d physics with verlet
While watching youtube I came across a youtube channel explaining the math used for coding physics called Coding Math. Two videos expecially showed how easy it was to program simple rigid body physics using verlet integration (Verlet Integration Part I and Verlet Integration Part II). Here is my version done with C and SDL:
I searched online for more info about rigid body physics using verlet integration and found this article A Verlet based approach for 2D game physics. 2D rigid body physics is relatively easy using this approach. You don't have work with momentum, angular momentum etc. and no splitting of the rotational and linear cases. It's not really physically accurate, but good enough for most games. I made my own implementation with C and SDL:
C code
/* * * based on this gameDev post: * https://www.gamedev.net/articles/programming/math-and-physics/a-verlet-based-approach-for-2d-game-physics-r2714/ * */ #include <stdio.h> #include <stdbool.h> #include "stdint.h" #include <SDL2/SDL.h> #include <math.h> #define PI 3.14159265 #define MIN(A, B) ((A) < (B) ? (A) : (B)) #define MAX(A, B) ((A) > (B) ? (A) : (B)) SDL_Window* gWindow = NULL; SDL_Renderer* gRenderer = NULL; const uint8_t* gKeyboard; SDL_Point gScreenSize = {640, 480}; typedef struct { float x, y; } v2; static inline v2 V2(float x, float y) { return (v2){x, y}; } static inline int floatToInt(float x) { return (int)(x + 0.5); } static inline SDL_Point v2ToSDLpt(v2 a) { return (SDL_Point){floatToInt(a.x), floatToInt(a.y)}; } void v2sToPts(v2 v2s[], SDL_Point pts[], int count) { for (int i = 0; i < count; i++) { pts[i] = v2ToSDLpt(v2s[i]); } } static inline v2 v2_add(v2 a, v2 b) { return (v2){a.x + b.x, a.y + b.y}; } void v2s_add_v2(v2* v2s, int count, v2 b) { for (int i = 0; i < count; i++) { v2s[i] = (v2){v2s[i].x + b.x, v2s[i].y + b.y}; } } static inline v2 v2_sub(v2 a, v2 b) { return (v2){a.x - b.x, a.y - b.y}; } static inline v2 v2_scale(v2 a, float b) { return (v2){a.x * b, a.y * b}; } static inline float v2_length_sq(v2 a) { return a.x * a.x + a.y * a.y; } static inline float v2_length(v2 a) { return sqrtf(v2_length_sq(a)); } static inline float v2_dot(v2 a, v2 b) { return a.x * b.x + a.y * b.y; } static inline v2 v2_normalize(v2 a) { float len = v2_length(a); return V2(a.x / len, a.y / len); } static inline void v2_print(v2 a) { SDL_Log("x = %f, y = %f\n", a.x, a.y); } static inline v2 v2_rot(v2 point, v2 origin, float angle) { v2 p = v2_sub(point, origin); // rotate point p = (v2){cos(angle)*p.x - sin(angle)*p.y, sin(angle)*p.x + cos(angle)*p.y}; return v2_add(p, origin); } void v2s_rot(v2* pts, int count, v2 origin, double angle) { // move to origin for (int i = 0; i < count; i++) { pts[i] = v2_sub(pts[i], origin); } // rotate points v2 p; float cosangle = cos(angle); float sinangle = sin(angle); for (int i = 0; i < count; i++) { p = pts[i]; pts[i] = (v2){cosangle*p.x - sinangle*p.y, sinangle*p.x + cosangle*p.y}; } // move back for (int i = 0; i < count; i++) { pts[i] = v2_add(pts[i], origin); } } bool initSDL() { // initialize SDL if (SDL_Init(SDL_INIT_VIDEO) < 0) { SDL_Log("SDL could not initialize! SDL_Error: %s\n", SDL_GetError()); return false; } //Get device display mode SDL_DisplayMode displayMode; if( SDL_GetCurrentDisplayMode( 0, &displayMode ) == 0 ) { gScreenSize.x = displayMode.w; gScreenSize.y = displayMode.h; } // create window gWindow = SDL_CreateWindow("verlet", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, gScreenSize.x, gScreenSize.y, SDL_WINDOW_SHOWN | SDL_WINDOW_RESIZABLE); if (gWindow == NULL) { SDL_Log("Window could not be created! SDL_Error: %s\n", SDL_GetError()); return false; } // create renderer gRenderer = SDL_CreateRenderer(gWindow, -1, SDL_RENDERER_PRESENTVSYNC); if(gRenderer == NULL) { SDL_Log( "Renderer could not be created! SDL Error: %s\n", SDL_GetError() ); return false; } // initialize renderer color SDL_SetRenderDrawColor(gRenderer, 255, 255, 255, 255); // allow colors with alpha transparancy values SDL_SetRenderDrawBlendMode(gRenderer, SDL_BLENDMODE_BLEND); return true; } void closeSDL() { // destroy window if (gRenderer) { SDL_DestroyRenderer(gRenderer); gRenderer = NULL; } // destroy window if (gWindow) { SDL_DestroyWindow(gWindow); gWindow = NULL; } // quit SDL subsystems SDL_Quit(); } typedef struct { v2 pos; v2 oldpos; } pt; static inline pt createPt(float x, float y) { return (pt){ .pos = V2(x, y), .oldpos = V2(x, y) }; } typedef struct { v2* p0, *p1; bool boundary; float length; } edge; static inline edge createEdge(v2* p0, v2* p1, bool boundary) { return (edge){ .p0 = p0, .p1 = p1, .boundary = boundary, .length = v2_length(v2_sub(*p1, *p0)) }; } static inline void drawEdge(SDL_Renderer* renderer, edge e) { SDL_Point p0 = v2ToSDLpt(*e.p0); SDL_Point p1 = v2ToSDLpt(*e.p1); SDL_RenderDrawLine(gRenderer, p0.x, p0.y, p1.x, p1.y); } typedef struct { v2 pos; v2 size; } box; typedef struct { v2 center; float minX, minY, maxX, maxY; // min/max coordinates of the bounding box int vertexCount, edgeCount; pt* vertices; edge* edges; } rigidBody; void recalcCenterAndBoundingBox(rigidBody* rb) { rb->center = V2(0, 0); rb->minX = rb->minY = 10000.0; rb->maxX = rb->maxY = -10000.0; for (int i = 0; i < rb->vertexCount; i++) { pt v = rb->vertices[i]; rb->center = v2_add(rb->center, v.pos); rb->minX = MIN(rb->minX, v.pos.x); rb->maxX = MAX(rb->maxX, v.pos.x); rb->minY = MIN(rb->minY, v.pos.y); rb->maxY = MAX(rb->maxY, v.pos.y); } rb->center = v2_scale(rb->center, 1.0 / rb->vertexCount); } rigidBody makeRigidBodyRectangle(v2 pos, v2 size) { rigidBody rb; // create points of the square rb.vertexCount = 4; rb.vertices = (pt*)malloc(rb.vertexCount * sizeof(pt)); rb.vertices[0] = createPt(pos.x, pos.y); rb.vertices[1] = createPt(pos.x + size.x, pos.y); rb.vertices[2] = createPt(pos.x + size.x, pos.y + size.y); rb.vertices[3] = createPt(pos.x, pos.y + size.y); // create edges of the square rb.edgeCount = 5; rb.edges = (edge*)malloc(rb.edgeCount * sizeof(edge)); rb.edges[0] = createEdge(&rb.vertices[0].pos, &rb.vertices[1].pos, true); rb.edges[1] = createEdge(&rb.vertices[1].pos, &rb.vertices[2].pos, true); rb.edges[2] = createEdge(&rb.vertices[2].pos, &rb.vertices[3].pos, true); rb.edges[3] = createEdge(&rb.vertices[3].pos, &rb.vertices[0].pos, true); rb.edges[4] = createEdge(&rb.vertices[0].pos, &rb.vertices[2].pos, false); // cross beam recalcCenterAndBoundingBox(&rb); return rb; } rigidBody makeRigidBodyTriangle(v2 pos, v2 size){ rigidBody rb; // create points of the triangle rb.vertexCount = 3; rb.vertices = (pt*)malloc(rb.vertexCount * sizeof(pt)); rb.vertices[0] = createPt(pos.x, pos.y); rb.vertices[1] = createPt(pos.x + size.x, pos.y); rb.vertices[2] = createPt(pos.x + size.x, pos.y + size.y); // create edges of the triangle rb.edgeCount = 3; rb.edges = (edge*)malloc(rb.edgeCount * sizeof(edge)); rb.edges[0] = createEdge(&rb.vertices[0].pos, &rb.vertices[1].pos, true); rb.edges[1] = createEdge(&rb.vertices[1].pos, &rb.vertices[2].pos, true); rb.edges[2] = createEdge(&rb.vertices[2].pos, &rb.vertices[0].pos, true); recalcCenterAndBoundingBox(&rb); return rb; } rigidBody makeRigidBodyPentagon(v2 pos, float radius) { rigidBody rb; // create points of the polygon rb.vertexCount = 5; rb.vertices = (pt*)malloc(rb.vertexCount * sizeof(pt)); float angle = 2*PI / rb.vertexCount; v2 origin = V2(0, 0); v2 start = V2(0, radius); for (int i = 0; i < rb.vertexCount; i++) { v2 rotatedPoint = v2_add(v2_rot(start, origin, angle * i), pos); rb.vertices[i] = createPt(rotatedPoint.x, rotatedPoint.y); } // create edges of the polygon rb.edgeCount = 7; rb.edges = (edge*)malloc(rb.edgeCount * sizeof(edge)); // outer edges for (int i = 0; i < rb.vertexCount; i++) { rb.edges[i] = createEdge(&rb.vertices[i].pos, &rb.vertices[(i + 1) % rb.vertexCount].pos, true); } // crossbeams rb.edges[5] = createEdge(&rb.vertices[0].pos, &rb.vertices[2].pos, false); rb.edges[6] = createEdge(&rb.vertices[0].pos, &rb.vertices[3].pos, false); recalcCenterAndBoundingBox(&rb); return rb; } void projectRigidBodyToAxis(rigidBody rbody, v2 axis, float* min, float* max, pt** minVertex, pt** maxVertex) { float dotP = v2_dot(rbody.vertices[0].pos, axis); *min = *max = dotP; *minVertex = rbody.vertices; *maxVertex = rbody.vertices; for (int i = 1; i < rbody.vertexCount; i++) { v2 vertex = rbody.vertices[i].pos; dotP = v2_dot(vertex, axis); if (dotP > *max) { *max = dotP; *maxVertex = rbody.vertices + i; } else if (dotP < *min) { *min = dotP; *minVertex = rbody.vertices + i; } } } float intervalDistance(float aMin, float aMax, float bMin, float bMax) { if (aMin < bMin) return bMin - aMax; else return aMin - bMax; } typedef struct { float depth; v2 normal; edge* e; // colliding edge pt* v; // colliding vertex } collisionInfo; bool detectCollision(rigidBody a, rigidBody b, collisionInfo* colInf) { float minDist = -1000000; bool collidingEdgeInRigidBodyA; // iterate through all of the edges of both bodies for (int i = 0; i < a.edgeCount + b.edgeCount; i++) { edge *e; if (i < a.edgeCount) { e = a.edges + i; } else { e = b.edges + i - a.edgeCount; } // skip edges inside rigid body if(!e->boundary) continue; v2 edgeVec = v2_sub(*e->p1, *e->p0); v2 edgeVecRot90 = V2(edgeVec.y, -edgeVec.x); v2 axis = v2_normalize(edgeVecRot90); float aMin, aMax, bMin, bMax; pt* aMinVertex, *aMaxVertex, *bMinVertex, *bMaxVertex; projectRigidBodyToAxis(a, axis, &aMin, &aMax, &aMinVertex, &aMaxVertex); projectRigidBodyToAxis(b, axis, &bMin, &bMax, &bMinVertex, &bMaxVertex); // find the distance for this axis between the rigid bodies float dist; bool ACloserThenB = aMin < bMin; if (ACloserThenB) dist = bMin - aMax; else dist = aMin - bMax; if (dist > 0) { return false; // separating axis, so no collision } else if(dist > minDist) { minDist = dist; // the colliding edge // (not always true if rigid body has parallel axes like a square, // but that doesn't really matter in this implementation) colInf->e = e; colInf->normal = axis; // find the colliding vertex collidingEdgeInRigidBodyA = i < a.edgeCount; if (ACloserThenB) { if (collidingEdgeInRigidBodyA) colInf->v = bMinVertex; else colInf->v = aMaxVertex; } else { if (collidingEdgeInRigidBodyA) colInf->v = bMaxVertex; else colInf->v = aMinVertex; } } } colInf->depth = -minDist; // make sure the collision normal points // from the rigid body with the colliding edge // to the rigid rigid body with the colliding vertex v2 tmp = collidingEdgeInRigidBodyA ? v2_sub(b.center, a.center) : v2_sub(a.center, b.center); if (v2_dot(colInf->normal, tmp) <= 0) { colInf->normal = v2_scale(colInf->normal, -1); } // no separating axis, report collision return true; } bool boundingBoxesCollide(rigidBody* a, rigidBody* b) { // simple bounding box overlapping test return (a->minX <= b->maxX) && (a->minY <= b->maxY) && (a->maxX >= b->minX) && (a->maxY >= b->minY); } int main(int argc, char* args[]) { if (!initSDL()) { SDL_Log("Failed to initialize SDL\n"); return 0; } bool quit = false; // seed the random number generator srand(234567); // event handler SDL_Event event; // keeps track of time between steps Uint32 newTime = 0, oldTime = 0; float grav = 0.001; float friction = 0.995; const int maxRigidBodies = 100; rigidBody rigidBodies[maxRigidBodies]; int rigidBodyCount = 0; bool addingRigidBody = false; rigidBody tmpRigidBodyRect; bool paused = false; bool periodPressed = false; float dt = 1, dtOld = 1; while (!quit) { // calculate time step newTime = SDL_GetTicks(); dtOld = dt; dt = newTime - oldTime; oldTime = newTime; // handle events on queue periodPressed = false; while (SDL_PollEvent(&event) != 0) { int mouseX, mouseY; if (event.type == SDL_QUIT) { quit = true; } else if(event.type == SDL_WINDOWEVENT) { if (event.window.event == SDL_WINDOWEVENT_SIZE_CHANGED) { gScreenSize.x = event.window.data1; gScreenSize.y = event.window.data2; SDL_RenderPresent(gRenderer); } } else if (event.type == SDL_MOUSEBUTTONDOWN) { if (rigidBodyCount < maxRigidBodies) { SDL_Log("adding rigid body\n"); addingRigidBody = true; SDL_GetMouseState(&mouseX, &mouseY); tmpRigidBodyRect = makeRigidBodyRectangle(V2(mouseX, mouseY), V2(10, 10)); } else { SDL_Log("max rigid bodies (= %d) reached, can't add more\n", maxRigidBodies); } } else if (event.type == SDL_MOUSEMOTION) { if (addingRigidBody) { SDL_GetMouseState(&mouseX, &mouseY); // resize vertices with mouse coordinates tmpRigidBodyRect.vertices[1] = createPt(mouseX, tmpRigidBodyRect.vertices[1].pos.y); tmpRigidBodyRect.vertices[2] = createPt(mouseX, mouseY); tmpRigidBodyRect.vertices[3] = createPt(tmpRigidBodyRect.vertices[3].pos.x, mouseY); // update edge lengths for (int i = 0; i < tmpRigidBodyRect.edgeCount; i++) { edge* e = tmpRigidBodyRect.edges + i; e->length = v2_length(v2_sub(*e->p1, *e->p0)); } } } else if (event.type == SDL_MOUSEBUTTONUP) { if (addingRigidBody) { rigidBodies[rigidBodyCount++] = tmpRigidBodyRect; addingRigidBody = false; } } else if (event.type == SDL_KEYDOWN) { switch (event.key.keysym.sym) { case SDLK_SPACE: if (rigidBodyCount < maxRigidBodies) { SDL_Log("adding rigid body\n"); switch (rand() % 3) { case 0: rigidBodies[rigidBodyCount++] = makeRigidBodyTriangle(V2(200, 200), V2(40, 40)); break; case 1: rigidBodies[rigidBodyCount++] = makeRigidBodyPentagon(V2(200, 200), 30); break; case 2: rigidBodies[rigidBodyCount++] = makeRigidBodyRectangle(V2(200, 200), V2(40, 40)); break; } } else { SDL_Log("max rigid bodies (= %d) reached, can't add more\n", maxRigidBodies); } break; case SDLK_p: SDL_Log("pausing game\n"); paused = paused ? false : true; break; case SDLK_PERIOD: SDL_Log("stepping to next frame\n"); periodPressed = true; break; } } // update keyboard gKeyboard = SDL_GetKeyboardState(NULL); } if (gKeyboard[SDL_SCANCODE_LSHIFT]) { SDL_Log("slowmotion timestep\n"); dt *= 0.1; } // clear screen, make background white SDL_SetRenderDrawColor(gRenderer, 255, 255, 255, 255); SDL_RenderClear(gRenderer); // -- physics update -- if (!paused || periodPressed) { // update forces and positions for (int i = 0; i < rigidBodyCount; i++) { rigidBody rb = rigidBodies[i]; // update points for (int i = 0; i < rb.vertexCount; i++) { pt* p = rb.vertices + i; v2 acc = V2(0, grav); // force control with keyboard float force = 0.0005; if (gKeyboard[SDL_SCANCODE_UP]) { acc = v2_add(acc, V2(0, -force)); } if (gKeyboard[SDL_SCANCODE_DOWN]) { acc = v2_add(acc, V2(0, force)); } if (gKeyboard[SDL_SCANCODE_LEFT]) { acc = v2_add(acc, V2(-force, 0)); } if (gKeyboard[SDL_SCANCODE_RIGHT]) { acc = v2_add(acc, V2(force, 0)); } v2 vel = v2_add(v2_scale(v2_sub(p->pos, p->oldpos), dt/dtOld), v2_scale(acc, dt*dt)); vel = v2_scale(vel, friction); p->oldpos = p->pos; p->pos = v2_add(p->pos, vel); } } // the more iterations, the more accurate the sim is for (int sim = 0; sim < 3; sim++) { // loop once over all rigid bodies for (int i = 0; i < rigidBodyCount; i++) { rigidBody* rb = rigidBodies + i; // update edges for (int i = 0; i < rb->edgeCount; i++) { edge e = rb->edges[i]; v2 delta = v2_sub(*e.p1, *e.p0); float distance = v2_length(delta); float difference = e.length - distance; float percent = difference / distance / 2; v2 offset = v2_scale(delta, percent); *e.p0 = v2_sub(*e.p0, offset); *e.p1 = v2_add(*e.p1, offset); } // recalculate centers and bounding box recalcCenterAndBoundingBox(rb); // collide with screen border for (int i = 0; i < rb->vertexCount; i++) { pt* p = rb->vertices + i; p->pos.x = MAX(MIN(p->pos.x, gScreenSize.x), 0); p->pos.y = MAX(MIN(p->pos.y, gScreenSize.y), 0); } } // iterate collisions for all rigid bodies for (int i = 0; i < rigidBodyCount - 1; i++) { for (int j = i + 1; j < rigidBodyCount; j++) { rigidBody* rb1 = rigidBodies + i; rigidBody* rb2 = rigidBodies + j; // optimization if (!boundingBoxesCollide(rb1, rb2)) continue; collisionInfo colInf; if (detectCollision(*rb1, *rb2, &colInf)) { // collision response // move the vertex by half the collision vector v2 collisionVector = v2_scale(colInf.normal, colInf.depth); colInf.v->pos = v2_add(colInf.v->pos, v2_scale(collisionVector, 0.5)); // find position of collision vertex on the colliding edge float t; v2* p0 = colInf.e->p0; v2* p1 = colInf.e->p1; if (abs(p0->x - p1->x) > abs(p0->y - p1->y)) t = (colInf.v->pos.x - collisionVector.x - p0->x) / (p1->x - p0->x); else t = (colInf.v->pos.y - collisionVector.y - p0->y) / (p1->y - p0->y); float lambda = 1.0 / (t*t + (1 - t) * (1 - t)); *p0 = v2_sub(*p0, v2_scale(collisionVector, 0.5 * lambda * (1 - t))); *p1 = v2_sub(*p1, v2_scale(collisionVector, 0.5 * lambda * t)); } } } } } // -- drawing -- // draw rigid bodies for (int i = 0; i < rigidBodyCount; i++) { rigidBody rb = rigidBodies[i]; for (int i = 0; i < rb.edgeCount; i++) { edge e = rb.edges[i]; if (e.boundary) SDL_SetRenderDrawColor(gRenderer, 255, 0, 0, 255); // red else SDL_SetRenderDrawColor(gRenderer, 0, 255, 0, 255); // green drawEdge(gRenderer, e); } } // drawing the temporary rigidbody when being added if (addingRigidBody) { for (int i = 0; i < tmpRigidBodyRect.edgeCount; i++) { edge e = tmpRigidBodyRect.edges[i]; SDL_SetRenderDrawColor(gRenderer, 30, 30, 30, 255); // grey drawEdge(gRenderer, e); } } // render to screen SDL_RenderPresent(gRenderer); } // free pointers to vertices and edges of rigid bodies for (int i = 0; i < rigidBodyCount; i++) { rigidBody rb = rigidBodies[i]; free(rb.vertices); free(rb.edges); } // sdl close closeSDL(); return 0; }