Clipping Based Collision Detection

This is a proof of concept which briefly explains how a simple clipping algorithm can provide the necessary information to detect and resolve object collision. This algorithm heavily relies on approximations and therefore is only suitable for applications that don't require accuracy.

For each face of object A, we want to check if any of the vertices are in front or behind the faces of object B. If a vertex is behind a face, we will use that to clip the other vertices towards it, until all vertices of that face are inside the volume of object B. However, if a face from object A is entirely outside of object B, then that face get discarded.

What remains are vertices that exists within the intersection of objects A and B. If no vertices remain after clipping then that tells us the two objects are not colliding.

In order to resolve a collision we need to know the point of contact, the separation vector and its penetration depth.

The contact point can be any point that is within the intersection of the two objects. Fortunately we already have a list of vertices that are inside the intersection, so this becomes trivial. For better approximation we can assume the contact point is the centroid of those vertices.

The separation vector is calculated by taking the sum of each face normal with the weight of its surface area and taking the unit of that result. This crude method will surprisingly always return a vector that is guaranteed to separate the two objects apart.

With the separation vector known, we can use that to find the minimum penetration depth by taking the min and max results of the dot product of each vertex and the separation vector. I say minimum because there are some cases where the penetration depth falls short. This is tolerable since the objects will continue to push away each frame until they no longer appear to be intersecting.

Below is the actual code from my physics engine:

static fps_real temp_va[FPS_MAX_VERTEXES][3];
static fps_real temp_vb[FPS_MAX_VERTEXES][3];
static fps_real clipped[FPS_MAX_VERTEXES][3];

void fps_clip_edge(
	fps_real po[3], // Plane origin
	fps_real pn[3], // Plane normal
	fps_real ea[3], // Edge point A
	fps_real eb[3]  // Edge point B
) {
	fps_real en[3];

	fps_vec3_sub(eb,ea,en);
	fps_vec3_unit(en,en);

	fps_real pn_po_dot = fps_vec3_dot(pn,po);
	fps_real pn_ea_dot = fps_vec3_dot(pn,ea);
	fps_real pn_en_dot = fps_vec3_dot(pn,en);

	fps_real t = (pn_po_dot-pn_ea_dot)/pn_en_dot;

	eb[0] = ea[0]+en[0]*t;
	eb[1] = ea[1]+en[1]*t;
	eb[2] = ea[2]+en[2]*t;
}

fps_uint fps_clip_triangle(
	fps_real  po[3], // Plane origin
	fps_real  pn[3], // Plane normal
	fps_real  ta[3], // Triangle point A
	fps_real  tb[3], // Triangle point B
	fps_real  tc[3]  // Triangle point C
) {
	fps_real ta_po[3];
	fps_real tb_po[3];
	fps_real tc_po[3];

	fps_vec3_sub(ta,po,ta_po);
	fps_vec3_sub(tb,po,tb_po);
	fps_vec3_sub(tc,po,tc_po);

	fps_vec3_unit(ta_po,ta_po);
	fps_vec3_unit(tb_po,tb_po);
	fps_vec3_unit(tc_po,tc_po);

	fps_real ta_pn_dot = fps_vec3_dot(ta_po,pn);
	fps_real tb_pn_dot = fps_vec3_dot(tb_po,pn);
	fps_real tc_pn_dot = fps_vec3_dot(tc_po,pn);

	// Find furthest vertex behind plane to use as origin
	// then clip the other points towards it
	if (ta_pn_dot<=0.0) {
		if (tb_pn_dot>0.001) fps_clip_edge(po,pn,ta,tb);
		if (tc_pn_dot>0.001) fps_clip_edge(po,pn,ta,tc);
	} else if (tb_pn_dot<=0.0) {
		if (ta_pn_dot>0.001) fps_clip_edge(po,pn,tb,ta);
		if (tc_pn_dot>0.001) fps_clip_edge(po,pn,tb,tc);
	} else if (tc_pn_dot<=0.0) {
		if (ta_pn_dot>0.001) fps_clip_edge(po,pn,tc,ta);
		if (tb_pn_dot>0.001) fps_clip_edge(po,pn,tc,tb);
	} else {
		return FPS_TRUE;
	}

	return FPS_FALSE;
}

void fps_clip_convex(
	fps_real  va[][3], // Vertexes A
	fps_real  vb[][3], // Vertexes B
	fps_real  vc[][3], // Clipped vertexes
	fps_uint  va_len,  // Vertexes A length
	fps_uint  vb_len,  // Vertexes B length
	fps_uint *vc_len   // Clipped length
) {
	for (fps_uint i=0; i<vb_len; i++) {
		fps_vec3_mov(vb[i],vc[i]);
	}

	for (fps_uint a=0; a<va_len; a+=3) {
		fps_real ba[3];
		fps_real ca[3];
		fps_real pn[3];

		fps_vec3_sub(va[a+1],va[a],ba);
		fps_vec3_sub(va[a+2],va[a],ca);

		fps_vec3_cross(ba,ca,pn);
		fps_vec3_unit(pn,pn);

		for (fps_uint b=*vc_len; b>0; b-=3) {
			if (fps_clip_triangle(
				va[a],
				pn,
				vc[b-3],
				vc[b-2],
				vc[b-1]
			)) { // Discard triangle
				fps_vec3_mov(vc[*vc_len-3],vc[b-3]);
				fps_vec3_mov(vc[*vc_len-2],vc[b-2]);
				fps_vec3_mov(vc[*vc_len-1],vc[b-1]);

				*vc_len-=3;
			}
		}
	}
}

fps_real fps_detect_collision(
	fps_real  va[][3], // Vertexes A
	fps_real  vb[][3], // Vertexes B
	fps_uint  va_len,  // Vertexes A length
	fps_uint  vb_len,  // Vertexes B length
	fps_real  cp[3],   // Contact point
	fps_real  sn[3]    // Separation normal
) {
	// Zero initialize
	fps_vec3_id(cp);
	fps_vec3_id(sn);

	// Clip intersecting convex volumes
	fps_uint vc_len = vb_len;

	fps_clip_convex(
		va,
		vb,
		clipped,
		va_len,
		vb_len,
		&vc_len
	);

	if (vc_len==0) return 0.0; // No collision

	// Approximate contact point and separation normal
	for (fps_uint i=0; i<vc_len; i+=3) {
		fps_real ba[3]; // Edge BA
		fps_real ca[3]; // Edge CA
		fps_real fn[3]; // Face normal

		fps_vec3_add(cp,clipped[i],cp);
		fps_vec3_add(cp,clipped[i+1],cp);
		fps_vec3_add(cp,clipped[i+2],cp);

		fps_vec3_sub(clipped[i+1],clipped[i],ba);
		fps_vec3_sub(clipped[i+2],clipped[i],ca);

		fps_vec3_cross(ba,ca,fn);
		fps_vec3_add(sn,fn,sn);
	}

	fps_vec3_num_div(cp,vc_len,cp);
	fps_vec3_unit(sn,sn);

	// Calculate minimum penetration depth
	fps_real fd = -FPS_REAL_MAX;
	fps_real nd = FPS_REAL_MAX;

	for (fps_uint i=0; i<vc_len; i++) {
		fps_real d = fps_vec3_dot(clipped[i],sn);

		fd = FPS_MAX(fd,d);
		nd = FPS_MIN(nd,d);
	}

	return fd-nd;
}

Voila! That's all there is to my collision detection. If you want to see the full source code of my physics engine, you can download it from the github repository here.