diff --git a/Source/Framework/Constants/MapConst.cs b/Source/Framework/Constants/MapConst.cs index 1b08c1d29..cc2389d42 100644 --- a/Source/Framework/Constants/MapConst.cs +++ b/Source/Framework/Constants/MapConst.cs @@ -71,7 +71,7 @@ namespace Framework.Constants public const uint MapLiquidMagic = 0x51494C4D; //"MLIQ"; public const uint mmapMagic = 0x4D4D4150; // 'MMAP' - public const int mmapVersion = 9; + public const int mmapVersion = 10; public const string VMapMagic = "VMAP_4.9"; public const float VMAPInvalidHeightValue = -200000.0f; diff --git a/Source/Framework/RecastDetour/Detour/DetourCommon.cs b/Source/Framework/RecastDetour/Detour/DetourCommon.cs index 35d94a3b7..64136917c 100644 --- a/Source/Framework/RecastDetour/Detour/DetourCommon.cs +++ b/Source/Framework/RecastDetour/Detour/DetourCommon.cs @@ -358,8 +358,8 @@ public static partial class Detour // Find sub triangle weighted by area. float thr = s * areasum; float acc = 0.0f; - float u = 0.0f; - int tri = 0; + float u = 1.0f; + int tri = npts - 1; for (int i = 2; i < npts; i++) { float dacc = areas[i]; diff --git a/Source/Framework/RecastDetour/Detour/DetourNavMesh.cs b/Source/Framework/RecastDetour/Detour/DetourNavMesh.cs index 8524b4ffc..00c4e1804 100644 --- a/Source/Framework/RecastDetour/Detour/DetourNavMesh.cs +++ b/Source/Framework/RecastDetour/Detour/DetourNavMesh.cs @@ -570,13 +570,13 @@ public static partial class Detour if (targetPoly.firstLink == DT_NULL_LINK) continue; - float[] ext = new float[] { targetCon.rad, target.header.walkableClimb, targetCon.rad }; + float[] halfExtents = new float[] { targetCon.rad, target.header.walkableClimb, targetCon.rad }; // Find polygon to connect to. //const float* p = &targetCon.pos[3]; int pIndex = 3; float[] nearestPt = new float[3]; - dtPolyRef polyRef = findNearestPolyInTile(tile, targetCon.pos, pIndex, ext, nearestPt); + dtPolyRef polyRef = findNearestPolyInTile(tile, targetCon.pos, pIndex, halfExtents, nearestPt); if (polyRef == 0) continue; // findNearestPoly may return too optimistic results, further check to make sure. @@ -676,13 +676,13 @@ public static partial class Detour dtOffMeshConnection con = tile.offMeshCons[i]; dtPoly poly = tile.polys[con.poly]; - float[] ext = new float[] { con.rad, tile.header.walkableClimb, con.rad }; + float[] halfExtents = new float[] { con.rad, tile.header.walkableClimb, con.rad }; // Find polygon to connect to. //const float* p = &con.pos[0]; // First vertex float[] nearestPt = new float[3]; - dtPolyRef polyRef = findNearestPolyInTile(tile, con.pos, 0, ext, nearestPt); + dtPolyRef polyRef = findNearestPolyInTile(tile, con.pos, 0, halfExtents, nearestPt); if (polyRef == 0) continue; // findNearestPoly may return too optimistic results, further check to make sure. @@ -812,7 +812,7 @@ public static partial class Detour } } float h = .0f; - if (dtClosestHeightPointTriangle(pos, posStart, vArrays[0], vIndices[0], vArrays[1], vIndices[1], vArrays[2], vIndices[2], ref h)) + if (dtClosestHeightPointTriangle(closest, posStart, vArrays[0], vIndices[0], vArrays[1], vIndices[1], vArrays[2], vIndices[2], ref h)) { closest[1] = h; break; @@ -820,12 +820,12 @@ public static partial class Detour } } - public dtPolyRef findNearestPolyInTile(dtMeshTile tile, float[] center, int centerStart, float[] extents, float[] nearestPt) + public dtPolyRef findNearestPolyInTile(dtMeshTile tile, float[] center, int centerStart, float[] halfExtents, float[] nearestPt) { float[] bmin = new float[3];//, bmax[3]; float[] bmax = new float[3]; - dtVsub(bmin, 0, center, centerStart, extents, 0); - dtVadd(bmax, 0, center, centerStart, extents, 0); + dtVsub(bmin, 0, center, centerStart, halfExtents, 0); + dtVadd(bmax, 0, center, centerStart, halfExtents, 0); // Get nearby polygons from proximity grid. dtPolyRef[] polys = new dtPolyRef[128]; diff --git a/Source/Framework/RecastDetour/Detour/DetourNavMeshBuilder.cs b/Source/Framework/RecastDetour/Detour/DetourNavMeshBuilder.cs index 9c1b89e78..2d7681219 100644 --- a/Source/Framework/RecastDetour/Detour/DetourNavMeshBuilder.cs +++ b/Source/Framework/RecastDetour/Detour/DetourNavMeshBuilder.cs @@ -906,44 +906,73 @@ public static partial class Detour } } - public static int createBVTree(ushort[] verts, int nverts, ushort[] polys, int npolys, int nvp, float cs, float ch, int nnodes, dtBVNode[] nodes) + public static int createBVTree(dtNavMeshCreateParams createParams, dtBVNode[] nodes, int nnodes) { // Build tree - BVItem[] items = new BVItem[npolys];//(BVItem*)dtAlloc(sizeof(BVItem)*npolys, DT_ALLOC_TEMP); + float quantFactor = 1 / createParams.cs; + BVItem[] items = new BVItem[createParams.polyCount];//(BVItem*)dtAlloc(sizeof(BVItem)*npolys, DT_ALLOC_TEMP); dtcsArrayItemsCreate(items); - for (int i = 0; i < npolys; i++) + for (int i = 0; i < createParams.polyCount; i++) { BVItem it = items[i]; it.i = i; - // Calc polygon bounds. - //const ushort* p = &polys[i*nvp*2]; - int pIndex = i * nvp * 2; - it.bmin[0] = it.bmax[0] = verts[polys[pIndex + 0] * 3 + 0]; - it.bmin[1] = it.bmax[1] = verts[polys[pIndex + 0] * 3 + 1]; - it.bmin[2] = it.bmax[2] = verts[polys[pIndex + 0] * 3 + 2]; - - for (int j = 1; j < nvp; ++j) + // Calc polygon bounds. Use detail meshes if available. + if (createParams.detailMeshes != null) { - if (polys[pIndex + j] == MESH_NULL_IDX) break; - ushort x = verts[polys[pIndex + j] * 3 + 0]; - ushort y = verts[polys[pIndex + j] * 3 + 1]; - ushort z = verts[polys[pIndex + j] * 3 + 2]; + int vb = (int)createParams.detailMeshes[i * 4 + 0]; + int ndv = (int)createParams.detailMeshes[i * 4 + 1]; + float[] bmin = new float[3]; + float[] bmax = new float[3]; - 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; + float[] dv = createParams.detailVerts[(vb * 3)..]; + dtVcopy(bmin, dv); + dtVcopy(bmax, dv); - 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; + 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] = (ushort)dtClamp((int)((bmin[0] - createParams.bmin[0]) * quantFactor), 0, 0xffff); + it.bmin[1] = (ushort)dtClamp((int)((bmin[1] - createParams.bmin[1]) * quantFactor), 0, 0xffff); + it.bmin[2] = (ushort)dtClamp((int)((bmin[2] - createParams.bmin[2]) * quantFactor), 0, 0xffff); + + it.bmax[0] = (ushort)dtClamp((int)((bmax[0] - createParams.bmin[0]) * quantFactor), 0, 0xffff); + it.bmax[1] = (ushort)dtClamp((int)((bmax[1] - createParams.bmin[1]) * quantFactor), 0, 0xffff); + it.bmax[2] = (ushort)dtClamp((int)((bmax[2] - createParams.bmin[2]) * quantFactor), 0, 0xffff); + } + else + { + ushort[] p = createParams.polys[(i * createParams.nvp * 2)..]; + it.bmin[0] = it.bmax[0] = createParams.verts[p[0] * 3 + 0]; + it.bmin[1] = it.bmax[1] = createParams.verts[p[0] * 3 + 1]; + it.bmin[2] = it.bmax[2] = createParams.verts[p[0] * 3 + 2]; + + for (int j = 1; j < createParams.nvp; ++j) + { + if (p[j] == MESH_NULL_IDX) break; + ushort x = createParams.verts[p[j] * 3 + 0]; + ushort y = createParams.verts[p[j] * 3 + 1]; + ushort z = createParams.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] = (ushort)MathF.Floor((float)it.bmin[1] * createParams.ch / createParams.cs); + it.bmax[1] = (ushort)MathF.Ceiling((float)it.bmax[1] * createParams.ch / createParams.cs); } - // Remap y - it.bmin[1] = (ushort)Math.Floor((float)it.bmin[1] * ch / cs); - it.bmax[1] = (ushort)Math.Ceiling((float)it.bmax[1] * ch / cs); } int curNode = 0; - subdivide(items, npolys, 0, npolys, ref curNode, nodes); + subdivide(items, createParams.polyCount, 0, createParams.polyCount, ref curNode, nodes); //dtFree(items); @@ -1412,8 +1441,7 @@ public static partial class Detour // TODO: take detail mesh into account! use byte per bbox extent? if (createParams.buildBvTree) { - createBVTree(createParams.verts, createParams.vertCount, createParams.polys, createParams.polyCount, - nvp, createParams.cs, createParams.ch, createParams.polyCount * 2, navBvtree); + createBVTree(createParams, navBvtree, createParams.polyCount * 2); } // Store Off-Mesh connections. diff --git a/Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs b/Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs index d8628a609..75b7ec7e3 100644 --- a/Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs +++ b/Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs @@ -709,7 +709,7 @@ public static partial class Detour } } float h = .0f; - if (dtClosestHeightPointTriangle(pos, 0, vSrc[0], vStarts[0], vSrc[1], vStarts[1], vSrc[2], vStarts[2], ref h)) + if (dtClosestHeightPointTriangle(closest, 0, vSrc[0], vStarts[0], vSrc[1], vStarts[1], vSrc[2], vStarts[2], ref h)) { closest[1] = h; break; @@ -864,7 +864,7 @@ public static partial class Detour /// Finds the polygon nearest to the specified center point. /// @param[in] center The center of the search box. [(x, y, z)] - /// @param[in] extents The search distance along each axis. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] /// @param[in] filter The polygon filter to apply to the query. /// @param[out] nearestRef The reference id of the nearest polygon. /// @param[out] nearestPt The nearest point on the polygon. [opt] [(x, y, z)] @@ -878,7 +878,7 @@ public static partial class Detour // @warning This function is not suitable for large area searches. If the search /// extents overlaps more than 128 polygons it may return an invalid result. /// - public dtStatus findNearestPoly(float[] center, float[] extents, dtQueryFilter filter, ref dtPolyRef nearestRef, ref float[] nearestPt) + public dtStatus findNearestPoly(float[] center, float[] halfExtents, dtQueryFilter filter, ref dtPolyRef nearestRef, ref float[] nearestPt) { Debug.Assert(m_nav != null); @@ -886,7 +886,7 @@ public static partial class Detour dtFindNearestPolyQuery query = new dtFindNearestPolyQuery(this, center); - dtStatus status = queryPolygons(center, extents, filter, query); + dtStatus status = queryPolygons(center, halfExtents, filter, query); if (dtStatusFailed(status)) return status; @@ -1030,7 +1030,7 @@ public static partial class Detour /// Finds polygons that overlap the search box. /// @param[in] center The center of the search box. [(x, y, z)] - /// @param[in] extents The search distance along each axis. [(x, y, z)] + /// @param[in] halfExtents The search distance along each axis. [(x, y, z)] /// @param[in] filter The polygon filter to apply to the query. /// @param[out] polys The reference ids of the polygons that overlap the query box. /// @param[out] polyCount The number of polygons in the search result. @@ -1045,14 +1045,14 @@ public static partial class Detour /// be filled to capacity. The method of choosing which polygons from the /// full set are included in the partial result set is undefined. /// - public dtStatus queryPolygons(float[] center, float[] extents, dtQueryFilter filter, dtFindNearestPolyQuery query) + public dtStatus queryPolygons(float[] center, float[] halfExtents, dtQueryFilter filter, dtFindNearestPolyQuery query) { Debug.Assert(m_nav != null); float[] bmin = new float[3]; float[] bmax = new float[3]; - dtVsub(bmin, center, extents); - dtVadd(bmax, center, extents); + dtVsub(bmin, center, halfExtents); + dtVadd(bmax, center, halfExtents); // Find tiles the query touches. int minx = 0, miny = 0, maxx = 0, maxy = 0;