|
|
|
@@ -0,0 +1,375 @@
|
|
|
|
|
/*
|
|
|
|
|
* road_geometry_overlap_test — standalone regression test for
|
|
|
|
|
* RoadGeometryLib wedge/segment generation.
|
|
|
|
|
*
|
|
|
|
|
* Builds the same A-B-C graph as RoadGeometryDemo in several height
|
|
|
|
|
* configurations and checks the generated slabs for self-intersecting
|
|
|
|
|
* geometry:
|
|
|
|
|
* - COPLANAR: two triangles on the same plane overlapping in area
|
|
|
|
|
* (z-fighting duplicates)
|
|
|
|
|
* - CROSSING: a triangle edge properly pierces the interior of another
|
|
|
|
|
* triangle (interpenetrating sheets)
|
|
|
|
|
*
|
|
|
|
|
* Regression coverage for the inner-corner fold (curb pinned at the
|
|
|
|
|
* miter corner K — ProceduralRoadGeometry.md sections 5.2/5.4) and the
|
|
|
|
|
* double slab extrusion fix. The same checks also run inside the
|
|
|
|
|
* headless terrain suite (roadWedgeGeometry, case 3b); this executable
|
|
|
|
|
* needs no scene, ECS or render window and runs in milliseconds.
|
|
|
|
|
*
|
|
|
|
|
* Usage:
|
|
|
|
|
* road_geometry_overlap_test
|
|
|
|
|
* Runs the built-in regression configurations (flat, raised/
|
|
|
|
|
* lowered corner node, raised endpoints). Exit code 0 when no
|
|
|
|
|
* piece self-intersects, 1 otherwise.
|
|
|
|
|
* road_geometry_overlap_test Ax Ay Az Bx By Bz Cx Cy Cz
|
|
|
|
|
* Analyse a single custom configuration (same checks).
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
#include <Ogre.h>
|
|
|
|
|
#include <ProceduralTriangleBuffer.h>
|
|
|
|
|
|
|
|
|
|
#include "components/RoadGraph.hpp"
|
|
|
|
|
#include "roadlib/RoadGeometryLib.hpp"
|
|
|
|
|
|
|
|
|
|
#include <algorithm>
|
|
|
|
|
#include <cfloat>
|
|
|
|
|
#include <cmath>
|
|
|
|
|
#include <cstdio>
|
|
|
|
|
#include <cstdlib>
|
|
|
|
|
#include <utility>
|
|
|
|
|
#include <vector>
|
|
|
|
|
|
|
|
|
|
using Ogre::Vector2;
|
|
|
|
|
using Ogre::Vector3;
|
|
|
|
|
|
|
|
|
|
typedef std::pair<double, double> P2; /* (x, z) */
|
|
|
|
|
|
|
|
|
|
static double cross2d(const P2 &a, const P2 &b)
|
|
|
|
|
{
|
|
|
|
|
return a.first * b.second - a.second * b.first;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static std::vector<P2> clipHalfPlane(const std::vector<P2> &poly, const P2 &a,
|
|
|
|
|
const P2 &b)
|
|
|
|
|
{
|
|
|
|
|
std::vector<P2> out;
|
|
|
|
|
if (poly.empty())
|
|
|
|
|
return out;
|
|
|
|
|
P2 edge(b.first - a.first, b.second - a.second);
|
|
|
|
|
auto inside = [&](const P2 &p) {
|
|
|
|
|
P2 rel(p.first - a.first, p.second - a.second);
|
|
|
|
|
return cross2d(edge, rel) >= 0.0;
|
|
|
|
|
};
|
|
|
|
|
auto intersect = [&](const P2 &p0, const P2 &p1) {
|
|
|
|
|
P2 e0(p0.first - a.first, p0.second - a.second);
|
|
|
|
|
P2 e1(p1.first - a.first, p1.second - a.second);
|
|
|
|
|
double d0 = cross2d(edge, e0);
|
|
|
|
|
double d1 = cross2d(edge, e1);
|
|
|
|
|
double t = d0 / (d0 - d1);
|
|
|
|
|
return P2(p0.first + (p1.first - p0.first) * t,
|
|
|
|
|
p0.second + (p1.second - p0.second) * t);
|
|
|
|
|
};
|
|
|
|
|
for (size_t i = 0; i < poly.size(); ++i) {
|
|
|
|
|
const P2 &cur = poly[i];
|
|
|
|
|
const P2 &prv = poly[(i + poly.size() - 1) % poly.size()];
|
|
|
|
|
bool inCur = inside(cur), inPrv = inside(prv);
|
|
|
|
|
if (inCur) {
|
|
|
|
|
if (!inPrv)
|
|
|
|
|
out.push_back(intersect(prv, cur));
|
|
|
|
|
out.push_back(cur);
|
|
|
|
|
} else if (inPrv) {
|
|
|
|
|
out.push_back(intersect(prv, cur));
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return out;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static double polyArea(const std::vector<P2> &poly)
|
|
|
|
|
{
|
|
|
|
|
if (poly.size() < 3)
|
|
|
|
|
return 0.0;
|
|
|
|
|
double s = 0.0;
|
|
|
|
|
for (size_t i = 0; i < poly.size(); ++i) {
|
|
|
|
|
const P2 &p = poly[i];
|
|
|
|
|
const P2 &q = poly[(i + 1) % poly.size()];
|
|
|
|
|
s += p.first * q.second - q.first * p.second;
|
|
|
|
|
}
|
|
|
|
|
return 0.5 * s;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static double triOverlapAreaXZ(const Vector3 t0[3], const Vector3 t1[3])
|
|
|
|
|
{
|
|
|
|
|
std::vector<P2> subject;
|
|
|
|
|
for (int i = 0; i < 3; ++i)
|
|
|
|
|
subject.push_back(P2(t0[i].x, t0[i].z));
|
|
|
|
|
std::vector<P2> clip;
|
|
|
|
|
for (int i = 0; i < 3; ++i)
|
|
|
|
|
clip.push_back(P2(t1[i].x, t1[i].z));
|
|
|
|
|
if (polyArea(clip) < 0.0)
|
|
|
|
|
std::reverse(clip.begin(), clip.end());
|
|
|
|
|
std::vector<P2> poly = subject;
|
|
|
|
|
for (int e = 0; e < 3 && !poly.empty(); ++e)
|
|
|
|
|
poly = clipHalfPlane(poly, clip[e], clip[(e + 1) % 3]);
|
|
|
|
|
return std::fabs(polyArea(poly));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Barycentric coords of p in triangle (a,b,c); returns false if degenerate. */
|
|
|
|
|
static bool bary(const Vector3 &p, const Vector3 &a, const Vector3 &b,
|
|
|
|
|
const Vector3 &c, float &u, float &v, float &w)
|
|
|
|
|
{
|
|
|
|
|
Vector3 v0 = b - a, v1 = c - a, v2 = p - a;
|
|
|
|
|
float d00 = v0.dotProduct(v0);
|
|
|
|
|
float d01 = v0.dotProduct(v1);
|
|
|
|
|
float d11 = v1.dotProduct(v1);
|
|
|
|
|
float d20 = v2.dotProduct(v0);
|
|
|
|
|
float d21 = v2.dotProduct(v1);
|
|
|
|
|
float denom = d00 * d11 - d01 * d01;
|
|
|
|
|
if (std::fabs(denom) < 1e-12f)
|
|
|
|
|
return false;
|
|
|
|
|
v = (d11 * d20 - d01 * d21) / denom;
|
|
|
|
|
w = (d00 * d21 - d01 * d20) / denom;
|
|
|
|
|
u = 1.0f - v - w;
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Segment (p0,p1) vs triangle (a,b,c): proper interior piercing test.
|
|
|
|
|
* Intersection must be strictly inside the triangle and strictly inside
|
|
|
|
|
* the segment (so shared vertices/edges do not count). */
|
|
|
|
|
static bool segTriPierce(const Vector3 &p0, const Vector3 &p1,
|
|
|
|
|
const Vector3 &a, const Vector3 &b, const Vector3 &c,
|
|
|
|
|
Vector3 &hit)
|
|
|
|
|
{
|
|
|
|
|
Vector3 n = (b - a).crossProduct(c - a);
|
|
|
|
|
float len = n.length();
|
|
|
|
|
if (len < 1e-8f)
|
|
|
|
|
return false;
|
|
|
|
|
n /= len;
|
|
|
|
|
float d0 = n.dotProduct(p0 - a);
|
|
|
|
|
float d1 = n.dotProduct(p1 - a);
|
|
|
|
|
if (d0 * d1 >= 0.0f)
|
|
|
|
|
return false; /* same side or touching */
|
|
|
|
|
float t = d0 / (d0 - d1);
|
|
|
|
|
if (t < 1e-4f || t > 1.0f - 1e-4f)
|
|
|
|
|
return false;
|
|
|
|
|
Vector3 p = p0 + (p1 - p0) * t;
|
|
|
|
|
float u, v, w;
|
|
|
|
|
if (!bary(p, a, b, c, u, v, w))
|
|
|
|
|
return false;
|
|
|
|
|
if (u < 1e-4f || v < 1e-4f || w < 1e-4f)
|
|
|
|
|
return false;
|
|
|
|
|
hit = p;
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
struct Analysis {
|
|
|
|
|
int coplanar = 0;
|
|
|
|
|
int crossing = 0;
|
|
|
|
|
bool nan = false;
|
|
|
|
|
Vector3 mn = Vector3(FLT_MAX, FLT_MAX, FLT_MAX);
|
|
|
|
|
Vector3 mx = Vector3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
static Analysis analyze(const Procedural::TriangleBuffer &buf,
|
|
|
|
|
const char *name)
|
|
|
|
|
{
|
|
|
|
|
const auto &verts = buf.getVertices();
|
|
|
|
|
const auto &indices = buf.getIndices();
|
|
|
|
|
size_t nTri = indices.size() / 3;
|
|
|
|
|
printf("== %s: %zu vertices, %zu tris\n", name, verts.size(), nTri);
|
|
|
|
|
|
|
|
|
|
Analysis res;
|
|
|
|
|
for (const auto &v : verts) {
|
|
|
|
|
const Vector3 &p = v.mPosition;
|
|
|
|
|
if (std::isnan(p.x) || std::isnan(p.y) || std::isnan(p.z))
|
|
|
|
|
res.nan = true;
|
|
|
|
|
res.mn.makeFloor(p);
|
|
|
|
|
res.mx.makeCeil(p);
|
|
|
|
|
}
|
|
|
|
|
printf(" bounds min (%g, %g, %g) max (%g, %g, %g)%s\n", res.mn.x,
|
|
|
|
|
res.mn.y, res.mn.z, res.mx.x, res.mx.y, res.mx.z,
|
|
|
|
|
res.nan ? " *** NaN ***" : "");
|
|
|
|
|
|
|
|
|
|
for (size_t i = 0; i < nTri; ++i) {
|
|
|
|
|
Vector3 t0[3];
|
|
|
|
|
for (int k = 0; k < 3; ++k)
|
|
|
|
|
t0[k] = verts[(size_t)indices[i * 3 + k]].mPosition;
|
|
|
|
|
Vector3 n0 = (t0[1] - t0[0]).crossProduct(t0[2] - t0[0]);
|
|
|
|
|
float l0 = n0.length();
|
|
|
|
|
if (l0 > 1e-8f)
|
|
|
|
|
n0 /= l0;
|
|
|
|
|
for (size_t j = i + 1; j < nTri; ++j) {
|
|
|
|
|
Vector3 t1[3];
|
|
|
|
|
for (int k = 0; k < 3; ++k)
|
|
|
|
|
t1[k] = verts[(size_t)indices[j * 3 + k]]
|
|
|
|
|
.mPosition;
|
|
|
|
|
Vector3 n1 = (t1[1] - t1[0]).crossProduct(t1[2] - t1[0]);
|
|
|
|
|
float l1 = n1.length();
|
|
|
|
|
if (l1 > 1e-8f)
|
|
|
|
|
n1 /= l1;
|
|
|
|
|
|
|
|
|
|
bool reported = false;
|
|
|
|
|
if (std::fabs(n0.dotProduct(n1)) > 0.9999f) {
|
|
|
|
|
/* Parallel planes: coplanar z-fight check. */
|
|
|
|
|
float dist =
|
|
|
|
|
std::fabs(n0.dotProduct(t1[0] - t0[0]));
|
|
|
|
|
if (dist < 1e-3f &&
|
|
|
|
|
triOverlapAreaXZ(t0, t1) > 1e-3) {
|
|
|
|
|
if (res.coplanar < 400)
|
|
|
|
|
printf(" COPLANAR %zu/%zu "
|
|
|
|
|
"area %.3f near "
|
|
|
|
|
"(%.2f,%.2f,%.2f)\n",
|
|
|
|
|
i, j,
|
|
|
|
|
triOverlapAreaXZ(t0, t1),
|
|
|
|
|
t0[0].x, t0[0].y,
|
|
|
|
|
t0[0].z);
|
|
|
|
|
++res.coplanar;
|
|
|
|
|
reported = true;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
if (reported)
|
|
|
|
|
continue;
|
|
|
|
|
Vector3 hit;
|
|
|
|
|
bool pierce = false;
|
|
|
|
|
for (int e = 0; e < 3 && !pierce; ++e)
|
|
|
|
|
pierce = segTriPierce(t0[e], t0[(e + 1) % 3],
|
|
|
|
|
t1[0], t1[1], t1[2], hit);
|
|
|
|
|
for (int e = 0; e < 3 && !pierce; ++e)
|
|
|
|
|
pierce = segTriPierce(t1[e], t1[(e + 1) % 3],
|
|
|
|
|
t0[0], t0[1], t0[2], hit);
|
|
|
|
|
if (pierce) {
|
|
|
|
|
if (res.crossing < 400)
|
|
|
|
|
printf(" CROSS %zu/%zu at "
|
|
|
|
|
"(%.3f,%.3f,%.3f)\n",
|
|
|
|
|
i, j, hit.x, hit.y, hit.z);
|
|
|
|
|
++res.crossing;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
printf(" coplanar-overlap pairs: %d crossing pairs: %d\n",
|
|
|
|
|
res.coplanar, res.crossing);
|
|
|
|
|
return res;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Build one piece and check it. Returns false on any defect. */
|
|
|
|
|
static bool checkPiece(bool built, const char *name,
|
|
|
|
|
Procedural::TriangleBuffer &buf, Analysis *out)
|
|
|
|
|
{
|
|
|
|
|
if (!built) {
|
|
|
|
|
printf("== %s: build failed\n", name);
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
Analysis a = analyze(buf, name);
|
|
|
|
|
if (out)
|
|
|
|
|
*out = a;
|
|
|
|
|
bool ok = !a.nan && a.coplanar == 0 && a.crossing == 0;
|
|
|
|
|
if (!ok)
|
|
|
|
|
printf(" *** %s FAILED ***\n", name);
|
|
|
|
|
return ok;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
static bool runConfig(const Vector3 &A, const Vector3 &B, const Vector3 &C)
|
|
|
|
|
{
|
|
|
|
|
printf("A=(%g,%g,%g) B=(%g,%g,%g) C=(%g,%g,%g)\n", A.x, A.y, A.z, B.x,
|
|
|
|
|
B.y, B.z, C.x, C.y, C.z);
|
|
|
|
|
|
|
|
|
|
RoadGraph graph;
|
|
|
|
|
graph.config.laneWidth = 3.0f;
|
|
|
|
|
graph.config.lanesPerDirection = 1;
|
|
|
|
|
graph.config.roadThickness = 0.3f;
|
|
|
|
|
int idA = graph.addNode(A, 0.0f);
|
|
|
|
|
int idB = graph.addNode(B, 0.0f);
|
|
|
|
|
int idC = graph.addNode(C, 0.0f);
|
|
|
|
|
graph.addEdge(idA, idB);
|
|
|
|
|
graph.addEdge(idB, idC);
|
|
|
|
|
|
|
|
|
|
std::vector<RoadWedge> wedges;
|
|
|
|
|
std::vector<RoadStraightSegment> segs;
|
|
|
|
|
enumerateWedges(graph, wedges, segs);
|
|
|
|
|
|
|
|
|
|
const RoadWedge *wSmall = nullptr, *wLarge = nullptr;
|
|
|
|
|
for (const auto &w : wedges) {
|
|
|
|
|
if (w.nodeId != idB || w.degenerate)
|
|
|
|
|
continue;
|
|
|
|
|
if (!wSmall || w.sweptAngleDeg < wSmall->sweptAngleDeg)
|
|
|
|
|
wSmall = &w;
|
|
|
|
|
if (!wLarge || w.sweptAngleDeg > wLarge->sweptAngleDeg)
|
|
|
|
|
wLarge = &w;
|
|
|
|
|
}
|
|
|
|
|
printf("wedges: small=%.1f deg large=%.1f deg, segments=%zu\n",
|
|
|
|
|
wSmall ? wSmall->sweptAngleDeg : -1.0f,
|
|
|
|
|
wLarge ? wLarge->sweptAngleDeg : -1.0f, segs.size());
|
|
|
|
|
|
|
|
|
|
bool ok = true;
|
|
|
|
|
if (!wSmall || !wLarge) {
|
|
|
|
|
printf("*** wedges at node B not found ***\n");
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
{
|
|
|
|
|
Procedural::TriangleBuffer buf;
|
|
|
|
|
bool built = RoadGeometryLib::buildWedgeGeometry(*wSmall, graph,
|
|
|
|
|
buf);
|
|
|
|
|
Analysis a;
|
|
|
|
|
ok &= checkPiece(built, "smaller wedge", buf, &a);
|
|
|
|
|
|
|
|
|
|
/* The fallback box template supplies the slab thickness:
|
|
|
|
|
* flat nodes must give Y within +/- thickness/2 (a second
|
|
|
|
|
* extrusion would double it). */
|
|
|
|
|
if (built && A.y == 0.0f && B.y == 0.0f && C.y == 0.0f &&
|
|
|
|
|
(a.mn.y < -0.16f || a.mx.y > 0.16f)) {
|
|
|
|
|
printf(" *** slab thickness wrong "
|
|
|
|
|
"(double extrusion?) ***\n");
|
|
|
|
|
ok = false;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
{
|
|
|
|
|
Procedural::TriangleBuffer buf;
|
|
|
|
|
bool built = RoadGeometryLib::buildWedgeGeometry(*wLarge, graph,
|
|
|
|
|
buf);
|
|
|
|
|
ok &= checkPiece(built, "larger wedge", buf, nullptr);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
for (const auto &s : segs) {
|
|
|
|
|
Procedural::TriangleBuffer buf;
|
|
|
|
|
bool built = RoadGeometryLib::buildSegmentGeometry(s, graph,
|
|
|
|
|
buf);
|
|
|
|
|
ok &= checkPiece(built,
|
|
|
|
|
s.nodeId == idA ? "segment A" : "segment C",
|
|
|
|
|
buf, nullptr);
|
|
|
|
|
}
|
|
|
|
|
return ok;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int main(int argc, char **argv)
|
|
|
|
|
{
|
|
|
|
|
bool ok = true;
|
|
|
|
|
|
|
|
|
|
if (argc == 10) {
|
|
|
|
|
Vector3 A(atof(argv[1]), atof(argv[2]), atof(argv[3]));
|
|
|
|
|
Vector3 B(atof(argv[4]), atof(argv[5]), atof(argv[6]));
|
|
|
|
|
Vector3 C(atof(argv[7]), atof(argv[8]), atof(argv[9]));
|
|
|
|
|
ok = runConfig(A, B, C);
|
|
|
|
|
} else if (argc == 1) {
|
|
|
|
|
/* Regression configurations: flat, raised/lowered corner
|
|
|
|
|
* node, raised endpoints. */
|
|
|
|
|
ok &= runConfig(Vector3(-5, 0, 0), Vector3(0, 0, 0),
|
|
|
|
|
Vector3(5, 0, 5));
|
|
|
|
|
ok &= runConfig(Vector3(-5, 0, 0), Vector3(0, 5, 0),
|
|
|
|
|
Vector3(5, 0, 5));
|
|
|
|
|
ok &= runConfig(Vector3(-5, 0, 0), Vector3(0, -4, 0),
|
|
|
|
|
Vector3(5, 0, 5));
|
|
|
|
|
ok &= runConfig(Vector3(-5, 5, 0), Vector3(0, 0, 0),
|
|
|
|
|
Vector3(5, 0, 5));
|
|
|
|
|
ok &= runConfig(Vector3(-5, 0, 0), Vector3(0, 0, 0),
|
|
|
|
|
Vector3(5, 5, 5));
|
|
|
|
|
} else {
|
|
|
|
|
fprintf(stderr,
|
|
|
|
|
"usage: %s [Ax Ay Az Bx By Bz Cx Cy Cz]\n",
|
|
|
|
|
argv[0]);
|
|
|
|
|
return 2;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
printf("%s\n", ok ? "ROAD GEOMETRY OVERLAP TEST: PASSED"
|
|
|
|
|
: "ROAD GEOMETRY OVERLAP TEST: FAILED");
|
|
|
|
|
return ok ? 0 : 1;
|
|
|
|
|
}
|
