Clean up InputGeometry

This commit is contained in:
Graham Pentheny
2025-04-09 01:15:49 -04:00
parent 4afcfe00c5
commit 9d90e162e1
2 changed files with 239 additions and 218 deletions

View File

@@ -34,39 +34,46 @@ static bool intersectSegmentTriangle(const float* sp, const float* sq,
const float* a, const float* b, const float* c,
float &t)
{
float v, w;
float ab[3], ac[3], qp[3], ap[3], norm[3], e[3];
float v;
float w;
float ab[3];
float ac[3];
float qp[3];
float ap[3];
float norm[3];
float e[3];
rcVsub(ab, b, a);
rcVsub(ac, c, a);
rcVsub(qp, sp, sq);
// Compute triangle normal. Can be precalculated or cached if
// intersecting multiple segments against the same triangle
rcVcross(norm, ab, ac);
// Compute denominator d. If d <= 0, segment is parallel to or points
// away from triangle, so exit early
float d = rcVdot(qp, norm);
if (d <= 0.0f) return false;
if (d <= 0.0f) { return false; }
// Compute intersection t value of pq with plane of triangle. A ray
// intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
// dividing by d until intersection has been found to pierce triangle
rcVsub(ap, sp, a);
t = rcVdot(ap, norm);
if (t < 0.0f) return false;
if (t > d) return false; // For segment; exclude this code line for a ray test
if (t < 0.0f) { return false; }
if (t > d) { return false; } // For segment; exclude this code line for a ray test
// Compute barycentric coordinate components and test if within bounds
rcVcross(e, qp, ap);
v = rcVdot(ac, e);
if (v < 0.0f || v > d) return false;
if (v < 0.0f || v > d) { return false; }
w = -rcVdot(ab, e);
if (w < 0.0f || v + w > d) return false;
if (w < 0.0f || v + w > d) { return false; }
// Segment/ray intersects triangle. Perform delayed division
t /= d;
return true;
}
@@ -83,20 +90,22 @@ static char* parseRow(char* buf, char* bufEnd, char* row, int len)
switch (c)
{
case '\n':
if (start) break;
if (start) { break; }
done = true;
break;
case '\r':
break;
case '\t':
case ' ':
if (start) break;
if (start) { break; }
// else falls through
default:
start = false;
row[n++] = c;
if (n >= len-1)
{
done = true;
}
break;
}
}
@@ -104,35 +113,16 @@ static char* parseRow(char* buf, char* bufEnd, char* row, int len)
return buf;
}
InputGeom::InputGeom() :
m_chunkyMesh(0),
m_mesh(0),
m_hasBuildSettings(false),
m_offMeshConCount(0),
m_volumeCount(0)
{
}
InputGeom::~InputGeom()
{
delete m_chunkyMesh;
delete m_mesh;
}
bool InputGeom::loadMesh(rcContext* ctx, const std::string& filepath)
{
if (m_mesh)
{
delete m_chunkyMesh;
m_chunkyMesh = 0;
delete m_mesh;
m_mesh = 0;
delete m_chunkyMesh; m_chunkyMesh = nullptr;
delete m_mesh; m_mesh = nullptr;
}
m_offMeshConCount = 0;
m_volumeCount = 0;
m_mesh = new rcMeshLoaderObj;
if (!m_mesh)
{
@@ -157,19 +147,16 @@ bool InputGeom::loadMesh(rcContext* ctx, const std::string& filepath)
{
ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Failed to build chunky mesh.");
return false;
}
}
return true;
}
bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
{
char* buf = 0;
FILE* fp = fopen(filepath.c_str(), "rb");
if (!fp)
{
return false;
}
if (!fp) { return false; }
if (fseek(fp, 0, SEEK_END) != 0)
{
fclose(fp);
@@ -187,7 +174,7 @@ bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
fclose(fp);
return false;
}
buf = new char[bufSize];
char* buf = new char[bufSize];
if (!buf)
{
fclose(fp);
@@ -200,11 +187,11 @@ bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
delete[] buf;
return false;
}
m_offMeshConCount = 0;
m_volumeCount = 0;
delete m_mesh;
m_mesh = 0;
delete m_mesh; m_mesh = nullptr;
char* src = buf;
char* srcEnd = buf + bufSize;
@@ -213,14 +200,13 @@ bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
{
// Parse one row
row[0] = '\0';
src = parseRow(src, srcEnd, row, sizeof(row)/sizeof(char));
src = parseRow(src, srcEnd, row, sizeof(row) / sizeof(char));
if (row[0] == 'f')
{
// File name.
const char* name = row+1;
const char* name = row + 1;
// Skip white spaces
while (*name && isspace(*name))
name++;
for (;*name && isspace(*name); ++name) {}
if (*name)
{
if (!loadMesh(ctx, name))
@@ -235,10 +221,12 @@ bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
// Off-mesh connection
if (m_offMeshConCount < MAX_OFFMESH_CONNECTIONS)
{
float* v = &m_offMeshConVerts[m_offMeshConCount*3*2];
int bidir, area = 0, flags = 0;
float* v = &m_offMeshConVerts[m_offMeshConCount * 3 * 2];
int bidir;
int area;
int flags;
float rad;
sscanf(row+1, "%f %f %f %f %f %f %f %d %d %d",
sscanf(row + 1, "%f %f %f %f %f %f %f %d %d %d",
&v[0], &v[1], &v[2], &v[3], &v[4], &v[5], &rad, &bidir, &area, &flags);
m_offMeshConRads[m_offMeshConCount] = rad;
m_offMeshConDirs[m_offMeshConCount] = (unsigned char)bidir;
@@ -253,12 +241,12 @@ bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
if (m_volumeCount < MAX_VOLUMES)
{
ConvexVolume* vol = &m_volumes[m_volumeCount++];
sscanf(row+1, "%d %d %f %f", &vol->nverts, &vol->area, &vol->hmin, &vol->hmax);
sscanf(row + 1, "%d %d %f %f", &vol->nverts, &vol->area, &vol->hmin, &vol->hmax);
for (int i = 0; i < vol->nverts; ++i)
{
row[0] = '\0';
src = parseRow(src, srcEnd, row, sizeof(row)/sizeof(char));
sscanf(row, "%f %f %f", &vol->verts[i*3+0], &vol->verts[i*3+1], &vol->verts[i*3+2]);
src = parseRow(src, srcEnd, row, sizeof(row) / sizeof(char));
sscanf(row, "%f %f %f", &vol->verts[i * 3 + 0], &vol->verts[i * 3 + 1], &vol->verts[i * 3 + 2]);
}
}
}
@@ -266,68 +254,73 @@ bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
{
// Settings
m_hasBuildSettings = true;
sscanf(row + 1, "%f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f",
&m_buildSettings.cellSize,
&m_buildSettings.cellHeight,
&m_buildSettings.agentHeight,
&m_buildSettings.agentRadius,
&m_buildSettings.agentMaxClimb,
&m_buildSettings.agentMaxSlope,
&m_buildSettings.regionMinSize,
&m_buildSettings.regionMergeSize,
&m_buildSettings.edgeMaxLen,
&m_buildSettings.edgeMaxError,
&m_buildSettings.vertsPerPoly,
&m_buildSettings.detailSampleDist,
&m_buildSettings.detailSampleMaxError,
&m_buildSettings.partitionType,
&m_buildSettings.navMeshBMin[0],
&m_buildSettings.navMeshBMin[1],
&m_buildSettings.navMeshBMin[2],
&m_buildSettings.navMeshBMax[0],
&m_buildSettings.navMeshBMax[1],
&m_buildSettings.navMeshBMax[2],
&m_buildSettings.tileSize);
sscanf(row + 1,
"%f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f",
&m_buildSettings.cellSize,
&m_buildSettings.cellHeight,
&m_buildSettings.agentHeight,
&m_buildSettings.agentRadius,
&m_buildSettings.agentMaxClimb,
&m_buildSettings.agentMaxSlope,
&m_buildSettings.regionMinSize,
&m_buildSettings.regionMergeSize,
&m_buildSettings.edgeMaxLen,
&m_buildSettings.edgeMaxError,
&m_buildSettings.vertsPerPoly,
&m_buildSettings.detailSampleDist,
&m_buildSettings.detailSampleMaxError,
&m_buildSettings.partitionType,
&m_buildSettings.navMeshBMin[0],
&m_buildSettings.navMeshBMin[1],
&m_buildSettings.navMeshBMin[2],
&m_buildSettings.navMeshBMax[0],
&m_buildSettings.navMeshBMax[1],
&m_buildSettings.navMeshBMax[2],
&m_buildSettings.tileSize);
}
}
delete [] buf;
return true;
}
bool InputGeom::load(rcContext* ctx, const std::string& filepath)
{
size_t extensionPos = filepath.find_last_of('.');
if (extensionPos == std::string::npos)
return false;
if (extensionPos == std::string::npos) { return false; }
std::string extension = filepath.substr(extensionPos);
std::transform(extension.begin(), extension.end(), extension.begin(), tolower);
if (extension == ".gset")
{
return loadGeomSet(ctx, filepath);
}
if (extension == ".obj")
{
return loadMesh(ctx, filepath);
}
return false;
}
bool InputGeom::saveGeomSet(const BuildSettings* settings)
{
if (!m_mesh) return false;
if (!m_mesh) { return false; }
// Change extension
std::string filepath = m_mesh->getFileName();
size_t extPos = filepath.find_last_of('.');
if (extPos != std::string::npos)
{
filepath = filepath.substr(0, extPos);
}
filepath += ".gset";
FILE* fp = fopen(filepath.c_str(), "w");
if (!fp) return false;
if (!fp) { return false; }
// Store mesh filename.
fprintf(fp, "f %s\n", m_mesh->getFileName().c_str());
@@ -358,17 +351,18 @@ bool InputGeom::saveGeomSet(const BuildSettings* settings)
settings->navMeshBMax[2],
settings->tileSize);
}
// Store off-mesh links.
for (int i = 0; i < m_offMeshConCount; ++i)
{
const float* v = &m_offMeshConVerts[i*3*2];
const float* v = &m_offMeshConVerts[i * 3 * 2];
const float rad = m_offMeshConRads[i];
const int bidir = m_offMeshConDirs[i];
const int area = m_offMeshConAreas[i];
const int flags = m_offMeshConFlags[i];
fprintf(fp, "c %f %f %f %f %f %f %f %d %d %d\n",
v[0], v[1], v[2], v[3], v[4], v[5], rad, bidir, area, flags);
fprintf(fp,
"c %f %f %f %f %f %f %f %d %d %d\n",
v[0], v[1], v[2], v[3], v[4], v[5], rad, bidir, area, flags);
}
// Convex volumes
@@ -377,33 +371,43 @@ bool InputGeom::saveGeomSet(const BuildSettings* settings)
ConvexVolume* vol = &m_volumes[i];
fprintf(fp, "v %d %d %f %f\n", vol->nverts, vol->area, vol->hmin, vol->hmax);
for (int j = 0; j < vol->nverts; ++j)
fprintf(fp, "%f %f %f\n", vol->verts[j*3+0], vol->verts[j*3+1], vol->verts[j*3+2]);
{
fprintf(fp, "%f %f %f\n", vol->verts[j * 3 + 0], vol->verts[j * 3 + 1], vol->verts[j * 3 + 2]);
}
}
fclose(fp);
return true;
}
static bool isectSegAABB(const float* sp, const float* sq,
const float* amin, const float* amax,
float& tmin, float& tmax)
static bool isectSegAABB(
const float* sp,
const float* sq,
const float* amin,
const float* amax,
float& tmin,
float& tmax)
{
static const float EPS = 1e-6f;
float d[3];
d[0] = sq[0] - sp[0];
d[1] = sq[1] - sp[1];
d[2] = sq[2] - sp[2];
float d[]
{
sq[0] - sp[0],
sq[1] - sp[1],
sq[2] - sp[2]
};
tmin = 0.0;
tmax = 1.0f;
for (int i = 0; i < 3; i++)
{
if (fabsf(d[i]) < EPS)
{
if (sp[i] < amin[i] || sp[i] > amax[i])
{
return false;
}
}
else
{
@@ -411,66 +415,78 @@ static bool isectSegAABB(const float* sp, const float* sq,
float t1 = (amin[i] - sp[i]) * ood;
float t2 = (amax[i] - sp[i]) * ood;
if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; }
if (t1 > tmin) tmin = t1;
if (t2 < tmax) tmax = t2;
if (tmin > tmax) return false;
if (t1 > tmin) { tmin = t1; }
if (t2 < tmax) { tmax = t2; }
if (tmin > tmax) { return false; }
}
}
return true;
}
bool InputGeom::raycastMesh(float* src, float* dst, float& tmin)
{
// Prune hit ray.
float btmin, btmax;
if (!isectSegAABB(src, dst, m_meshBMin, m_meshBMax, btmin, btmax))
return false;
float p[2], q[2];
p[0] = src[0] + (dst[0]-src[0])*btmin;
p[1] = src[2] + (dst[2]-src[2])*btmin;
q[0] = src[0] + (dst[0]-src[0])*btmax;
q[1] = src[2] + (dst[2]-src[2])*btmax;
float btmin;
float btmax;
if (!isectSegAABB(src, dst, m_meshBMin, m_meshBMax, btmin, btmax)) { return false; }
float p[]
{
p[0] = src[0] + (dst[0] - src[0]) * btmin,
p[1] = src[2] + (dst[2] - src[2]) * btmin
};
float q[]
{
src[0] + (dst[0] - src[0]) * btmax,
src[2] + (dst[2] - src[2]) * btmax
};
int cid[512];
const int ncid = rcGetChunksOverlappingSegment(m_chunkyMesh, p, q, cid, 512);
if (!ncid)
return false;
if (!ncid) { return false; }
tmin = 1.0f;
bool hit = false;
const float* verts = m_mesh->getVerts();
for (int i = 0; i < ncid; ++i)
{
const rcChunkyTriMeshNode& node = m_chunkyMesh->nodes[cid[i]];
const int* tris = &m_chunkyMesh->tris[node.i*3];
const int* tris = &m_chunkyMesh->tris[node.i * 3];
const int ntris = node.n;
for (int j = 0; j < ntris*3; j += 3)
for (int j = 0; j < ntris * 3; j += 3)
{
float t = 1;
if (intersectSegmentTriangle(src, dst,
&verts[tris[j]*3],
&verts[tris[j+1]*3],
&verts[tris[j+2]*3], t))
if (intersectSegmentTriangle(
src, dst,
&verts[tris[j] * 3],
&verts[tris[j + 1] * 3],
&verts[tris[j + 2] * 3], t))
{
if (t < tmin)
{
tmin = t;
}
hit = true;
}
}
}
return hit;
}
void InputGeom::addOffMeshConnection(const float* spos, const float* epos, const float rad,
unsigned char bidir, unsigned char area, unsigned short flags)
void InputGeom::addOffMeshConnection(
const float* spos,
const float* epos,
const float rad,
unsigned char bidir,
unsigned char area,
unsigned short flags)
{
if (m_offMeshConCount >= MAX_OFFMESH_CONNECTIONS) return;
float* v = &m_offMeshConVerts[m_offMeshConCount*3*2];
if (m_offMeshConCount >= MAX_OFFMESH_CONNECTIONS) { return; }
float* v = &m_offMeshConVerts[m_offMeshConCount * 3 * 2];
m_offMeshConRads[m_offMeshConCount] = rad;
m_offMeshConDirs[m_offMeshConCount] = bidir;
m_offMeshConAreas[m_offMeshConCount] = area;
@@ -484,8 +500,8 @@ void InputGeom::addOffMeshConnection(const float* spos, const float* epos, const
void InputGeom::deleteOffMeshConnection(int i)
{
m_offMeshConCount--;
float* src = &m_offMeshConVerts[m_offMeshConCount*3*2];
float* dst = &m_offMeshConVerts[i*3*2];
float* src = &m_offMeshConVerts[m_offMeshConCount * 3 * 2];
float* dst = &m_offMeshConVerts[i * 3 * 2];
rcVcopy(&dst[0], &src[0]);
rcVcopy(&dst[3], &src[3]);
m_offMeshConRads[i] = m_offMeshConRads[m_offMeshConCount];
@@ -496,42 +512,45 @@ void InputGeom::deleteOffMeshConnection(int i)
void InputGeom::drawOffMeshConnections(duDebugDraw* dd, bool hilight)
{
unsigned int conColor = duRGBA(192,0,128,192);
unsigned int baseColor = duRGBA(0,0,0,64);
unsigned int conColor = duRGBA(192, 0, 128, 192);
unsigned int baseColor = duRGBA(0, 0, 0, 64);
dd->depthMask(false);
dd->begin(DU_DRAW_LINES, 2.0f);
for (int i = 0; i < m_offMeshConCount; ++i)
{
float* v = &m_offMeshConVerts[i*3*2];
float* v = &m_offMeshConVerts[i * 3 * 2];
dd->vertex(v[0],v[1],v[2], baseColor);
dd->vertex(v[0],v[1]+0.2f,v[2], baseColor);
dd->vertex(v[3],v[4],v[5], baseColor);
dd->vertex(v[3],v[4]+0.2f,v[5], baseColor);
duAppendCircle(dd, v[0],v[1]+0.1f,v[2], m_offMeshConRads[i], baseColor);
duAppendCircle(dd, v[3],v[4]+0.1f,v[5], m_offMeshConRads[i], baseColor);
dd->vertex(v[0], v[1], v[2], baseColor);
dd->vertex(v[0], v[1] + 0.2f, v[2], baseColor);
dd->vertex(v[3], v[4], v[5], baseColor);
dd->vertex(v[3], v[4] + 0.2f, v[5], baseColor);
duAppendCircle(dd, v[0], v[1] + 0.1f, v[2], m_offMeshConRads[i], baseColor);
duAppendCircle(dd, v[3], v[4] + 0.1f, v[5], m_offMeshConRads[i], baseColor);
if (hilight)
{
duAppendArc(dd, v[0],v[1],v[2], v[3],v[4],v[5], 0.25f,
duAppendArc(dd, v[0], v[1], v[2], v[3], v[4], v[5], 0.25f,
(m_offMeshConDirs[i]&1) ? 0.6f : 0.0f, 0.6f, conColor);
}
}
}
dd->end();
dd->depthMask(true);
}
void InputGeom::addConvexVolume(const float* verts, const int nverts,
const float minh, const float maxh, unsigned char area)
void InputGeom::addConvexVolume(
const float* verts,
const int nverts,
const float minh,
const float maxh,
unsigned char area)
{
if (m_volumeCount >= MAX_VOLUMES) return;
if (m_volumeCount >= MAX_VOLUMES) { return; }
ConvexVolume* vol = &m_volumes[m_volumeCount++];
memset(vol, 0, sizeof(ConvexVolume));
memcpy(vol->verts, verts, sizeof(float)*3*nverts);
memcpy(vol->verts, verts, sizeof(float) * 3 * nverts);
vol->hmin = minh;
vol->hmax = maxh;
vol->nverts = nverts;
@@ -547,32 +566,31 @@ void InputGeom::deleteConvexVolume(int i)
void InputGeom::drawConvexVolumes(struct duDebugDraw* dd, bool /*hilight*/)
{
dd->depthMask(false);
dd->begin(DU_DRAW_TRIS);
for (int i = 0; i < m_volumeCount; ++i)
{
const ConvexVolume* vol = &m_volumes[i];
unsigned int col = duTransCol(dd->areaToCol(vol->area), 32);
for (int j = 0, k = vol->nverts-1; j < vol->nverts; k = j++)
{
const float* va = &vol->verts[k*3];
const float* vb = &vol->verts[j*3];
const float* va = &vol->verts[k * 3];
const float* vb = &vol->verts[j * 3];
dd->vertex(vol->verts[0],vol->hmax,vol->verts[2], col);
dd->vertex(vb[0],vol->hmax,vb[2], col);
dd->vertex(va[0],vol->hmax,va[2], col);
dd->vertex(va[0],vol->hmin,va[2], duDarkenCol(col));
dd->vertex(va[0],vol->hmax,va[2], col);
dd->vertex(vb[0],vol->hmax,vb[2], col);
dd->vertex(vol->verts[0], vol->hmax, vol->verts[2], col);
dd->vertex(vb[0], vol->hmax, vb[2], col);
dd->vertex(va[0], vol->hmax, va[2], col);
dd->vertex(va[0],vol->hmin,va[2], duDarkenCol(col));
dd->vertex(vb[0],vol->hmax,vb[2], col);
dd->vertex(vb[0],vol->hmin,vb[2], duDarkenCol(col));
dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
dd->vertex(va[0], vol->hmax, va[2], col);
dd->vertex(vb[0], vol->hmax, vb[2], col);
dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
dd->vertex(vb[0], vol->hmax, vb[2], col);
dd->vertex(vb[0], vol->hmin, vb[2], duDarkenCol(col));
}
}
dd->end();
dd->begin(DU_DRAW_LINES, 2.0f);
@@ -582,14 +600,14 @@ void InputGeom::drawConvexVolumes(struct duDebugDraw* dd, bool /*hilight*/)
unsigned int col = duTransCol(dd->areaToCol(vol->area), 220);
for (int j = 0, k = vol->nverts-1; j < vol->nverts; k = j++)
{
const float* va = &vol->verts[k*3];
const float* vb = &vol->verts[j*3];
dd->vertex(va[0],vol->hmin,va[2], duDarkenCol(col));
dd->vertex(vb[0],vol->hmin,vb[2], duDarkenCol(col));
dd->vertex(va[0],vol->hmax,va[2], col);
dd->vertex(vb[0],vol->hmax,vb[2], col);
dd->vertex(va[0],vol->hmin,va[2], duDarkenCol(col));
dd->vertex(va[0],vol->hmax,va[2], col);
const float* va = &vol->verts[k * 3];
const float* vb = &vol->verts[j * 3];
dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
dd->vertex(vb[0], vol->hmin, vb[2], duDarkenCol(col));
dd->vertex(va[0], vol->hmax, va[2], col);
dd->vertex(vb[0], vol->hmax, vb[2], col);
dd->vertex(va[0], vol->hmin, va[2], duDarkenCol(col));
dd->vertex(va[0], vol->hmax, va[2], col);
}
}
dd->end();
@@ -601,13 +619,11 @@ void InputGeom::drawConvexVolumes(struct duDebugDraw* dd, bool /*hilight*/)
unsigned int col = duDarkenCol(duTransCol(dd->areaToCol(vol->area), 220));
for (int j = 0; j < vol->nverts; ++j)
{
dd->vertex(vol->verts[j*3+0],vol->verts[j*3+1]+0.1f,vol->verts[j*3+2], col);
dd->vertex(vol->verts[j*3+0],vol->hmin,vol->verts[j*3+2], col);
dd->vertex(vol->verts[j*3+0],vol->hmax,vol->verts[j*3+2], col);
dd->vertex(vol->verts[j * 3 + 0], vol->verts[j * 3 + 1] + 0.1f, vol->verts[j * 3 + 2], col);
dd->vertex(vol->verts[j * 3 + 0], vol->hmin, vol->verts[j * 3 + 2], col);
dd->vertex(vol->verts[j * 3 + 0], vol->hmax, vol->verts[j * 3 + 2], col);
}
}
dd->end();
dd->depthMask(true);
}