Commit f25428c3 authored by Libor Moravčík's avatar Libor Moravčík
Browse files

Definitions of functions moved from math/geometry/utils.hpp to .cpp file

parent 977fd7a5

math/include/math/geometry/utils.hpp

+28 −371
Original line number Diff line number Diff line
@@ -9,6 +9,7 @@ 


namespace geometry

{



struct ClosestPoints

{

	scalar distance;
@@ -28,392 +29,48 @@ struct ClosestPoints 
	}

};



inline ClosestPoints closest_points_point_line(const vec3 &point, const vec3 &a, const vec3 &b,

					       const scalar epsilon = Num::epsilon)

{

	const auto ab = b - a;

	const auto ap = point - a;



	const scalar ab_length2 = dot(ab, ab);



	// Handle degenerate case where a and b are the same point

	if (ab_length2 < epsilon)

	{

		return {distance(point, a), point, a};

	}



	const auto t = dot(ap, ab) / ab_length2;

	const auto clamped_t = clamp(t, 0.0f, 1.0f);

	const auto projection = a + ab * clamped_t;

	return {distance(point, projection), point, projection};

}



inline ClosestPoints closest_points_point_triangle(const vec3 &point, const vec3 &a, const vec3 &b,

						   const vec3 &c)

{

	const auto ab = b - a;

	const auto ac = c - a;

	const auto ap = point - a;



	// Check if point is in vertex region outside A

	const auto d1 = dot(ab, ap);

	const auto d2 = dot(ac, ap);

	if (d1 <= 0.0f && d2 <= 0.0f)

	{

		return {distance(point, a), point,

			a}; // A is closest, barycentric coordinates (1, 0, 0)

	}



	// Check if point is in vertex region outside B

	const auto bp = point - b;

	const auto d3 = dot(ab, bp);

	const auto d4 = dot(ac, bp);

	if (d3 >= 0.0f && d4 <= d3)

	{

		return {distance(point, b), point,

			b}; // B is closest, barycentric coordinates (0, 1, 0)

	}



	// Check if point is in edge region of AB, if so return projection on AB

	const auto vc = d1 * d4 - d3 * d2;

	if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)

	{

		const auto v = d1 / (d1 - d3);

		const auto projection = a + ab * v;

		return {distance(point, projection), point,

			projection}; // AB is closest, barycentric coordinates (1-v, v, 0)

	}



	// Check if point is in vertex region outside C

	const auto cp = point - c;

	const auto d5 = dot(ab, cp);

	const auto d6 = dot(ac, cp);

	if (d6 >= 0.0f && d5 <= d6)

	{

		return {distance(point, c), point,

			c}; // C is closest, barycentric coordinates (0, 0, 1)

	}



	// Check if point is in edge region of AC, if so return projection on AC

	const auto vb = d5 * d2 - d1 * d6;

	if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)

	{

		const auto w = d2 / (d2 - d6);

		const auto projection = a + ac * w;

		return {distance(point, projection), point,

			projection}; // AC is closest, barycentric coordinates (1-w, 0, w)

	}



	// Check if point is in edge region of BC, if so return projection on BC

	const auto va = d3 * d6 - d5 * d4;

	if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)

	{

		const auto w = (d4 - d3) / ((d4 - d3) + (d5 - d6));

		const auto projection = b + (c - b) * w;

		return {distance(point, projection), point,

			projection}; // BC is closest, barycentric coordinates (0, 1-w, w)

	}



	// Point is inside the triangle, return projection on the triangle

	const auto denom = 1.0f / (va + vb + vc);

	const auto v = vb * denom;

	const auto w = vc * denom;

	const auto projection = a + ab * v + ac * w;

	return {distance(point, projection), point, projection}; // Inside the triangle

}



inline ClosestPoints closest_points_point_face(const vec3 &point, const std::vector<vec3> &vertices)

{

	// Split the face into triangles and calculate the distance to each of them

	ClosestPoints min_distance = ClosestPoints::max();

	for (size_t i = 1; i < vertices.size() - 1; i++)

	{

		const auto distance =

		    closest_points_point_triangle(point, vertices[0], vertices[i], vertices[i + 1]);

		min_distance = std::min(min_distance, distance);

	}

	return min_distance;

}



