[GeoLib] Restructured and fix bug in isPointInTriangle code.

Impl. a new algorithm for checking if a point is within a triangle and removed old sources for this task.

GeoLib/AnalyticalGeometry.cpp

@@ -184,30 +184,51 @@ bool lineSegmentsIntersect(const GeoLib::Polyline* ply,
 	return false;
 }
 
-static
-bool isPointInTriangle(const double p[3], const double a[3], const double b[3], const double c[3])
+bool isPointInTriangle(const GeoLib::Point* p, const GeoLib::Point* a, const GeoLib::Point* b,
+                       const GeoLib::Point* c)
+{
+	return isPointInTriangle(*p, *a, *b, *c);
+}
+
+bool isPointInTriangle(GeoLib::Point const& q,
+                       GeoLib::Point const& a,
+                       GeoLib::Point const& b,
+                       GeoLib::Point const& c,
+                       double eps)
 {
-	// criterion: p-b = u0 * (b - a) + u1 * (b - c); 0 <= u0, u1 <= 1, u0+u1 <= 1
-	MathLib::DenseMatrix<double> mat(2, 2);
-	mat(0, 0) = a[0] - b[0];
-	mat(0, 1) = c[0] - b[0];
-	mat(1, 0) = a[1] - b[1];
-	mat(1, 1) = c[1] - b[1];
-	double rhs[2] = { p[0] - b[0], p[1] - b[1] };
+	MathLib::Vector3 const v(a, b);
+	MathLib::Vector3 const w(a, c);
+
+	MathLib::DenseMatrix<double> mat (2,2);
+	mat(0,0) = v.getSqrLength();
+	mat(0,1) = v[0]*w[0] + v[1]*w[1] + v[2]*w[2];
+	mat(1,0) = mat(0,1);
+	mat(1,1) = w.getSqrLength();
+	double y[2] = {
+		v[0] * (q[0] - a[0]) + v[1] * (q[1] - a[1]) + v[2] * (q[2] - a[2]),
+		w[0] * (q[0] - a[0]) + w[1] * (q[1] - a[1]) + w[2] * (q[2] - a[2])
+	};
 
 	MathLib::GaussAlgorithm<MathLib::DenseMatrix<double>, double*> gauss(mat);
-	gauss.solve(rhs);
+	gauss.solve(y);
+
+	const double lower (std::numeric_limits<float>::epsilon());
+	const double upper (1 + lower);
+
+	if (-lower <= y[0] && y[0] <= upper && -lower <= y[1] && y[1] <= upper && y[0] + y[1] <=
+	    upper) {
+		GeoLib::Point const q_projected(
+			a[0] + y[0] * v[0] + y[1] * w[0],
+			a[1] + y[0] * v[1] + y[1] * w[1],
+			a[2] + y[0] * v[2] + y[1] * w[2]
+		);
+		if (MathLib::sqrDist(q, q_projected) < eps)
+			return true;
+	}
 
-	if (0 <= rhs[0] && rhs[0] <= 1 && 0 <= rhs[1] && rhs[1] <= 1 && rhs[0] + rhs[1] <= 1)
-		return true;
 	return false;
 }
 
-bool isPointInTriangle(const GeoLib::Point* p, const GeoLib::Point* a, const GeoLib::Point* b,
-                       const GeoLib::Point* c)
-{
-	return isPointInTriangle(p->getCoords(), a->getCoords(), b->getCoords(), c->getCoords());
-}
 
 static
 double getOrientedTriArea(GeoLib::Point const& a, GeoLib::Point const& b, GeoLib::Point const& c)
@@ -218,36 +239,6 @@ double getOrientedTriArea(GeoLib::Point const& a, GeoLib::Point const& b, GeoLib
 	return 0.5 * sqrt(MathLib::scalarProduct(w, w));
 }
 
-bool isPointInTriangle(GeoLib::Point const& p, GeoLib::Point const& a, GeoLib::Point const& b,
-                       GeoLib::Point const& c, double eps)
-{
-	const unsigned dim(3);
-	MathLib::DenseMatrix<double> m(dim, dim);
-	for (unsigned i(0); i < dim; i++)
-		m(i, 0) = b[i] - a[i];
-	for (unsigned i(0); i < dim; i++)
-		m(i, 1) = c[i] - a[i];
-	for (unsigned i(0); i < dim; i++)
-		m(i, 2) = p[i] - a[i];
-
-	// point p is in the same plane as the triangle if and only if
-	// the following determinate of the 3x3 matrix equals zero (up to an eps)
-	double det3x3(m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2))
-	              - m(1, 0) * (m(2, 1) * m(0, 2) - m(0, 1) * m(2, 2))
-	              + m(2, 0) * (m(0, 1) * m(1, 2) - m(1, 1) * m(0, 2)));
-	if (fabs(det3x3) > eps)
-		return false;
-
-	double total_area(getOrientedTriArea(a, b, c));
-	double abp_area(getOrientedTriArea(a, b, p));
-	double bcp_area(getOrientedTriArea(b, c, p));
-	double cap_area(getOrientedTriArea(c, a, p));
-
-	if (fabs(abp_area + bcp_area + cap_area - total_area) < eps)
-		return true;
-	return false;
-}
-
 // NewellPlane from book Real-Time Collision detection p. 494
 void getNewellPlane(const std::vector<GeoLib::Point*>& pnts, MathLib::Vector3 &plane_normal, double& d)
 {

GeoLib/AnalyticalGeometry.h

@@ -101,9 +101,19 @@ void rotatePointsToXZ(std::vector<GeoLib::Point*> &pnts);
 bool isPointInTriangle (const GeoLib::Point* p,
 		const GeoLib::Point* a, const GeoLib::Point* b, const GeoLib::Point* c);
 
+/**
+ * Tests if the given point p is within the triangle, defined by its edge nodes a, b and c.
+ * Using the eps it is possible to test a 'epsilon' neighbourhood around the triangle.
+ * @param p test point
+ * @param a edge node of triangle
+ * @param b edge node of triangle
+ * @param c edge node of triangle
+ * @param eps size of neighbourhood (orthogonal distance to the plane spaned by triangle)
+ * @return true if the test point p is within the 'epsilon'-neighbourhood of the triangle
+ */
 bool isPointInTriangle(GeoLib::Point const& p,
 				GeoLib::Point const& a, GeoLib::Point const& b, GeoLib::Point const& c,
-				double eps = std::numeric_limits<double>::epsilon());
+				double eps = std::numeric_limits<float>::epsilon());
 
 /**
  * test for intersections of the line segments of the Polyline