Polygonal Collision Detection

collision.cpp

//	Imagine we have two edges bounded by the vertices (a,b) and (d,e)
//	line(a=>b) = (b-a)*mu + a
//	line(d=>e) = (e-d)*lambda + d
//
//	or
//		 (     )        ( a.x )
//	ab = | b-a | * mu + |     |
//		 (     )        ( a.y )
//
//		 (     )        ( d.x )
//	de = | e-d | * lm + |     |
//		 (     )        ( d.y )
//
//	let <x,y> = b-a and <x',y'> = e-d
//
//	x*mu+a.x = x'*lm+d.x
//	y*mu+a.y = y'*lm+d.y
//
//	-y(x*mu+a.x = x'*lm+d.x)
//	 x(y*mu+a.y = y'*lm+d.y)
//	lm*(xy'-x'y) = x*a.y-y*a.x+y*d.x-x*d.y
//
//			x*(a.y-d.y)+y*(d.x-a.x)
//	lamda = -----------------------
//				  xy' - x'y

//	lambda dictates the "distance" between a de and ab as long as ab is uniform

#include "collision.h"

float intersection_parameter(seg l, vec p1){
	// edge = l, normal = transform(y,-x) to l
	// Parameterization:
	// l: (x,y)*lambda + (xa, ya)
	// n: (x,y)*mu + (x0, y0)
	float x = l.dir.x, y = l.dir.y, x0 = l.p0.x, y0 = l.p0.y, xa = p1.x, ya = p1.y;
	// A little bit of algebra magic... (Note that lambda parameterizes the edges.)
	return (x*(x0-xa)+y*(y0-ya))/(x*x+y*y); // Make sure that they are all positive or negative uniformly
}

#define ABS(n) ((n)>=0?(n):-(n))
bool collision(polygon a, polygon b){
	// Hold polygon a fixed
	int i, j;
	for (i = 0; i < a.n; i++){
		float sigma = 0, abs = 0, epsilon = 0.00000000001; // epsilon for marginal discrepancy in floating point arithmetic
		for (j = 0; j < b.n; j++){
			float parameter = intersection_parameter(a.edges[i], b.vertices[j]);
			sigma += parameter; abs += ABS(parameter); // There are easier ways to make sure that all of the signs are the same
		}
		if (ABS(ABS(sigma) - abs) > epsilon) return true;
	}
	return false;
}

#include <iostream>

int main(){
	polygon a = Polygon(4, v(0,0),v(0,3),v(3,3),v(3,0)), b = Polygon(4, v(4,4),v(4,6),v(6,6),v(6,4));
	std::cout << collision(a, b) << std::endl; // Should be false
	a = Polygon(4, v(0,0),v(0,5),v(5,3),v(3,0));  b = Polygon(4, v(4,4),v(4,6),v(6,6),v(6,4));
	std::cout << collision(a, b) << std::endl; // Should be false
	a = Polygon(4, v(0,0),v(0,5),v(5,4),v(3,0));  b = Polygon(4, v(4,4),v(4,6),v(6,6),v(6,4));
	std::cout << collision(a, b) << std::endl; // Should be true
	a = Polygon(4, v(0,0),v(0,5),v(5,4),v(3,0));  b = Polygon(4, v(4,4),v(4,6),v(6,6),v(6,4));
	std::cout << collision(a, b) << std::endl; // Should be true
	return 0;
}

collision.h

#ifndef COLLISION_H_
#define COLLISION_H_

#include <stdarg.h>
#include <stdlib.h>
#include <stdio.h>

// Implementation details only, disregard as this is irrelevant to the implementation of the actual algorithm.

typedef struct {float x, y;} vec;
typedef struct {vec p0, dir;} ray;
typedef struct {vec p0, p1, dir;} seg;
typedef struct {int n; vec *vertices; seg *edges;} polygon; // Assumption: Simply connected => chain vertices together

polygon new_polygon(int nvertices, vec *vertices){
	seg *edges = (seg*)malloc(sizeof(seg)*(nvertices));
	for (int i = 0; i < nvertices-1; i++){
		vec dir = {vertices[i+1].x-vertices[i].x, vertices[i+1].y-vertices[i].y};seg cur = {vertices[i], vertices[i+1], dir};
		edges[i] = cur;
	}
	vec dir = {vertices[0].x-vertices[nvertices-1].x, vertices[0].y-vertices[nvertices-1].y};seg cur = {vertices[nvertices-1], vertices[0], dir};
	edges[nvertices-1] = cur;
	polygon shape = {nvertices, vertices, edges};
	return shape;
}

vec v(float x, float y){
	vec a = {x, y};
	return a;
}

polygon Polygon(int nvertices, ...){
	va_list args;
	va_start(args, nvertices);
	vec *vertices = (vec*)malloc(sizeof(vec)*nvertices);
	for (int i = 0; i < nvertices; i++){
		vertices[i] = va_arg(args, vec);
	}
	va_end(args);
	return new_polygon(nvertices, vertices);
}

#endif /* COLLISION_H_ */