Compute proximity of triangle-triangle in 3D

proximity.cpp

// Adapted from: https://github.com/flexible-collision-library/fcl

#include <Eigen/Geometry>

namespace Intersect{

	typedef double Scalar;
	typedef Eigen::Vector3d Vector3;

	const Scalar EPSILON = 1e-5;

	Scalar distanceToPlane(const Vector3& n, Scalar t, const Vector3& v)
	{
		return n.dot(v) - t;
	}

	bool buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, Scalar* t)
	{
		Vector3 n_ = (v2 - v1).cross(v3 - v1);

		//bool can_normalize = false;
		//n_.normalize(&can_normalize);
		//if(can_normalize)
		n_.normalize();
		{
			*n = n_;
			*t = n_.dot(v1);
			return true;
		}

		return false;
	}

	inline void computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, Scalar t, Scalar* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points)
	{
		*num_deepest_points = 0;
		Scalar max_depth = -std::numeric_limits<Scalar>::max();
		unsigned int num_deepest_points_ = 0;
		unsigned int num_neg = 0;
		unsigned int num_pos = 0;
		unsigned int num_zero = 0;

		for(unsigned int i = 0; i < num_clipped_points; ++i)
		{
			Scalar dist = -distanceToPlane(n, t, clipped_points[i]);
			if(dist > EPSILON) num_pos++;
			else if(dist < -EPSILON) num_neg++;
			else num_zero++;
			if(dist > max_depth)
			{
				max_depth = dist;
				num_deepest_points_ = 1;
				deepest_points[num_deepest_points_ - 1] = clipped_points[i];
			}
			else if(dist + 1e-6 >= max_depth)
			{
				num_deepest_points_++;
				deepest_points[num_deepest_points_ - 1] = clipped_points[i];
			}
		}

		if(max_depth < -EPSILON)
			num_deepest_points_ = 0;

		if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0)))
			num_deepest_points_ = 0;

		*penetration_depth = max_depth;
		*num_deepest_points = num_deepest_points_;
	}

	inline int project6(const Vector3& ax,
		const Vector3& p1, const Vector3& p2, const Vector3& p3,
		const Vector3& q1, const Vector3& q2, const Vector3& q3)
	{
		Scalar P1 = ax.dot(p1);
		Scalar P2 = ax.dot(p2);
		Scalar P3 = ax.dot(p3);
		Scalar Q1 = ax.dot(q1);
		Scalar Q2 = ax.dot(q2);
		Scalar Q3 = ax.dot(q3);

		Scalar mn1 = std::min(P1, std::min(P2, P3));
		Scalar mx2 = std::max(Q1, std::max(Q2, Q3));
		if(mn1 > mx2) return 0;

		Scalar mx1 = std::max(P1, std::max(P2, P3));
		Scalar mn2 = std::min(Q1, std::min(Q2, Q3));

		if(mn2 > mx1) return 0;
		return 1;
	}

	inline bool intersect_Triangle(const Vector3& P1, const Vector3& P2, const Vector3& P3,
		const Vector3& Q1, const Vector3& Q2, const Vector3& Q3,
		Vector3* contact_points,
		unsigned int* num_contact_points,
		Scalar* penetration_depth,
		Vector3* normal)
	{
		Vector3 p1 = P1 - P1;
		Vector3 p2 = P2 - P1;
		Vector3 p3 = P3 - P1;
		Vector3 q1 = Q1 - P1;
		Vector3 q2 = Q2 - P1;
		Vector3 q3 = Q3 - P1;

		Vector3 e1 = p2 - p1;
		Vector3 e2 = p3 - p2;
		Vector3 n1 = e1.cross(e2);
		if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 f1 = q2 - q1;
		Vector3 f2 = q3 - q2;
		Vector3 m1 = f1.cross(f2);
		if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 ef11 = e1.cross(f1);
		if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 ef12 = e1.cross(f2);
		if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 f3 = q1 - q3;
		Vector3 ef13 = e1.cross(f3);
		if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 ef21 = e2.cross(f1);
		if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 ef22 = e2.cross(f2);
		if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 ef23 = e2.cross(f3);
		if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 e3 = p1 - p3;
		Vector3 ef31 = e3.cross(f1);
		if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 ef32 = e3.cross(f2);
		if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 ef33 = e3.cross(f3);
		if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 g1 = e1.cross(n1);
		if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 g2 = e2.cross(n1);
		if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 g3 = e3.cross(n1);
		if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 h1 = f1.cross(m1);
		if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 h2 = f2.cross(m1);
		if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false;

		Vector3 h3 = f3.cross(m1);
		if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false;

		if(contact_points && num_contact_points && penetration_depth && normal)
		{
			Vector3 n1, n2;
			Scalar t1, t2;
			buildTrianglePlane(P1, P2, P3, &n1, &t1);
			buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);

			Vector3 deepest_points1[3];
			unsigned int num_deepest_points1 = 0;
			Vector3 deepest_points2[3];
			unsigned int num_deepest_points2 = 0;
			Scalar penetration_depth1, penetration_depth2;

			Vector3 P[3] = {P1, P2, P3};
			Vector3 Q[3] = {Q1, Q2, Q3};

			computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
			computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);

			if(penetration_depth1 > penetration_depth2)
			{
				*num_contact_points = std::min(num_deepest_points2, (unsigned int)2);
				for(unsigned int i = 0; i < *num_contact_points; ++i)
				{
					contact_points[i] = deepest_points2[i];
				}

				*normal = n1;
				*penetration_depth = penetration_depth2;
			}
			else
			{
				*num_contact_points = std::min(num_deepest_points1, (unsigned int)2);
				for(unsigned int i = 0; i < *num_contact_points; ++i)
				{
					contact_points[i] = deepest_points1[i];
				}

				*normal = -n2;
				*penetration_depth = penetration_depth1;
			}
		}

		return true;
	}
}