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;
}