Take detail mesh into account for creating BV-tree (recastnavigation#231

)

In U and A shaped terrain it was possible that polygons extruded their
BV-tree nodes significantly. This happened because all points for the
polygon (when excluding the detail mesh) were either on the high-level
terrain, or on the low-level terrain. This fixes the long-standing todo
about this.

As a result of this change, findNearestPoly and queryPolygons should
return more accurate results.

Fix recastnavigation#230

Detour/Source/DetourNavMeshBuilder.cpp

@@ -168,45 +168,72 @@ static void subdivide(BVItem* items, int nitems, int imin, int imax, int& curNod
 	}
 }
 
-static int createBVTree(const unsigned short* verts, const int /*nverts*/,
-						const unsigned short* polys, const int npolys, const int nvp,
-						const float cs, const float ch,
-						const int /*nnodes*/, dtBVNode* nodes)
+static int createBVTree(dtNavMeshCreateParams* params, dtBVNode* nodes, int /*nnodes*/)
 {
 	// Build tree
-	BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*npolys, DT_ALLOC_TEMP);
-	for (int i = 0; i < npolys; i++)
+	float quantFactor = 1 / params->cs;
+	BVItem* items = (BVItem*)dtAlloc(sizeof(BVItem)*params->polyCount, DT_ALLOC_TEMP);
+	for (int i = 0; i < params->polyCount; i++)
 	{
 		BVItem& it = items[i];
 		it.i = i;
-		// Calc polygon bounds.
-		const unsigned short* p = &polys[i*nvp*2];
-		it.bmin[0] = it.bmax[0] = verts[p[0]*3+0];
-		it.bmin[1] = it.bmax[1] = verts[p[0]*3+1];
-		it.bmin[2] = it.bmax[2] = verts[p[0]*3+2];
-
-		for (int j = 1; j < nvp; ++j)
+		// Calc polygon bounds. Use detail meshes if available.
+		if (params->detailMeshes)
 		{
-			if (p[j] == MESH_NULL_IDX) break;
-			unsigned short x = verts[p[j]*3+0];
-			unsigned short y = verts[p[j]*3+1];
-			unsigned short z = verts[p[j]*3+2];
-
-			if (x < it.bmin[0]) it.bmin[0] = x;
-			if (y < it.bmin[1]) it.bmin[1] = y;
-			if (z < it.bmin[2]) it.bmin[2] = z;
-
-			if (x > it.bmax[0]) it.bmax[0] = x;
-			if (y > it.bmax[1]) it.bmax[1] = y;
-			if (z > it.bmax[2]) it.bmax[2] = z;
+			int vb = (int)params->detailMeshes[i*4+0];
+			int ndv = (int)params->detailMeshes[i*4+1];
+			float bmin[3];
+			float bmax[3];
+
+			const float* dv = &params->detailVerts[vb*3];
+			dtVcopy(bmin, dv);
+			dtVcopy(bmax, dv);
+
+			for (int j = 1; j < ndv; j++)
+			{
+				dtVmin(bmin, &dv[j * 3]);
+				dtVmax(bmax, &dv[j * 3]);
+			}
+
+			// BV-tree uses cs for all dimensions
+			it.bmin[0] = (unsigned short)dtClamp((int)((bmin[0] - params->bmin[0])*quantFactor), 0, 0xffff);
+			it.bmin[1] = (unsigned short)dtClamp((int)((bmin[1] - params->bmin[1])*quantFactor), 0, 0xffff);
+			it.bmin[2] = (unsigned short)dtClamp((int)((bmin[2] - params->bmin[2])*quantFactor), 0, 0xffff);
+
+			it.bmax[0] = (unsigned short)dtClamp((int)((bmax[0] - params->bmin[0])*quantFactor), 0, 0xffff);
+			it.bmax[1] = (unsigned short)dtClamp((int)((bmax[1] - params->bmin[1])*quantFactor), 0, 0xffff);
+			it.bmax[2] = (unsigned short)dtClamp((int)((bmax[2] - params->bmin[2])*quantFactor), 0, 0xffff);
+		}
+		else
+		{
+			const unsigned short* p = &params->polys[i*params->nvp * 2];
+			it.bmin[0] = it.bmax[0] = params->verts[p[0] * 3 + 0];
+			it.bmin[1] = it.bmax[1] = params->verts[p[0] * 3 + 1];
+			it.bmin[2] = it.bmax[2] = params->verts[p[0] * 3 + 2];
+
+			for (int j = 1; j < params->nvp; ++j)
+			{
+				if (p[j] == MESH_NULL_IDX) break;
+				unsigned short x = params->verts[p[j] * 3 + 0];
+				unsigned short y = params->verts[p[j] * 3 + 1];
+				unsigned short z = params->verts[p[j] * 3 + 2];
+
+				if (x < it.bmin[0]) it.bmin[0] = x;
+				if (y < it.bmin[1]) it.bmin[1] = y;
+				if (z < it.bmin[2]) it.bmin[2] = z;
+
+				if (x > it.bmax[0]) it.bmax[0] = x;
+				if (y > it.bmax[1]) it.bmax[1] = y;
+				if (z > it.bmax[2]) it.bmax[2] = z;
+			}
+			// Remap y
+			it.bmin[1] = (unsigned short)dtMathFloorf((float)it.bmin[1] * params->ch / params->cs);
+			it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1] * params->ch / params->cs);
 		}
-		// Remap y
-		it.bmin[1] = (unsigned short)dtMathFloorf((float)it.bmin[1]*ch/cs);
-		it.bmax[1] = (unsigned short)dtMathCeilf((float)it.bmax[1]*ch/cs);
 	}
 
 	int curNode = 0;
-	subdivide(items, npolys, 0, npolys, curNode, nodes);
+	subdivide(items, params->polyCount, 0, params->polyCount, curNode, nodes);
 
 	dtFree(items);
 
@@ -595,11 +622,9 @@ bool dtCreateNavMeshData(dtNavMeshCreateParams* params, unsigned char** outData,
 	}
 
 	// Store and create BVtree.
-	// TODO: take detail mesh into account! use byte per bbox extent?
 	if (params->buildBvTree)
 	{
-		createBVTree(params->verts, params->vertCount, params->polys, params->polyCount,
-					 nvp, params->cs, params->ch, params->polyCount*2, navBvtree);
+		createBVTree(params, navBvtree, 2*params->polyCount);
 	}
 
 	// Store Off-Mesh connections.