Using Polyline::getPoint() for accessing polyline data.

MathLib/AnalyticalGeometry.cpp

@@ -12,6 +12,8 @@
  *
  */
 
+#include "AnalyticalGeometry.h"
+
 #include <algorithm>
 #include <cmath>
 #include <cstdlib> // for exit
@@ -19,30 +21,28 @@
 #include <limits>
 #include <list>
 
-// Base
+// BaseLib
 #include "quicksort.h"
 
-// GEO
+// GeoLib
 #include "Polyline.h"
 #include "Triangle.h"
 
 // MathLib
-#include "AnalyticalGeometry.h"
 #include "LinAlg/Dense/Matrix.h" // for transformation matrix
 #include "LinAlg/Solvers/GaussAlgorithm.h"
 #include "MathTools.h"
 
 namespace MathLib
 {
-Orientation getOrientation (const double& p0_x, const double& p0_y,
-                            const double& p1_x, const double& p1_y,
-                            const double& p2_x, const double& p2_y)
+Orientation getOrientation(const double& p0_x, const double& p0_y, const double& p1_x,
+                           const double& p1_y, const double& p2_x, const double& p2_y)
 {
-	double h1 ((p1_x - p0_x) * (p2_y - p0_y));
-	double h2 ((p2_x - p0_x) * (p1_y - p0_y));
+	double h1((p1_x - p0_x) * (p2_y - p0_y));
+	double h2((p2_x - p0_x) * (p1_y - p0_y));
 
-	double tol (sqrt(std::numeric_limits<double>::min()));
-	if (fabs (h1 - h2) <= tol * std::max (fabs(h1), fabs(h2)))
+	double tol(sqrt( std::numeric_limits<double>::min()));
+	if (fabs(h1 - h2) <= tol * std::max(fabs(h1), fabs(h2)))
 		return COLLINEAR;
 	if (h1 - h2 > 0.0)
 		return CCW;
@@ -50,20 +50,18 @@ Orientation getOrientation (const double& p0_x, const double& p0_y,
 	return CW;
 }
 
-Orientation getOrientation (const GeoLib::Point* p0,
-                            const GeoLib::Point* p1,
-                            const GeoLib::Point* p2)
+Orientation getOrientation(const GeoLib::Point* p0, const GeoLib::Point* p1,
+                           const GeoLib::Point* p2)
 {
-	return getOrientation ((*p0)[0], (*p0)[1], (*p1)[0], (*p1)[1], (*p2)[0], (*p2)[1]);
+	return getOrientation((*p0)[0], (*p0)[1], (*p1)[0], (*p1)[1], (*p2)[0], (*p2)[1]);
 }
 
-bool lineSegmentIntersect (const GeoLib::Point& a, const GeoLib::Point& b,
-                           const GeoLib::Point& c, const GeoLib::Point& d,
-                           GeoLib::Point& s)
+bool lineSegmentIntersect(const GeoLib::Point& a, const GeoLib::Point& b, const GeoLib::Point& c,
+                          const GeoLib::Point& d, GeoLib::Point& s)
 {
-	Matrix<double> mat(2,2);
-	mat(0,0) = b[0] - a[0];
-	mat(1,0) = b[1] - a[1];
+	Matrix<double> mat(2, 2);
+	mat(0, 0) = b[0] - a[0];
+	mat(1, 0) = b[1] - a[1];
 	mat(0,1) = c[0] - d[0];
 	mat(1,1) = c[1] - d[1];
 
@@ -78,9 +76,9 @@ bool lineSegmentIntersect (const GeoLib::Point& a, const GeoLib::Point& b,
 	} else {
 		// vector (D-C) is not parallel to x-axis
 		if (fabs(mat(0,1)) >= eps) {
-		// vector (B-A) is not parallel to x-axis
-		// \f$(B-A)\f$ and \f$(D-C)\f$ are parallel iff there exists
-		// a constant \f$c\f$ such that \f$(B-A) = c (D-C)\f$
+			// vector (B-A) is not parallel to x-axis
+			// \f$(B-A)\f$ and \f$(D-C)\f$ are parallel iff there exists
+			// a constant \f$c\f$ such that \f$(B-A) = c (D-C)\f$
 			if (fabs (mat(0,0) / mat(0,1) - mat(1,0) / mat(1,1)) < eps * fabs (mat(0,0) / mat(0,1)))
 				return false;
 		}
@@ -103,14 +101,16 @@ bool lineSegmentIntersect (const GeoLib::Point& a, const GeoLib::Point& b,
 			return true;
 		else
 			return false;
-	} else delete [] rhs;
+	}
+	else
+		delete [] rhs;
 	return false;
 }
 
 bool lineSegmentsIntersect(const GeoLib::Polyline* ply, size_t &idx0, size_t &idx1,
                            GeoLib::Point& intersection_pnt)
 {
-	size_t n_segs (ply->getNumberOfPoints() - 1);
+	size_t n_segs(ply->getNumberOfPoints() - 1);
 	/**
 	 * computing the intersections of all possible pairs of line segments of the given polyline
 	 * as follows:
@@ -118,9 +118,9 @@ bool lineSegmentsIntersect(const GeoLib::Polyline* ply, size_t &idx0, size_t &id
 	 * of the polyline and segment \f$s_2 = (C,B)\f$ defined by \f$j\f$-th and
 	 * \f$j+1\f$-st point of the polyline, \f$j>k+1\f$
 	 */
-	for (size_t k(0); k<n_segs-2; k++) {
-		for (size_t j(k+2); j<n_segs; j++) {
-			if (k!=0 || j<n_segs-1) {
+	for (size_t k(0); k < n_segs - 2; k++) {
+		for (size_t j(k + 2); j < n_segs; j++) {
+			if (k != 0 || j < n_segs - 1) {
 				if (lineSegmentIntersect(*(ply->getPoint(k)), *(ply->getPoint(k + 1)),
 				                         *(ply->getPoint(j)), *(ply->getPoint(j + 1)),
 				                         intersection_pnt)) {
@@ -134,58 +134,56 @@ bool lineSegmentsIntersect(const GeoLib::Polyline* ply, size_t &idx0, size_t &id
 	return false;
 }
 
-bool isPointInTriangle (const double p[3], const double a[3], const double b[3], const double c[3])
+bool isPointInTriangle(const double p[3], const double a[3], const double b[3], const double c[3])
 {
 	// criterion: p-b = u0 * (b - a) + u1 * (b - c); 0 <= u0, u1 <= 1, u0+u1 <= 1
-	MathLib::Matrix<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::Matrix<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::GaussAlgorithm gauss (mat);
-	gauss.execute (rhs);
+	MathLib::GaussAlgorithm gauss(mat);
+	gauss.execute(rhs);
 
 	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)
+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());
+	return isPointInTriangle(p->getCoords(), a->getCoords(), b->getCoords(), c->getCoords());
 }
 
-double getOrientedTriArea(GeoLib::Point const& a, GeoLib::Point const& b,
-                          GeoLib::Point const& c)
+double getOrientedTriArea(GeoLib::Point const& a, GeoLib::Point const& b, GeoLib::Point const& c)
 {
-	const double u[3] = {c[0] - a[0], c[1] - a[1], c[2] - a[2]};
-	const double v[3] = {b[0] - a[0], b[1] - a[1], b[2] - a[2]};
+	const double u[3] = { c[0] - a[0], c[1] - a[1], c[2] - a[2] };
+	const double v[3] = { b[0] - a[0], b[1] - a[1], b[2] - a[2] };
 	double w[3];
 	MathLib::crossProd(u, v, w);
-	return 0.5 * sqrt(MathLib::scpr<double,3>(w,w));
+	return 0.5 * sqrt(MathLib::scpr<double, 3>(w, w));
 }
 
-bool isPointInTriangle(GeoLib::Point const& p, GeoLib::Point const& a,
-                       GeoLib::Point const& b, GeoLib::Point const& c,
-                       double eps)
+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::Matrix<double> m (dim,dim);
+	MathLib::Matrix<double> m(dim, dim);
 	for (unsigned i(0); i < dim; i++)
-		m(i,0) = b[i] - a[i];
+		m(i, 0) = b[i] - a[i];
 	for (unsigned i(0); i < dim; i++)
-		m(i,1) = c[i] - a[i];
+		m(i, 1) = c[i] - a[i];
 	for (unsigned i(0); i < dim; i++)
-		m(i,2) = p[i] - a[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)));
+	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;
 
@@ -200,12 +198,11 @@ bool isPointInTriangle(GeoLib::Point const& p, GeoLib::Point const& a,
 }
 
 // NewellPlane from book Real-Time Collision detection p. 494
-void getNewellPlane(const std::vector<GeoLib::Point*>& pnts, Vector &plane_normal,
-                    double& d)
+void getNewellPlane(const std::vector<GeoLib::Point*>& pnts, Vector &plane_normal, double& d)
 {
 	d = 0;
 	Vector centroid;
-	size_t n_pnts (pnts.size());
+	size_t n_pnts(pnts.size());
 	for (size_t i(n_pnts - 1), j(0); j < n_pnts; i = j, j++) {
 		plane_normal[0] += ((*(pnts[i]))[1] - (*(pnts[j]))[1])
 		                   * ((*(pnts[i]))[2] + (*(pnts[j]))[2]); // projection on yz
@@ -221,10 +218,9 @@ void getNewellPlane(const std::vector<GeoLib::Point*>& pnts, Vector &plane_norma
 	d = centroid.Dot(plane_normal) / n_pnts;
 }
 
-void rotatePointsToXY(Vector &plane_normal,
-                      std::vector<GeoLib::Point*> &pnts)
+void rotatePointsToXY(Vector &plane_normal, std::vector<GeoLib::Point*> &pnts)
 {
-	double small_value (sqrt (std::numeric_limits<double>::min()));
+	double small_value(sqrt( std::numeric_limits<double>::min()));
 	if (fabs(plane_normal[0]) < small_value && fabs(plane_normal[1]) < small_value)
 		return;
 
@@ -232,16 +228,16 @@ void rotatePointsToXY(Vector &plane_normal,
 	computeRotationMatrixToXY(plane_normal, rot_mat);
 	rotatePoints(rot_mat, pnts);
 
-	double* tmp (rot_mat * plane_normal.getCoords());
+	double* tmp(rot_mat * plane_normal.getCoords());
 	for (std::size_t j(0); j < 3; j++)
 		plane_normal[j] = tmp[j];
 
-	delete [] tmp;
+	delete[] tmp;
 }
 
 void rotatePointsToXZ(Vector &n, std::vector<GeoLib::Point*> &pnts)
 {
-	double small_value (sqrt (std::numeric_limits<double>::min()));
+	double small_value(sqrt( std::numeric_limits<double>::min()));
 	if (fabs(n[0]) < small_value && fabs(n[1]) < small_value)
 		return;
 
@@ -267,11 +263,11 @@ void rotatePointsToXZ(Vector &n, std::vector<GeoLib::Point*> &pnts)
 
 	rotatePoints(rot_mat, pnts);
 
-	double *tmp (rot_mat * n.getCoords());
+	double *tmp(rot_mat * n.getCoords());
 	for (std::size_t j(0); j < 3; j++)
 		n[j] = tmp[j];
 
-	delete [] tmp;
+	delete[] tmp;
 }
 
 void computeRotationMatrixToXY(Vector const& plane_normal, Matrix<double> & rot_mat)