|
@@ -184,6 +184,7 @@ namespace ClosestPoint
|
|
|
float best_dist_sq = inC.LengthSq();
|
|
|
|
|
|
// If the closest point must include C then A or B cannot be closest
|
|
|
+ // Note that we test vertices first because we want to prefer a closest vertex over a closest edge (this results in an outSet with fewer bits set)
|
|
|
if constexpr (!MustIncludeC)
|
|
|
{
|
|
|
// Try vertex A
|
|
@@ -203,21 +204,6 @@ namespace ClosestPoint
|
|
|
closest_point = inB;
|
|
|
best_dist_sq = b_len_sq;
|
|
|
}
|
|
|
-
|
|
|
- // Edge AB
|
|
|
- float ab_len_sq = ab.LengthSq();
|
|
|
- if (ab_len_sq > Square(FLT_EPSILON))
|
|
|
- {
|
|
|
- float v = Clamp(-a.Dot(ab) / ab_len_sq, 0.0f, 1.0f);
|
|
|
- Vec3 q = a + v * ab;
|
|
|
- float dist_sq = q.LengthSq();
|
|
|
- if (dist_sq < best_dist_sq)
|
|
|
- {
|
|
|
- closest_set = swap_ac.GetX()? 0b0110 : 0b0011;
|
|
|
- closest_point = q;
|
|
|
- best_dist_sq = dist_sq;
|
|
|
- }
|
|
|
- }
|
|
|
}
|
|
|
|
|
|
// Edge AC
|
|
@@ -236,7 +222,7 @@ namespace ClosestPoint
|
|
|
}
|
|
|
|
|
|
// Edge BC
|
|
|
- Vec3 bc = c - inB;
|
|
|
+ Vec3 bc = inC - inB;
|
|
|
float bc_len_sq = bc.LengthSq();
|
|
|
if (bc_len_sq > Square(FLT_EPSILON))
|
|
|
{
|
|
@@ -245,12 +231,32 @@ namespace ClosestPoint
|
|
|
float dist_sq = q.LengthSq();
|
|
|
if (dist_sq < best_dist_sq)
|
|
|
{
|
|
|
- closest_set = swap_ac.GetX()? 0b0011 : 0b0110;
|
|
|
+ closest_set = 0b0110;
|
|
|
closest_point = q;
|
|
|
best_dist_sq = dist_sq;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ // If the closest point must include C then AB cannot be closest
|
|
|
+ if constexpr (!MustIncludeC)
|
|
|
+ {
|
|
|
+ // Edge AB
|
|
|
+ ab = inB - inA;
|
|
|
+ float ab_len_sq = ab.LengthSq();
|
|
|
+ if (ab_len_sq > Square(FLT_EPSILON))
|
|
|
+ {
|
|
|
+ float v = Clamp(-inA.Dot(ab) / ab_len_sq, 0.0f, 1.0f);
|
|
|
+ Vec3 q = inA + v * ab;
|
|
|
+ float dist_sq = q.LengthSq();
|
|
|
+ if (dist_sq < best_dist_sq)
|
|
|
+ {
|
|
|
+ closest_set = 0b0011;
|
|
|
+ closest_point = q;
|
|
|
+ best_dist_sq = dist_sq;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
outSet = closest_set;
|
|
|
return closest_point;
|
|
|
}
|