inline ClosestPoints closest_points_line_line(const vec3 &a1, const vec3 &a2, const vec3 &b1,

					      const vec3 &b2, const scalar epsilon = Num::epsilon)

{

	const auto d1 = a2 - a1;

	const auto d2 = b2 - b1;



	const auto r = a1 - b1;

	const auto a = dot(d1, d1);

	const auto e = dot(d2, d2);

	const auto f = dot(d2, r);



	// Check if some or both lines are degenerate

	if (a < epsilon && e < epsilon)

	{

		return {distance(a1, b1), a1, b1};

	}

	if (a < epsilon)

	{

		return closest_points_point_line(a1, b1, b2);

	}

	if (e < epsilon)

	{

		return closest_points_point_line(b1, a1, a2);

	}



	// If the lines are not degenerate, calculate the closest points

	scalar b = dot(d1, d2);

	scalar c = dot(d1, r);

	scalar denom = a * e - b * b;



	// Calculate the parameters of the closest points

	scalar s;

	if (denom > epsilon)

	{

		s = clamp((b * f - c * e) / denom, 0.0f, 1.0f);

	}

	else

	{

		s = 0.0f;

	}



	scalar t = (b * s + f) / e;

	if (t < 0.0f)

	{

		t = 0.0f;

		s = clamp(-c / a, 0.0f, 1.0f);

	}

	else if (t > 1.0f)

	{

		t = 1.0f;

		s = clamp((b - c) / a, 0.0f, 1.0f);

	}



	// Get the closest points on the lines

	const auto c1 = a1 + d1 * s;

	const auto c2 = b1 + d2 * t;



	return {distance(c1, c2), c1, c2};

}



inline std::optional<vec3> line_triangle_intersection(const vec3 &p0, const vec3 &p1,

						      const vec3 &v0, const vec3 &v1,

						      const vec3 &v2,

						      const scalar epsilon = Num::epsilon)

{

	// Möller–Trumbore intersection algorithm

	const vec3 edge1 = {v1.x - v0.x, v1.y - v0.y, v1.z - v0.z};

	const vec3 edge2 = {v2.x - v0.x, v2.y - v0.y, v2.z - v0.z};



	// Compute the intersection normal

	const auto h = cross(p1 - p0, edge2);

	const auto a = dot(edge1, h);

ClosestPoints closest_points_point_line(const vec3 &point, const vec3 &a, const vec3 &b,

					const scalar epsilon = Num::epsilon);



	if (std::abs(a) < epsilon) return std::nullopt; // Line is parallel or coplanar

ClosestPoints closest_points_point_triangle(const vec3 &point, const vec3 &a, const vec3 &b,

					    const vec3 &c);



	const auto f = 1.0f / a;

	vec3 s = {p0.x - v0.x, p0.y - v0.y, p0.z - v0.z};

	const auto u = f * dot(s, h);

ClosestPoints closest_points_point_face(const vec3 &point, const std::vector<vec3> &vertices);



	if (u < 0.0f || u > 1.0f) return std::nullopt; // Intersection is outside the triangle

ClosestPoints closest_points_line_line(const vec3 &a1, const vec3 &a2, const vec3 &b1,

				       const vec3 &b2, const scalar epsilon = Num::epsilon);



	const auto q = cross(s, edge1);

	const auto v = f * dot(p1 - p0, q);



	if (v < 0.0f || u + v > 1.0f) return std::nullopt; // Intersection is outside the triangle



	const auto t = f * dot(edge2, q);



	if (t < 0.0f || t > 1.0f) return std::nullopt; // Intersection is outside the line segment



	// Intersection point

	vec3 intersection = {p0.x + (p1.x - p0.x) * t, p0.y + (p1.y - p0.y) * t,

			     p0.z + (p1.z - p0.z) * t};



	return intersection;

}



