Gilbert–Johnson–Keerthi Simplex Distance Algorithm for Collision Detection

Also known as GJK Simplex, is an algorithm used in order to detect collision between convex objects. Even though this method is simple, its implementation can get a little complicated. It relies con two important operations called the Minkowski sum and the Minkowski difference.

 /**
* \brief iterates the simplex when it has 2 active points
* \param simplex - to iterate from
* \return - direction to the center form the new simplex
*/
vec3 gjk_simplex::iterate_line(gjk_simplex* simplex) {
	//get the points
	const std::array<glm::vec3, 4>& points_simplex = simplex->points();
	//new point = a will always be at second position in the simplex (if its a line) = points_simplex[1]
	//therefore we know that points_simplex[0] = b is not the closest point
	//get the line ab (points_simplex[0] - points_simplex[1])
	vec3 line_ab = points_simplex[0] - points_simplex[1];
	//get the vector to the origin from a
	vec3 line_a0 = -points_simplex[1];
	//if the line is closer to the origin
	if (dot(line_ab, line_a0) >= 0.0f) {
		//then we get a vector perpendicular to the line in the direction of the origin
		return cross(cross(line_ab, line_a0), line_ab);
	}
	//if not then the closest point to the origin is point a
	//remove point b from the simplex
	simplex->remove_point(0);
	//set the new direction from point a to the origin
	return line_a0;
}

/**
* iterates the simplex when it has 3 active points
* @param simplex - to iterate from
* @return - direction to the center form the new simplex
*/
vec3 gjk_simplex::iterate_triangle(gjk_simplex* simplex) {
	//get the points
	const std::array<glm::vec3, 4>& points_simplex = simplex->points();
	//new point = a will always be at third position in the simplex (if its a line) = points_simplex[2]
	//a = points_simplex[2], b = points_simplex[1], c = points_simplex[0]
	vec3 ab = points_simplex[1] - points_simplex[2];	//line from a to b
	vec3 ac = points_simplex[0] - points_simplex[2];	//line from a to c
	vec3 abc = cross(ab, ac);							//normal to the triangle
	vec3 ab_edge = cross(ab, abc);						//edge of a and b
	vec3 ac_edge = cross(abc, ac);						//edge of a and c
	vec3 a0 = -points_simplex[2];						//vector from a to origin
														//check if it is outside one of the edges
	if (dot(ac_edge, a0) >= 0.0f) {
		//check if the origin is near the line
		if (dot(ac, a0) >= 0.0f) {
			//if it is, then we make the simplex be only the line ac
			simplex->remove_point(1);
			//get the direction from the edge to the origin
			return cross(cross(ac, a0), ac);
		}
		//if not then check if near a or near ab
		if (dot(ab, a0) >= 0.0f) {
			//if the origin is closer to the edge then the simplex is now only a and b
			simplex->remove_point(0);
			//get the new direction from the edge to the origin
			return cross(cross(ab, a0), ab);;
		}
		//if not then the closest point is a
		simplex->remove_point(0);
		simplex->remove_point(1);
		//set direction from a to origin
		return a0;
	}
	//if its not outside edge ac then we check if inside triangle or the side of the other edge
	{
		//check if it is near the edge ab
		if (dot(ab_edge, a0) >= 0.0f) {
			//check if near a or near ab
			if (dot(ab, a0) >= 0.0f) {
				//if the origin is closer to the edge then the simplex is now only a and b
				simplex->remove_point(0);
				//get the new direction from the edge to the origin
				return cross(ab, cross(a0, ab));
			}
			//if not then the closest point is a
			simplex->remove_point(0);
			simplex->remove_point(1);
			//set direction from a to origin
			return a0;
		}
		//then it is enclosed by the triangle planes

		if (dot(abc, a0) >= 0.0f) {
			//if the origin is 'on top' (in the same direction of the normal of the triangle)
			//the simplex stays the same
			//get the direction
			return abc;
		}
		//the it is 'below' the triangle
		//set up the simplex as a new one
		gjk_simplex new_simplex{};
		new_simplex.add_point(points_simplex[1]);
		new_simplex.add_point(points_simplex[0]);
		new_simplex.add_point(points_simplex[2]);
		*simplex = new_simplex;
		//set new direction
		return -abc;
	}
}

