Skip to content

Commit 5e7b17f

Browse files
JoseAntFerJoseAntFer
committed
Return barycentric coordinates of the nearest point instead of the nearest point
1 parent e55a15c commit 5e7b17f

File tree

3 files changed

+91
-257
lines changed

3 files changed

+91
-257
lines changed

Diff for: ‎README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -21,9 +21,9 @@ tmd::Result result = mesh_distance.signed_distance({ x, y, z });
2121

2222
// Print result
2323
std::cout << "Signed distance: " << result.distance << std::endl;
24-
std::cout << "Nearest point: " << result.nearest_point << std::endl;
2524
std::cout << "Nearest entity: " << result.nearest_entity << std::endl;
2625
std::cout << "Nearest triangle index: " << result.triangle_id << std::endl;
26+
std::cout << "Nearest point barycentric coordinates: " << result.barycentric << std::endl;
2727
```
2828
2929
## What you need to know about TriangleMeshDistance

Diff for: ‎TriangleMeshDistance/include/tmd/TriangleMeshDistance.h

+79-255
Original file line numberDiff line numberDiff line change
@@ -73,14 +73,14 @@ namespace tmd
7373
/**
7474
* Computes the squared distance, the nearest entity (vertex, edge or face) and the nearest point from a point to a triangle.
7575
*/
76-
static double point_triangle_sq_unsigned(NearestEntity& nearest_entity, Vec3d& nearest_point, const Vec3d& point, const Vec3d& v0, const Vec3d& v1, const Vec3d& v2);
76+
static void point_triangle_sq_unsigned(double& distance_sq, NearestEntity& nearest_entity, Vec3d& barycentric, const Vec3d& point, const Vec3d& v0, const Vec3d& v1, const Vec3d& v2);
7777
// -----------------------------------
7878

7979
// Struct that contains the result of a distance query
8080
struct Result
8181
{
8282
double distance = std::numeric_limits<double>::max();
83-
Vec3d nearest_point;
83+
Vec3d barycentric;
8484
tmd::NearestEntity nearest_entity;
8585
int triangle_id = -1;
8686
};
@@ -287,7 +287,12 @@ inline tmd::Result tmd::TriangleMeshDistance::signed_distance(const std::array<d
287287
break;
288288
}
289289

290-
const Vec3d u = p - result.nearest_point;
290+
const Vec3d nearest_point(
291+
result.barycentric[0]*this->vertices[triangle[0]] +
292+
result.barycentric[1]*this->vertices[triangle[1]] +
293+
result.barycentric[2]*this->vertices[triangle[2]]
294+
);
295+
const Vec3d u = p - nearest_point;
291296
result.distance *= (u.dot(pseudonormal) >= 0.0) ? 1.0 : -1.0;
292297

293298
return result;
@@ -507,13 +512,14 @@ inline void tmd::TriangleMeshDistance::_query(Result& result, const Node& node,
507512
const Vec3d& v1 = this->vertices[triangle[1]];
508513
const Vec3d& v2 = this->vertices[triangle[2]];
509514

510-
Vec3d nearest_point;
515+
double distance_sq;
511516
tmd::NearestEntity nearest_entity;
512-
const double distance_sq = tmd::point_triangle_sq_unsigned(nearest_entity, nearest_point, point, v0, v1, v2);
517+
Vec3d barycentric;
518+
tmd::point_triangle_sq_unsigned(distance_sq, nearest_entity, barycentric, point, v0, v1, v2);
513519

514520
if (distance_sq < result.distance * result.distance) {
515-
result.nearest_point = nearest_point;
516521
result.nearest_entity = nearest_entity;
522+
result.barycentric = barycentric;
517523
result.distance = std::sqrt(distance_sq);
518524
result.triangle_id = triangle_id;
519525
}
@@ -547,260 +553,78 @@ inline void tmd::TriangleMeshDistance::_query(Result& result, const Node& node,
547553
}
548554
}
549555

550-
static double tmd::point_triangle_sq_unsigned(NearestEntity& nearest_entity, Vec3d& nearest_point, const Vec3d& point, const Vec3d& v0, const Vec3d& v1, const Vec3d& v2)
556+
static void tmd::point_triangle_sq_unsigned(double& distance_sq, NearestEntity& nearest_entity, Vec3d& barycentric, const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c)
551557
{
552-
Vec3d diff = v0 - point;
553-
Vec3d edge0 = v1 - v0;
554-
Vec3d edge1 = v2 - v0;
555-
double a00 = edge0.dot(edge0);
556-
double a01 = edge0.dot(edge1);
557-
double a11 = edge1.dot(edge1);
558-
double b0 = diff.dot(edge0);
559-
double b1 = diff.dot(edge1);
560-
double c = diff.dot(diff);
561-
double det = std::abs(a00 * a11 - a01 * a01);
562-
double s = a01 * b1 - a11 * b0;
563-
double t = a01 * b0 - a00 * b1;
564-
565-
double d2 = -1.0;
566-
567-
if (s + t <= det)
568-
{
569-
if (s < 0)
570-
{
571-
if (t < 0) // region 4
572-
{
573-
if (b0 < 0)
574-
{
575-
t = 0;
576-
if (-b0 >= a00)
577-
{
578-
nearest_entity = NearestEntity::V1;
579-
s = 1;
580-
d2 = a00 + (2) * b0 + c;
581-
}
582-
else
583-
{
584-
nearest_entity = NearestEntity::E01;
585-
s = -b0 / a00;
586-
d2 = b0 * s + c;
587-
}
588-
}
589-
else
590-
{
591-
s = 0;
592-
if (b1 >= 0)
593-
{
594-
nearest_entity = NearestEntity::V0;
595-
t = 0;
596-
d2 = c;
597-
}
598-
else if (-b1 >= a11)
599-
{
600-
nearest_entity = NearestEntity::V2;
601-
t = 1;
602-
d2 = a11 + (2) * b1 + c;
603-
}
604-
else
605-
{
606-
nearest_entity = NearestEntity::E02;
607-
t = -b1 / a11;
608-
d2 = b1 * t + c;
609-
}
610-
}
611-
}
612-
else // region 3
613-
{
614-
s = 0;
615-
if (b1 >= 0)
616-
{
617-
nearest_entity = NearestEntity::V0;
618-
t = 0;
619-
d2 = c;
620-
}
621-
else if (-b1 >= a11)
622-
{
623-
nearest_entity = NearestEntity::V2;
624-
t = 1;
625-
d2 = a11 + (2) * b1 + c;
626-
}
627-
else
628-
{
629-
nearest_entity = NearestEntity::E02;
630-
t = -b1 / a11;
631-
d2 = b1 * t + c;
632-
}
633-
}
634-
}
635-
else if (t < 0) // region 5
636-
{
637-
t = 0;
638-
if (b0 >= 0)
639-
{
640-
nearest_entity = NearestEntity::V0;
641-
s = 0;
642-
d2 = c;
643-
}
644-
else if (-b0 >= a00)
645-
{
646-
nearest_entity = NearestEntity::V1;
647-
s = 1;
648-
d2 = a00 + (2) * b0 + c;
649-
}
650-
else
651-
{
652-
nearest_entity = NearestEntity::E01;
653-
s = -b0 / a00;
654-
d2 = b0 * s + c;
655-
}
656-
}
657-
else // region 0
658-
{
659-
nearest_entity = NearestEntity::F;
660-
// minimum at interior point
661-
double invDet = (1) / det;
662-
s *= invDet;
663-
t *= invDet;
664-
d2 = s * (a00 * s + a01 * t + (2) * b0) +
665-
t * (a01 * s + a11 * t + (2) * b1) + c;
666-
}
558+
// This function is a modified version of the one found in the Real-Time Collision Detection book by Ericson.
559+
Vec3d ab = b - a;
560+
Vec3d ac = c - a;
561+
Vec3d bc = c - b;
562+
563+
// Compute parametric position s for projection P’ of P on AB
564+
double snom = (p - a).dot(ab), sdenom = (p - b).dot(a - b);
565+
// Compute parametric position t for projection P’ of P on AC
566+
double tnom = (p - a).dot(ac), tdenom = (p - c).dot(a - c);
567+
if (snom <= 0.0 && tnom <= 0.0) {
568+
nearest_entity = NearestEntity::V0;
569+
barycentric = { 1.0, 0.0, 0.0 };
570+
distance_sq = (p - a).squaredNorm();
571+
return;
667572
}
668-
else
669-
{
670-
double tmp0, tmp1, numer, denom;
671573

672-
if (s < 0) // region 2
673-
{
674-
tmp0 = a01 + b0;
675-
tmp1 = a11 + b1;
676-
if (tmp1 > tmp0)
677-
{
678-
numer = tmp1 - tmp0;
679-
denom = a00 - (2) * a01 + a11;
680-
if (numer >= denom)
681-
{
682-
nearest_entity = NearestEntity::V1;
683-
s = 1;
684-
t = 0;
685-
d2 = a00 + (2) * b0 + c;
686-
}
687-
else
688-
{
689-
nearest_entity = NearestEntity::E12;
690-
s = numer / denom;
691-
t = 1 - s;
692-
d2 = s * (a00 * s + a01 * t + (2) * b0) +
693-
t * (a01 * s + a11 * t + (2) * b1) + c;
694-
}
695-
}
696-
else
697-
{
698-
s = 0;
699-
if (tmp1 <= 0)
700-
{
701-
nearest_entity = NearestEntity::V2;
702-
t = 1;
703-
d2 = a11 + (2) * b1 + c;
704-
}
705-
else if (b1 >= 0)
706-
{
707-
nearest_entity = NearestEntity::V0;
708-
t = 0;
709-
d2 = c;
710-
}
711-
else
712-
{
713-
nearest_entity = NearestEntity::E02;
714-
t = -b1 / a11;
715-
d2 = b1 * t + c;
716-
}
717-
}
718-
}
719-
else if (t < 0) // region 6
720-
{
721-
tmp0 = a01 + b1;
722-
tmp1 = a00 + b0;
723-
if (tmp1 > tmp0)
724-
{
725-
numer = tmp1 - tmp0;
726-
denom = a00 - (2) * a01 + a11;
727-
if (numer >= denom)
728-
{
729-
nearest_entity = NearestEntity::V2;
730-
t = 1;
731-
s = 0;
732-
d2 = a11 + (2) * b1 + c;
733-
}
734-
else
735-
{
736-
nearest_entity = NearestEntity::E12;
737-
t = numer / denom;
738-
s = 1 - t;
739-
d2 = s * (a00 * s + a01 * t + (2) * b0) +
740-
t * (a01 * s + a11 * t + (2) * b1) + c;
741-
}
742-
}
743-
else
744-
{
745-
t = 0;
746-
if (tmp1 <= 0)
747-
{
748-
nearest_entity = NearestEntity::V1;
749-
s = 1;
750-
d2 = a00 + (2) * b0 + c;
751-
}
752-
else if (b0 >= 0)
753-
{
754-
nearest_entity = NearestEntity::V0;
755-
s = 0;
756-
d2 = c;
757-
}
758-
else
759-
{
760-
nearest_entity = NearestEntity::E01;
761-
s = -b0 / a00;
762-
d2 = b0 * s + c;
763-
}
764-
}
765-
}
766-
else // region 1
767-
{
768-
numer = a11 + b1 - a01 - b0;
769-
if (numer <= 0)
770-
{
771-
nearest_entity = NearestEntity::V2;
772-
s = 0;
773-
t = 1;
774-
d2 = a11 + (2) * b1 + c;
775-
}
776-
else
777-
{
778-
denom = a00 - (2) * a01 + a11;
779-
if (numer >= denom)
780-
{
781-
nearest_entity = NearestEntity::V1;
782-
s = 1;
783-
t = 0;
784-
d2 = a00 + (2) * b0 + c;
785-
}
786-
else
787-
{
788-
nearest_entity = NearestEntity::E12;
789-
s = numer / denom;
790-
t = 1 - s;
791-
d2 = s * (a00 * s + a01 * t + (2) * b0) +
792-
t * (a01 * s + a11 * t + (2) * b1) + c;
793-
}
794-
}
795-
}
574+
// Compute parametric position u for projection P’ of P on BC
575+
double unom = (p - b).dot(bc), udenom = (p - c).dot(b - c);
576+
if (sdenom <= 0.0 && unom <= 0.0) {
577+
nearest_entity = NearestEntity::V1;
578+
barycentric = { 0.0, 1.0, 0.0 };
579+
distance_sq = (p - b).squaredNorm();
580+
return;
581+
}
582+
if (tdenom <= 0.0 && udenom <= 0.0) {
583+
nearest_entity = NearestEntity::V2;
584+
barycentric = { 0.0, 0.0, 1.0 };
585+
distance_sq = (p - c).squaredNorm();
586+
return;
796587
}
797588

798-
// Account for numerical round-off error.
799-
if (d2 < 0)
800-
{
801-
d2 = 0;
589+
// Normal for the triangle
590+
Vec3d n = ab.cross(ac);
591+
592+
// Check if P is outside AB
593+
double vc = n.dot((a - p).cross(b - p));
594+
if (vc <= 0.0 && snom >= 0.0 && sdenom >= 0.0) {
595+
double arc = snom / (snom + sdenom);
596+
nearest_entity = NearestEntity::E01;
597+
barycentric = { 1.0 - arc, arc, 0.0 };
598+
distance_sq = (p - (barycentric[0]*a + barycentric[1]*b)).squaredNorm();
599+
return;
600+
}
601+
602+
// Check if P is outside BC
603+
double va = n.dot((b - p).cross(c - p));
604+
if (va <= 0.0 && unom >= 0.0 && udenom >= 0.0) {
605+
double arc = unom / (unom + udenom);
606+
nearest_entity = NearestEntity::E12;
607+
barycentric = { 0.0, 1.0 - arc, arc };
608+
distance_sq = (p - (barycentric[1]*b + barycentric[2]*c)).squaredNorm();
609+
return;
610+
}
611+
612+
// Check if P is outside AC
613+
double vb = n.dot((c - p).cross(a - p));
614+
if (vb <= 0.0 && tnom >= 0.0 && tdenom >= 0.0) {
615+
double arc = tnom / (tnom + tdenom);
616+
nearest_entity = NearestEntity::E02;
617+
barycentric = { 1.0 - arc, 0.0, arc };
618+
distance_sq = (p - (barycentric[0]*a + barycentric[2]*c)).squaredNorm();
619+
return;
802620
}
803621

804-
nearest_point = v0 + s * edge0 + t * edge1;
805-
return d2;
622+
// P must project inside the triangle; compute using barycentric coordinates
623+
double u = va / (va + vb + vc);
624+
double v = vb / (va + vb + vc);
625+
double w = 1.0 - u - v; // = vc / (va + vb + vc)
626+
nearest_entity = NearestEntity::F;
627+
barycentric = { u, v, w };
628+
distance_sq = (p - (u*a + v*b + w*c)).squaredNorm();
629+
return;
806630
}

0 commit comments

Comments
 (0)