inline ClosestPoints closest_points_line_triangle(const vec3 &a, const vec3 &b, const vec3 &v0,

std::optional<vec3> line_triangle_intersection(const vec3 &p0, const vec3 &p1, const vec3 &v0,

					       const vec3 &v1, const vec3 &v2,

						  const scalar epsilon = Num::epsilon)

{

	// Check for intersection

	if (const auto intersection = line_triangle_intersection(a, b, v0, v1, v2, epsilon))

	{

		return {0.0f, *intersection, *intersection};

	}



	// Compute distance between the line and each of the edges

	const auto d0 = closest_points_line_line(a, b, v0, v1, epsilon);

	const auto d1 = closest_points_line_line(a, b, v1, v2, epsilon);

	const auto d2 = closest_points_line_line(a, b, v2, v0, epsilon);



	// Compute distance between endpoints and the triangle

	const auto d3 = closest_points_point_triangle(a, v0, v1, v2);

	const auto d4 = closest_points_point_triangle(b, v0, v1, v2);

					       const scalar epsilon = Num::epsilon);



	// Return the minimum distance

	return std::min({d0, d1, d2, d3, d4});

}

ClosestPoints closest_points_line_triangle(const vec3 &a, const vec3 &b, const vec3 &v0,

					   const vec3 &v1, const vec3 &v2,

					   const scalar epsilon = Num::epsilon);



inline ClosestPoints closest_points_line_face(const vec3 &a, const vec3 &b,

ClosestPoints closest_points_line_face(const vec3 &a, const vec3 &b,

				       const std::vector<vec3> &vertices,

					      const scalar epsilon = Num::epsilon)

{

	// Split the face into triangles and calculate the distance to each of them

	ClosestPoints min_distance = ClosestPoints::max();

	for (size_t i = 1; i < vertices.size() - 1; i++)

	{

		const auto distance = closest_points_line_triangle(a, b, vertices[0], vertices[i],

								   vertices[i + 1], epsilon);

		min_distance = std::min(min_distance, distance);

	}

	return min_distance;

}





inline ClosestPoints closest_points_triangle_triangle(const vec3 &a0, const vec3 &a1,

						      const vec3 &a2, const vec3 &b0,

						      const vec3 &b1, const vec3 &b2,

						      const scalar epsilon = Num::epsilon)

{

	auto distances = std::vector<ClosestPoints>();

	const auto triangle_a = std::vector{a0, a1, a2};

	const auto triangle_b = std::vector{b0, b1, b2};



	// Compute the distance between each pair of edges

	for (size_t i = 0; i < 3; i++)

	{

		for (size_t j = 0; j < 3; j++)

		{

			const auto distance = closest_points_line_line(

			    triangle_a[i], triangle_a[(i + 1) % 3], triangle_b[j],

			    triangle_b[(j + 1) % 3], epsilon);

			distances.push_back(distance);

		}

	}



	// Compute the distance between the vertices and the opposite triangle

	for (size_t i = 0; i < 3; i++)

	{

		const auto distance = closest_points_point_triangle(triangle_a[i], b0, b1, b2);

		distances.push_back(distance);

	}

	for (size_t i = 0; i < 3; i++)

	{

		const auto distance = closest_points_point_triangle(triangle_b[i], a0, a1, a2);

		distances.push_back(distance);

	}

				       const scalar epsilon = Num::epsilon);



	return *std::ranges::min_element(distances);

}

ClosestPoints closest_points_triangle_triangle(const vec3 &a0, const vec3 &a1, const vec3 &a2,

					       const vec3 &b0, const vec3 &b1, const vec3 &b2,

					       const scalar epsilon = Num::epsilon);