/**
	* iterates the simplex when it has 4 active points
	* @param simplex - to iterate from
	* @return - direction to the center form the new simplex
	*/
vec3 gjk_simplex::iterate_tetrahedron(gjk_simplex* simplex) {
	//get the points
	const std::array<glm::vec3, 4>& points_simplex = simplex->points();
	//a = points_simplex[3], b = points_simplex[2], c = points_simplex[1], d = points_simplex[0]
	//get the direction from a to origin
	vec3 a0 = -points_simplex[3];
	//get the normal of each of the triangles
	vec3 norm_adb{}, norm_abc{}, norm_acd{};
	norm_adb = cross(points_simplex[0] - points_simplex[3], points_simplex[2] - points_simplex[3]);
	norm_abc = cross(points_simplex[2] - points_simplex[3], points_simplex[1] - points_simplex[3]);
	norm_acd = cross(points_simplex[1] - points_simplex[3], points_simplex[0] - points_simplex[3]);
	//new simplex triangle
	gjk_simplex new_simplex{};
	//check which of the 3 triangles is closer to the origin
	if (dot(norm_adb, a0) >= 0.0f) {
			//call the triangle function
			new_simplex.add_point(points_simplex[0]);
			new_simplex.add_point(points_simplex[2]);
			new_simplex.add_point(points_simplex[3]);
			*simplex = new_simplex;
			return iterate_triangle(simplex);
		}
	if (dot(norm_abc, a0) >= 0.0f) {
			//call the triangle function
			new_simplex.add_point(points_simplex[1]);
			new_simplex.add_point(points_simplex[2]);
			new_simplex.add_point(points_simplex[3]);
			*simplex = new_simplex;
			return iterate_triangle(simplex);
		}
	if (dot(norm_acd, a0) >= 0.0f) {
			//call the triangle function
			new_simplex.add_point(points_simplex[0]);
			new_simplex.add_point(points_simplex[1]);
			new_simplex.add_point(points_simplex[3]);
			*simplex = new_simplex;
			return iterate_triangle(simplex);
		}
	return vec3(0.0f);
}

/**
* adds given point to the simplex
* @param point - to add
*/
void gjk_simplex::add_point(const vec3 & point){
	//get the index of where to put it
	size_t index = point_count();
	//just so we dont get out of bounds
	assert(index < 4);
	//set the free one to be the new point
	m_simplex[index] = point;
	//this way the added point will always be at the end
	m_simplex_active[index] = 1;
}

/**
* removes the given point from the simplex
* @param posiiton - of the point to remove
*/
void gjk_simplex::remove_point(const size_t & position){
	//check size
	assert(position < 4);
	//change the active one
	m_simplex_active[position] = 0;
	reduce();
}

/**
* moves the information in order to have it contiguous
*/
void gjk_simplex::reduce(){
	//loop until we have all
	std::array<vec3, 4> new_array{};
	std::bitset<4> new_active{0};
	//fill it with all the new information
	int i{0}, j{0};
	for (; i < 4; i++) {
		if (m_simplex_active[i]) {
			new_array[j] = m_simplex[i];
			new_active[j] = 1;
			j++;
		}
	}
	//just update new ones
	m_simplex = new_array;
	m_simplex_active = new_active;
}

/**
* iterates through the points in the simplex and discards the points that do not form the simplex closer to the origin
* and returns the direction where the origin is lovated from the new simplex.
* @retun direction of the origin
*/
vec3 gjk_simplex::iterate(){
	//check if the origin is inside the simplex
	const size_t count_simplex = point_count();

	//check if it is a line
	if (count_simplex == 2) {
		return iterate_line(this);
	}

	//when the simplex is a triangle
	if (count_simplex == 3) {
		return iterate_triangle(this);
	}

	//when the simplex is a tetrahedron
	{
		return iterate_tetrahedron(this);
	}
}