mirror of
https://github.com/Relintai/pandemonium_engine.git
synced 2024-12-22 11:56:49 +01:00
Fix Geometry::get_closest_points_between_segments() returns NaN
Also fix Geometry::get_closest_distance_between_segments() returning incorrect values.
This commit is contained in:
Marcel Admiraal
parent
cddfcbab2c
commit
226e29cfca
@ -40,19 +40,110 @@
|
||||
|
||||
#define SCALE_FACTOR 100000.0 // Based on CMP_EPSILON.
|
||||
|
||||
// This implementation is very inefficient, commenting unless bugs happen. See the other one.
|
||||
/*
|
||||
bool Geometry::is_point_in_polygon(const Vector2 &p_point, const Vector<Vector2> &p_polygon) {
|
||||
void Geometry::get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt) {
|
||||
// Based on David Eberly's Computation of Distance Between Line Segments algorithm.
|
||||
|
||||
Vector<int> indices = Geometry::triangulate_polygon(p_polygon);
|
||||
for (int j = 0; j + 3 <= indices.size(); j += 3) {
|
||||
int i1 = indices[j], i2 = indices[j + 1], i3 = indices[j + 2];
|
||||
if (Geometry::is_point_in_triangle(p_point, p_polygon[i1], p_polygon[i2], p_polygon[i3]))
|
||||
return true;
|
||||
Vector3 p = p_p1 - p_p0;
|
||||
Vector3 q = p_q1 - p_q0;
|
||||
Vector3 r = p_p0 - p_q0;
|
||||
|
||||
real_t a = p.dot(p);
|
||||
real_t b = p.dot(q);
|
||||
real_t c = q.dot(q);
|
||||
real_t d = p.dot(r);
|
||||
real_t e = q.dot(r);
|
||||
|
||||
real_t s = 0.0f;
|
||||
real_t t = 0.0f;
|
||||
|
||||
real_t det = a * c - b * b;
|
||||
if (det > CMP_EPSILON) {
|
||||
// Non-parallel segments
|
||||
real_t bte = b * e;
|
||||
real_t ctd = c * d;
|
||||
|
||||
if (bte <= ctd) {
|
||||
// s <= 0.0f
|
||||
if (e <= 0.0f) {
|
||||
// t <= 0.0f
|
||||
s = (-d >= a ? 1 : (-d > 0.0f ? -d / a : 0.0f));
|
||||
t = 0.0f;
|
||||
} else if (e < c) {
|
||||
// 0.0f < t < 1
|
||||
s = 0.0f;
|
||||
t = e / c;
|
||||
} else {
|
||||
// t >= 1
|
||||
s = (b - d >= a ? 1 : (b - d > 0.0f ? (b - d) / a : 0.0f));
|
||||
t = 1;
|
||||
}
|
||||
} else {
|
||||
// s > 0.0f
|
||||
s = bte - ctd;
|
||||
if (s >= det) {
|
||||
// s >= 1
|
||||
if (b + e <= 0.0f) {
|
||||
// t <= 0.0f
|
||||
s = (-d <= 0.0f ? 0.0f : (-d < a ? -d / a : 1));
|
||||
t = 0.0f;
|
||||
} else if (b + e < c) {
|
||||
// 0.0f < t < 1
|
||||
s = 1;
|
||||
t = (b + e) / c;
|
||||
} else {
|
||||
// t >= 1
|
||||
s = (b - d <= 0.0f ? 0.0f : (b - d < a ? (b - d) / a : 1));
|
||||
t = 1;
|
||||
}
|
||||
} else {
|
||||
// 0.0f < s < 1
|
||||
real_t ate = a * e;
|
||||
real_t btd = b * d;
|
||||
|
||||
if (ate <= btd) {
|
||||
// t <= 0.0f
|
||||
s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
|
||||
t = 0.0f;
|
||||
} else {
|
||||
// t > 0.0f
|
||||
t = ate - btd;
|
||||
if (t >= det) {
|
||||
// t >= 1
|
||||
s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
|
||||
t = 1;
|
||||
} else {
|
||||
// 0.0f < t < 1
|
||||
s /= det;
|
||||
t /= det;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Parallel segments
|
||||
if (e <= 0.0f) {
|
||||
s = (-d <= 0.0f ? 0.0f : (-d >= a ? 1 : -d / a));
|
||||
t = 0.0f;
|
||||
} else if (e >= c) {
|
||||
s = (b - d <= 0.0f ? 0.0f : (b - d >= a ? 1 : (b - d) / a));
|
||||
t = 1;
|
||||
} else {
|
||||
s = 0.0f;
|
||||
t = e / c;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
|
||||
r_ps = (1 - s) * p_p0 + s * p_p1;
|
||||
r_qt = (1 - t) * p_q0 + t * p_q1;
|
||||
}
|
||||
|
||||
real_t Geometry::get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1) {
|
||||
Vector3 ps;
|
||||
Vector3 qt;
|
||||
get_closest_points_between_segments(p_p0, p_p1, p_q0, p_q1, ps, qt);
|
||||
Vector3 st = qt - ps;
|
||||
return st.length();
|
||||
}
|
||||
*/
|
||||
|
||||
void Geometry::OccluderMeshData::clear() {
|
||||
faces.clear();
|
||||
|
||||
@ -101,96 +101,8 @@ public:
|
||||
return Math::sqrt((c1 - c2).dot(c1 - c2));
|
||||
}
|
||||
|
||||
static void get_closest_points_between_segments(const Vector3 &p1, const Vector3 &p2, const Vector3 &q1, const Vector3 &q2, Vector3 &c1, Vector3 &c2) {
|
||||
// Do the function 'd' as defined by pb. I think is is dot product of some sort.
|
||||
#define d_of(m, n, o, p) ((m.x - n.x) * (o.x - p.x) + (m.y - n.y) * (o.y - p.y) + (m.z - n.z) * (o.z - p.z))
|
||||
|
||||
// Calculate the parametric position on the 2 curves, mua and mub.
|
||||
real_t mua = (d_of(p1, q1, q2, q1) * d_of(q2, q1, p2, p1) - d_of(p1, q1, p2, p1) * d_of(q2, q1, q2, q1)) / (d_of(p2, p1, p2, p1) * d_of(q2, q1, q2, q1) - d_of(q2, q1, p2, p1) * d_of(q2, q1, p2, p1));
|
||||
real_t mub = (d_of(p1, q1, q2, q1) + mua * d_of(q2, q1, p2, p1)) / d_of(q2, q1, q2, q1);
|
||||
|
||||
// Clip the value between [0..1] constraining the solution to lie on the original curves.
|
||||
if (mua < 0) {
|
||||
mua = 0;
|
||||
}
|
||||
if (mub < 0) {
|
||||
mub = 0;
|
||||
}
|
||||
if (mua > 1) {
|
||||
mua = 1;
|
||||
}
|
||||
if (mub > 1) {
|
||||
mub = 1;
|
||||
}
|
||||
c1 = p1.linear_interpolate(p2, mua);
|
||||
c2 = q1.linear_interpolate(q2, mub);
|
||||
}
|
||||
|
||||
static real_t get_closest_distance_between_segments(const Vector3 &p_from_a, const Vector3 &p_to_a, const Vector3 &p_from_b, const Vector3 &p_to_b) {
|
||||
Vector3 u = p_to_a - p_from_a;
|
||||
Vector3 v = p_to_b - p_from_b;
|
||||
Vector3 w = p_from_a - p_to_a;
|
||||
real_t a = u.dot(u); // Always >= 0
|
||||
real_t b = u.dot(v);
|
||||
real_t c = v.dot(v); // Always >= 0
|
||||
real_t d = u.dot(w);
|
||||
real_t e = v.dot(w);
|
||||
real_t D = a * c - b * b; // Always >= 0
|
||||
real_t sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
|
||||
real_t tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
|
||||
|
||||
// Compute the line parameters of the two closest points.
|
||||
if (D < (real_t)CMP_EPSILON) { // The lines are almost parallel.
|
||||
sN = 0; // Force using point P0 on segment S1
|
||||
sD = 1; // to prevent possible division by 0.0 later.
|
||||
tN = e;
|
||||
tD = c;
|
||||
} else { // Get the closest points on the infinite lines
|
||||
sN = (b * e - c * d);
|
||||
tN = (a * e - b * d);
|
||||
if (sN < 0) { // sc < 0 => the s=0 edge is visible.
|
||||
sN = 0;
|
||||
tN = e;
|
||||
tD = c;
|
||||
} else if (sN > sD) { // sc > 1 => the s=1 edge is visible.
|
||||
sN = sD;
|
||||
tN = e + b;
|
||||
tD = c;
|
||||
}
|
||||
}
|
||||
|
||||
if (tN < 0) { // tc < 0 => the t=0 edge is visible.
|
||||
tN = 0;
|
||||
// Recompute sc for this edge.
|
||||
if (-d < 0) {
|
||||
sN = 0;
|
||||
} else if (-d > a) {
|
||||
sN = sD;
|
||||
} else {
|
||||
sN = -d;
|
||||
sD = a;
|
||||
}
|
||||
} else if (tN > tD) { // tc > 1 => the t=1 edge is visible.
|
||||
tN = tD;
|
||||
// Recompute sc for this edge.
|
||||
if ((-d + b) < 0) {
|
||||
sN = 0;
|
||||
} else if ((-d + b) > a) {
|
||||
sN = sD;
|
||||
} else {
|
||||
sN = (-d + b);
|
||||
sD = a;
|
||||
}
|
||||
}
|
||||
// Finally do the division to get sc and tc.
|
||||
sc = (Math::is_zero_approx(sN) ? 0 : sN / sD);
|
||||
tc = (Math::is_zero_approx(tN) ? 0 : tN / tD);
|
||||
|
||||
// Get the difference of the two closest points.
|
||||
Vector3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
|
||||
|
||||
return dP.length(); // Return the closest distance.
|
||||
}
|
||||
static void get_closest_points_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1, Vector3 &r_ps, Vector3 &r_qt);
|
||||
static real_t get_closest_distance_between_segments(const Vector3 &p_p0, const Vector3 &p_p1, const Vector3 &p_q0, const Vector3 &p_q1);
|
||||
|
||||
static inline bool ray_intersects_triangle(const Vector3 &p_from, const Vector3 &p_dir, const Vector3 &p_v0, const Vector3 &p_v1, const Vector3 &p_v2, Vector3 *r_res = nullptr) {
|
||||
Vector3 e1 = p_v1 - p_v0;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user