inline ClosestPoints closest_points_face_face(const std::vector<vec3> &vertices_a,

ClosestPoints closest_points_face_face(const std::vector<vec3> &vertices_a,

				       const std::vector<vec3> &vertices_b,

					      const scalar epsilon = Num::epsilon)

{

	// Split the faces into triangles and calculate the distance to each pair of triangles

	ClosestPoints min_distance = ClosestPoints::max();

	for (size_t i = 1; i < vertices_a.size() - 1; i++)

	{

		for (size_t j = 1; j < vertices_b.size() - 1; j++)

		{

			const auto distance = closest_points_triangle_triangle(

			    vertices_a[0], vertices_a[i], vertices_a[i + 1], vertices_b[0],

			    vertices_b[j], vertices_b[j + 1], epsilon);

			min_distance = std::min(min_distance, distance);

		}

	}

	return min_distance;

}



inline bool convex_test(const std::vector<vec3> &points,

			const std::vector<std::array<size_t, 3>> &indices)

{

	for (const auto &face : indices)

	{

		const auto &v0 = points[face[0]];

		const auto &v1 = points[face[1]];

		const auto &v2 = points[face[2]];



		const auto edge1 = v1 - v0;

		const auto edge2 = v2 - v0;



		const auto normal = normalized(cross(edge1, edge2));

				       const scalar epsilon = Num::epsilon);



		double d = -dot(normal, v0);



		for (size_t i = 0; i < points.size(); ++i)

		{

			if (i == face[0] || i == face[1] || i == face[2]) continue;



			const auto p = points[i];

			const auto distance = dot(normal, p) + d;



			if (distance > Num::epsilon) return false;

		}

	}



	return true;

}

bool convex_test(const std::vector<vec3> &points,

		 const std::vector<std::array<size_t, 3>> &indices);



// Compute barycentric coordinates for

// point p with respect to triangle (a, b, c)

// code taken from (page 2): https://ceng2.ktu.edu.tr/~cakir/files/grafikler/Texture_Mapping.pdf

inline vec3 barycentric(const vec3 &a, const vec3 &b, const vec3 &c, const vec3 &p = {0, 0, 0})

{

	vec3 v0 = b - a, v1 = c - a, v2 = p - a;

	scalar d00 = dot(v0, v0);

	scalar d01 = dot(v0, v1);

	scalar d11 = dot(v1, v1);

	scalar d20 = dot(v2, v0);



	scalar d21 = dot(v2, v1);

	scalar denom = d00 * d11 - d01 * d01;

vec3 barycentric(const vec3 &a, const vec3 &b, const vec3 &c, const vec3 &p = {0, 0, 0});



	scalar v, w, u;

	v = (d11 * d20 - d01 * d21) / denom;

	w = (d00 * d21 - d01 * d20) / denom;

	u = 1.0f - v - w;

	return {u, v, w};

}

scalar distance_point_triangle(const vec3 &p, const vec3 &v1, const vec3 &v2, const vec3 &v3);



inline scalar distance_point_triangle(const vec3 &p, const vec3 &v1, const vec3 &v2, const vec3 &v3)

{

	// prepare data

	vec3 v21 = v2 - v1;

	vec3 p1 = p - v1;

	vec3 v32 = v3 - v2;

	vec3 p2 = p - v2;

	vec3 v13 = v1 - v3;

	vec3 p3 = p - v3;

	vec3 nor = cross(v21, v13);



	return sqrt( // inside/outside test

	    (sign(dot(cross(v21, nor), p1)) + sign(dot(cross(v32, nor), p2)) +

		 sign(dot(cross(v13, nor), p3)) <

	     2.0)

		?

		// 3 edges

		min(min(len2(v21 * clamp(dot(v21, p1) / len2(v21), 0.0, 1.0) - p1),

			len2(v32 * clamp(dot(v32, p2) / len2(v32), 0.0, 1.0) - p2)),

		    len2(v13 * clamp(dot(v13, p3) / len2(v13), 0.0, 1.0) - p3))

		:

		// 1 face

		dot(nor, p1) * dot(nor, p1) / len2(nor));

}



inline scalar distance_point_face(const vec3 &point, const std::vector<vec3> &vertices)

{

	// Split the face into triangles and calculate the distance to each of them

	scalar min_distance = FLT_MAX;

	for (size_t i = 1; i < vertices.size() - 1; i++)

	{

		const auto distance =

		    distance_point_triangle(point, vertices[0], vertices[i], vertices[i + 1]);

		min_distance = std::min(min_distance, distance);

	}

	return min_distance;

}

scalar distance_point_face(const vec3 &point, const std::vector<vec3> &vertices);



inline scalar distance_point_line(const vec3 &point, const vec3 &a, const vec3 &b)

{

math/src/geometry/utils.cpp

0 → 100644
+382 −0

File added.

Preview size limit exceeded, changes collapsed.