diff options
Diffstat (limited to 'extern/recastnavigation/Recast/Source')
-rw-r--r-- | extern/recastnavigation/Recast/Source/Recast.cpp | 272 | ||||
-rw-r--r-- | extern/recastnavigation/Recast/Source/RecastContour.cpp | 732 | ||||
-rw-r--r-- | extern/recastnavigation/Recast/Source/RecastFilter.cpp | 249 | ||||
-rw-r--r-- | extern/recastnavigation/Recast/Source/RecastLog.cpp | 77 | ||||
-rw-r--r-- | extern/recastnavigation/Recast/Source/RecastMesh.cpp | 1218 | ||||
-rw-r--r-- | extern/recastnavigation/Recast/Source/RecastMeshDetail.cpp | 993 | ||||
-rw-r--r-- | extern/recastnavigation/Recast/Source/RecastRasterization.cpp | 308 | ||||
-rw-r--r-- | extern/recastnavigation/Recast/Source/RecastRegion.cpp | 1081 | ||||
-rw-r--r-- | extern/recastnavigation/Recast/Source/RecastTimer.cpp | 58 |
9 files changed, 4988 insertions, 0 deletions
diff --git a/extern/recastnavigation/Recast/Source/Recast.cpp b/extern/recastnavigation/Recast/Source/Recast.cpp new file mode 100644 index 00000000000..0db26c2c1cd --- /dev/null +++ b/extern/recastnavigation/Recast/Source/Recast.cpp @@ -0,0 +1,272 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include <float.h> +#define _USE_MATH_DEFINES +#include <math.h> +#include <string.h> +#include <stdlib.h> +#include <stdio.h> +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +void rcIntArray::resize(int n) +{ + if (n > m_cap) + { + if (!m_cap) m_cap = 8; + while (m_cap < n) m_cap *= 2; + int* newData = new int[m_cap]; + if (m_size && newData) memcpy(newData, m_data, m_size*sizeof(int)); + delete [] m_data; + m_data = newData; + } + m_size = n; +} + +void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax) +{ + // Calculate bounding box. + vcopy(bmin, verts); + vcopy(bmax, verts); + for (int i = 1; i < nv; ++i) + { + const float* v = &verts[i*3]; + vmin(bmin, v); + vmax(bmax, v); + } +} + +void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h) +{ + *w = (int)((bmax[0] - bmin[0])/cs+0.5f); + *h = (int)((bmax[2] - bmin[2])/cs+0.5f); +} + +bool rcCreateHeightfield(rcHeightfield& hf, int width, int height, + const float* bmin, const float* bmax, + float cs, float ch) +{ + hf.width = width; + hf.height = height; + hf.spans = new rcSpan*[hf.width*hf.height]; + vcopy(hf.bmin, bmin); + vcopy(hf.bmax, bmax); + hf.cs = cs; + hf.ch = ch; + if (!hf.spans) + return false; + memset(hf.spans, 0, sizeof(rcSpan*)*hf.width*hf.height); + return true; +} + +static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm) +{ + float e0[3], e1[3]; + vsub(e0, v1, v0); + vsub(e1, v2, v0); + vcross(norm, e0, e1); + vnormalize(norm); +} + +void rcMarkWalkableTriangles(const float walkableSlopeAngle, + const float* verts, int nv, + const int* tris, int nt, + unsigned char* flags) +{ + const float walkableThr = cosf(walkableSlopeAngle/180.0f*(float)M_PI); + + float norm[3]; + + for (int i = 0; i < nt; ++i) + { + const int* tri = &tris[i*3]; + calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm); + // Check if the face is walkable. + if (norm[1] > walkableThr) + flags[i] |= RC_WALKABLE; + } +} + +static int getSpanCount(unsigned char flags, rcHeightfield& hf) +{ + const int w = hf.width; + const int h = hf.height; + int spanCount = 0; + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + for (rcSpan* s = hf.spans[x + y*w]; s; s = s->next) + { + if (s->flags == flags) + spanCount++; + } + } + } + return spanCount; +} + +inline void setCon(rcCompactSpan& s, int dir, int i) +{ + s.con &= ~(0xf << (dir*4)); + s.con |= (i&0xf) << (dir*4); +} + +bool rcBuildCompactHeightfield(const int walkableHeight, const int walkableClimb, + unsigned char flags, rcHeightfield& hf, + rcCompactHeightfield& chf) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const int w = hf.width; + const int h = hf.height; + const int spanCount = getSpanCount(flags, hf); + + // Fill in header. + chf.width = w; + chf.height = h; + chf.spanCount = spanCount; + chf.walkableHeight = walkableHeight; + chf.walkableClimb = walkableClimb; + chf.maxRegions = 0; + vcopy(chf.bmin, hf.bmin); + vcopy(chf.bmax, hf.bmax); + chf.bmax[1] += walkableHeight*hf.ch; + chf.cs = hf.cs; + chf.ch = hf.ch; + chf.cells = new rcCompactCell[w*h]; + if (!chf.cells) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)", w*h); + return false; + } + memset(chf.cells, 0, sizeof(rcCompactCell)*w*h); + chf.spans = new rcCompactSpan[spanCount]; + if (!chf.spans) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount); + return false; + } + memset(chf.spans, 0, sizeof(rcCompactSpan)*spanCount); + + const int MAX_HEIGHT = 0xffff; + + // Fill in cells and spans. + int idx = 0; + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcSpan* s = hf.spans[x + y*w]; + // If there are no spans at this cell, just leave the data to index=0, count=0. + if (!s) continue; + rcCompactCell& c = chf.cells[x+y*w]; + c.index = idx; + c.count = 0; + while (s) + { + if (s->flags == flags) + { + const int bot = (int)s->smax; + const int top = s->next ? (int)s->next->smin : MAX_HEIGHT; + chf.spans[idx].y = (unsigned short)rcClamp(bot, 0, 0xffff); + chf.spans[idx].h = (unsigned char)rcClamp(top - bot, 0, 0xff); + idx++; + c.count++; + } + s = s->next; + } + } + } + + // Find neighbour connections. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + rcCompactSpan& s = chf.spans[i]; + for (int dir = 0; dir < 4; ++dir) + { + setCon(s, dir, 0xf); + const int nx = x + rcGetDirOffsetX(dir); + const int ny = y + rcGetDirOffsetY(dir); + // First check that the neighbour cell is in bounds. + if (nx < 0 || ny < 0 || nx >= w || ny >= h) + continue; + // Iterate over all neighbour spans and check if any of the is + // accessible from current cell. + const rcCompactCell& nc = chf.cells[nx+ny*w]; + for (int k = (int)nc.index, nk = (int)(nc.index+nc.count); k < nk; ++k) + { + const rcCompactSpan& ns = chf.spans[k]; + const int bot = rcMax(s.y, ns.y); + const int top = rcMin(s.y+s.h, ns.y+ns.h); + + // Check that the gap between the spans is walkable, + // and that the climb height between the gaps is not too high. + if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb) + { + // Mark direction as walkable. + setCon(s, dir, k - (int)nc.index); + break; + } + } + } + } + } + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->buildCompact += rcGetDeltaTimeUsec(startTime, endTime); + + return true; +} + +static int getHeightfieldMemoryUsage(const rcHeightfield& hf) +{ + int size = 0; + size += sizeof(hf); + size += hf.width * hf.height * sizeof(rcSpan*); + + rcSpanPool* pool = hf.pools; + while (pool) + { + size += (sizeof(rcSpanPool) - sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL; + pool = pool->next; + } + return size; +} + +static int getCompactHeightFieldMemoryusage(const rcCompactHeightfield& chf) +{ + int size = 0; + size += sizeof(rcCompactHeightfield); + size += sizeof(rcCompactSpan) * chf.spanCount; + size += sizeof(rcCompactCell) * chf.width * chf.height; + return size; +} diff --git a/extern/recastnavigation/Recast/Source/RecastContour.cpp b/extern/recastnavigation/Recast/Source/RecastContour.cpp new file mode 100644 index 00000000000..96f763a18f3 --- /dev/null +++ b/extern/recastnavigation/Recast/Source/RecastContour.cpp @@ -0,0 +1,732 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#define _USE_MATH_DEFINES +#include <math.h> +#include <string.h> +#include <stdio.h> +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +static int getCornerHeight(int x, int y, int i, int dir, + const rcCompactHeightfield& chf, + bool& isBorderVertex) +{ + const rcCompactSpan& s = chf.spans[i]; + int ch = (int)s.y; + int dirp = (dir+1) & 0x3; + + unsigned short regs[4] = {0,0,0,0}; + + regs[0] = s.reg; + + if (rcGetCon(s, dir) != 0xf) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir); + const rcCompactSpan& as = chf.spans[ai]; + ch = rcMax(ch, (int)as.y); + regs[1] = as.reg; + if (rcGetCon(as, dirp) != 0xf) + { + const int ax2 = ax + rcGetDirOffsetX(dirp); + const int ay2 = ay + rcGetDirOffsetY(dirp); + const int ai2 = (int)chf.cells[ax2+ay2*chf.width].index + rcGetCon(as, dirp); + const rcCompactSpan& as2 = chf.spans[ai2]; + ch = rcMax(ch, (int)as2.y); + regs[2] = as2.reg; + } + } + if (rcGetCon(s, dirp) != 0xf) + { + const int ax = x + rcGetDirOffsetX(dirp); + const int ay = y + rcGetDirOffsetY(dirp); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dirp); + const rcCompactSpan& as = chf.spans[ai]; + ch = rcMax(ch, (int)as.y); + regs[3] = as.reg; + if (rcGetCon(as, dir) != 0xf) + { + const int ax2 = ax + rcGetDirOffsetX(dir); + const int ay2 = ay + rcGetDirOffsetY(dir); + const int ai2 = (int)chf.cells[ax2+ay2*chf.width].index + rcGetCon(as, dir); + const rcCompactSpan& as2 = chf.spans[ai2]; + ch = rcMax(ch, (int)as2.y); + regs[2] = as2.reg; + } + } + + // Check if the vertex is special edge vertex, these vertices will be removed later. + for (int j = 0; j < 4; ++j) + { + const int a = j; + const int b = (j+1) & 0x3; + const int c = (j+2) & 0x3; + const int d = (j+3) & 0x3; + + // The vertex is a border vertex there are two same exterior cells in a row, + // followed by two interior cells and none of the regions are out of bounds. + const bool twoSameExts = (regs[a] & regs[b] & RC_BORDER_REG) != 0 && regs[a] == regs[b]; + const bool twoInts = ((regs[c] | regs[d]) & RC_BORDER_REG) == 0; + const bool noZeros = regs[a] != 0 && regs[b] != 0 && regs[c] != 0 && regs[d] != 0; + if (twoSameExts && twoInts && noZeros) + { + isBorderVertex = true; + break; + } + } + + return ch; +} + +static void walkContour(int x, int y, int i, + rcCompactHeightfield& chf, + unsigned char* flags, rcIntArray& points) +{ + // Choose the first non-connected edge + unsigned char dir = 0; + while ((flags[i] & (1 << dir)) == 0) + dir++; + + unsigned char startDir = dir; + int starti = i; + + int iter = 0; + while (++iter < 40000) + { + if (flags[i] & (1 << dir)) + { + // Choose the edge corner + bool isBorderVertex = false; + int px = x; + int py = getCornerHeight(x, y, i, dir, chf, isBorderVertex); + int pz = y; + switch(dir) + { + case 0: pz++; break; + case 1: px++; pz++; break; + case 2: px++; break; + } + int r = 0; + const rcCompactSpan& s = chf.spans[i]; + if (rcGetCon(s, dir) != 0xf) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir); + const rcCompactSpan& as = chf.spans[ai]; + r = (int)as.reg; + } + if (isBorderVertex) + r |= RC_BORDER_VERTEX; + points.push(px); + points.push(py); + points.push(pz); + points.push(r); + + flags[i] &= ~(1 << dir); // Remove visited edges + dir = (dir+1) & 0x3; // Rotate CW + } + else + { + int ni = -1; + const int nx = x + rcGetDirOffsetX(dir); + const int ny = y + rcGetDirOffsetY(dir); + const rcCompactSpan& s = chf.spans[i]; + if (rcGetCon(s, dir) != 0xf) + { + const rcCompactCell& nc = chf.cells[nx+ny*chf.width]; + ni = (int)nc.index + rcGetCon(s, dir); + } + if (ni == -1) + { + // Should not happen. + return; + } + x = nx; + y = ny; + i = ni; + dir = (dir+3) & 0x3; // Rotate CCW + } + + if (starti == i && startDir == dir) + { + break; + } + } +} + +static float distancePtSeg(int x, int y, int z, + int px, int py, int pz, + int qx, int qy, int qz) +{ +/* float pqx = (float)(qx - px); + float pqy = (float)(qy - py); + float pqz = (float)(qz - pz); + float dx = (float)(x - px); + float dy = (float)(y - py); + float dz = (float)(z - pz); + float d = pqx*pqx + pqy*pqy + pqz*pqz; + float t = pqx*dx + pqy*dy + pqz*dz; + if (d > 0) + t /= d; + if (t < 0) + t = 0; + else if (t > 1) + t = 1; + + dx = px + t*pqx - x; + dy = py + t*pqy - y; + dz = pz + t*pqz - z; + + return dx*dx + dy*dy + dz*dz;*/ + + float pqx = (float)(qx - px); + float pqz = (float)(qz - pz); + float dx = (float)(x - px); + float dz = (float)(z - pz); + float d = pqx*pqx + pqz*pqz; + float t = pqx*dx + pqz*dz; + if (d > 0) + t /= d; + if (t < 0) + t = 0; + else if (t > 1) + t = 1; + + dx = px + t*pqx - x; + dz = pz + t*pqz - z; + + return dx*dx + dz*dz; +} + +static void simplifyContour(rcIntArray& points, rcIntArray& simplified, float maxError, int maxEdgeLen) +{ + // Add initial points. + bool noConnections = true; + for (int i = 0; i < points.size(); i += 4) + { + if ((points[i+3] & 0xffff) != 0) + { + noConnections = false; + break; + } + } + + if (noConnections) + { + // If there is no connections at all, + // create some initial points for the simplification process. + // Find lower-left and upper-right vertices of the contour. + int llx = points[0]; + int lly = points[1]; + int llz = points[2]; + int lli = 0; + int urx = points[0]; + int ury = points[1]; + int urz = points[2]; + int uri = 0; + for (int i = 0; i < points.size(); i += 4) + { + int x = points[i+0]; + int y = points[i+1]; + int z = points[i+2]; + if (x < llx || (x == llx && z < llz)) + { + llx = x; + lly = y; + llz = z; + lli = i/4; + } + if (x >= urx || (x == urx && z > urz)) + { + urx = x; + ury = y; + urz = z; + uri = i/4; + } + } + simplified.push(llx); + simplified.push(lly); + simplified.push(llz); + simplified.push(lli); + + simplified.push(urx); + simplified.push(ury); + simplified.push(urz); + simplified.push(uri); + } + else + { + // The contour has some portals to other regions. + // Add a new point to every location where the region changes. + for (int i = 0, ni = points.size()/4; i < ni; ++i) + { + int ii = (i+1) % ni; + if ((points[i*4+3] & 0xffff) != (points[ii*4+3] & 0xffff)) + { + simplified.push(points[i*4+0]); + simplified.push(points[i*4+1]); + simplified.push(points[i*4+2]); + simplified.push(i); + } + } + } + + // Add points until all raw points are within + // error tolerance to the simplified shape. + const int pn = points.size()/4; + for (int i = 0; i < simplified.size()/4; ) + { + int ii = (i+1) % (simplified.size()/4); + + int ax = simplified[i*4+0]; + int ay = simplified[i*4+1]; + int az = simplified[i*4+2]; + int ai = simplified[i*4+3]; + + int bx = simplified[ii*4+0]; + int by = simplified[ii*4+1]; + int bz = simplified[ii*4+2]; + int bi = simplified[ii*4+3]; + + // Find maximum deviation from the segment. + float maxd = 0; + int maxi = -1; + int ci = (ai+1) % pn; + + // Tesselate only outer edges. + if ((points[ci*4+3] & 0xffff) == 0) + { + while (ci != bi) + { + float d = distancePtSeg(points[ci*4+0], points[ci*4+1]/4, points[ci*4+2], + ax, ay/4, az, bx, by/4, bz); + if (d > maxd) + { + maxd = d; + maxi = ci; + } + ci = (ci+1) % pn; + } + } + + + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1 && maxd > (maxError*maxError)) + { + // Add space for the new point. + simplified.resize(simplified.size()+4); + int n = simplified.size()/4; + for (int j = n-1; j > i; --j) + { + simplified[j*4+0] = simplified[(j-1)*4+0]; + simplified[j*4+1] = simplified[(j-1)*4+1]; + simplified[j*4+2] = simplified[(j-1)*4+2]; + simplified[j*4+3] = simplified[(j-1)*4+3]; + } + // Add the point. + simplified[(i+1)*4+0] = points[maxi*4+0]; + simplified[(i+1)*4+1] = points[maxi*4+1]; + simplified[(i+1)*4+2] = points[maxi*4+2]; + simplified[(i+1)*4+3] = maxi; + } + else + { + ++i; + } + } + + // Split too long edges. + if (maxEdgeLen > 0) + { + for (int i = 0; i < simplified.size()/4; ) + { + int ii = (i+1) % (simplified.size()/4); + + int ax = simplified[i*4+0]; + int az = simplified[i*4+2]; + int ai = simplified[i*4+3]; + + int bx = simplified[ii*4+0]; + int bz = simplified[ii*4+2]; + int bi = simplified[ii*4+3]; + + // Find maximum deviation from the segment. + int maxi = -1; + int ci = (ai+1) % pn; + + // Tesselate only outer edges. + if ((points[ci*4+3] & 0xffff) == 0) + { + int dx = bx - ax; + int dz = bz - az; + if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen) + { + int n = bi < ai ? (bi+pn - ai) : (bi - ai); + maxi = (ai + n/2) % pn; + } + } + + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1) + { + // Add space for the new point. + simplified.resize(simplified.size()+4); + int n = simplified.size()/4; + for (int j = n-1; j > i; --j) + { + simplified[j*4+0] = simplified[(j-1)*4+0]; + simplified[j*4+1] = simplified[(j-1)*4+1]; + simplified[j*4+2] = simplified[(j-1)*4+2]; + simplified[j*4+3] = simplified[(j-1)*4+3]; + } + // Add the point. + simplified[(i+1)*4+0] = points[maxi*4+0]; + simplified[(i+1)*4+1] = points[maxi*4+1]; + simplified[(i+1)*4+2] = points[maxi*4+2]; + simplified[(i+1)*4+3] = maxi; + } + else + { + ++i; + } + } + } + + for (int i = 0; i < simplified.size()/4; ++i) + { + // The edge vertex flag is take from the current raw point, + // and the neighbour region is take from the next raw point. + const int ai = (simplified[i*4+3]+1) % pn; + const int bi = simplified[i*4+3]; + simplified[i*4+3] = (points[ai*4+3] & 0xffff) | (points[bi*4+3] & RC_BORDER_VERTEX); + } + +} + +static void removeDegenerateSegments(rcIntArray& simplified) +{ + // Remove adjacent vertices which are equal on xz-plane, + // or else the triangulator will get confused. + for (int i = 0; i < simplified.size()/4; ++i) + { + int ni = i+1; + if (ni >= (simplified.size()/4)) + ni = 0; + + if (simplified[i*4+0] == simplified[ni*4+0] && + simplified[i*4+2] == simplified[ni*4+2]) + { + // Degenerate segment, remove. + for (int j = i; j < simplified.size()/4-1; ++j) + { + simplified[j*4+0] = simplified[(j+1)*4+0]; + simplified[j*4+1] = simplified[(j+1)*4+1]; + simplified[j*4+2] = simplified[(j+1)*4+2]; + simplified[j*4+3] = simplified[(j+1)*4+3]; + } + simplified.pop(); + } + } +} + +static int calcAreaOfPolygon2D(const int* verts, const int nverts) +{ + int area = 0; + for (int i = 0, j = nverts-1; i < nverts; j=i++) + { + const int* vi = &verts[i*4]; + const int* vj = &verts[j*4]; + area += vi[0] * vj[2] - vj[0] * vi[2]; + } + return (area+1) / 2; +} + +static void getClosestIndices(const int* vertsa, const int nvertsa, + const int* vertsb, const int nvertsb, + int& ia, int& ib) +{ + int closestDist = 0xfffffff; + for (int i = 0; i < nvertsa; ++i) + { + const int* va = &vertsa[i*4]; + for (int j = 0; j < nvertsb; ++j) + { + const int* vb = &vertsb[j*4]; + const int dx = vb[0] - va[0]; + const int dz = vb[2] - va[2]; + const int d = dx*dx + dz*dz; + if (d < closestDist) + { + ia = i; + ib = j; + closestDist = d; + } + } + } +} + +static bool mergeContours(rcContour& ca, rcContour& cb, int ia, int ib) +{ + const int maxVerts = ca.nverts + cb.nverts + 2; + int* verts = new int[maxVerts*4]; + if (!verts) + return false; + + int nv = 0; + + // Copy contour A. + for (int i = 0; i <= ca.nverts; ++i) + { + int* dst = &verts[nv*4]; + const int* src = &ca.verts[((ia+i)%ca.nverts)*4]; + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + dst[3] = src[3]; + nv++; + } + + // Copy contour B + for (int i = 0; i <= cb.nverts; ++i) + { + int* dst = &verts[nv*4]; + const int* src = &cb.verts[((ib+i)%cb.nverts)*4]; + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + dst[3] = src[3]; + nv++; + } + + delete [] ca.verts; + ca.verts = verts; + ca.nverts = nv; + + delete [] cb.verts; + cb.verts = 0; + cb.nverts = 0; + + return true; +} + +bool rcBuildContours(rcCompactHeightfield& chf, + const float maxError, const int maxEdgeLen, + rcContourSet& cset) +{ + const int w = chf.width; + const int h = chf.height; + + rcTimeVal startTime = rcGetPerformanceTimer(); + + vcopy(cset.bmin, chf.bmin); + vcopy(cset.bmax, chf.bmax); + cset.cs = chf.cs; + cset.ch = chf.ch; + + const int maxContours = chf.maxRegions*2; + cset.conts = new rcContour[maxContours]; + if (!cset.conts) + return false; + cset.nconts = 0; + + unsigned char* flags = new unsigned char[chf.spanCount]; + if (!flags) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'flags'."); + return false; + } + + rcTimeVal traceStartTime = rcGetPerformanceTimer(); + + + // Mark boundaries. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + unsigned char res = 0; + const rcCompactSpan& s = chf.spans[i]; + if (!s.reg || (s.reg & RC_BORDER_REG)) + { + flags[i] = 0; + continue; + } + for (int dir = 0; dir < 4; ++dir) + { + unsigned short r = 0; + if (rcGetCon(s, dir) != 0xf) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); + const rcCompactSpan& as = chf.spans[ai]; + r = as.reg; + } + if (r == s.reg) + res |= (1 << dir); + } + flags[i] = res ^ 0xf; // Inverse, mark non connected edges. + } + } + } + + rcTimeVal traceEndTime = rcGetPerformanceTimer(); + + rcTimeVal simplifyStartTime = rcGetPerformanceTimer(); + + rcIntArray verts(256); + rcIntArray simplified(64); + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (flags[i] == 0 || flags[i] == 0xf) + { + flags[i] = 0; + continue; + } + unsigned short reg = chf.spans[i].reg; + if (!reg || (reg & RC_BORDER_REG)) + continue; + + verts.resize(0); + simplified.resize(0); + walkContour(x, y, i, chf, flags, verts); + simplifyContour(verts, simplified, maxError, maxEdgeLen); + removeDegenerateSegments(simplified); + + // Store region->contour remap info. + // Create contour. + if (simplified.size()/4 >= 3) + { + if (cset.nconts >= maxContours) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildContours: Too many contours %d, max %d.", cset.nconts, maxContours); + return false; + } + + rcContour* cont = &cset.conts[cset.nconts++]; + + cont->nverts = simplified.size()/4; + cont->verts = new int[cont->nverts*4]; + memcpy(cont->verts, &simplified[0], sizeof(int)*cont->nverts*4); + + cont->nrverts = verts.size()/4; + cont->rverts = new int[cont->nrverts*4]; + memcpy(cont->rverts, &verts[0], sizeof(int)*cont->nrverts*4); + +/* cont->cx = cont->cy = cont->cz = 0; + for (int i = 0; i < cont->nverts; ++i) + { + cont->cx += cont->verts[i*4+0]; + cont->cy += cont->verts[i*4+1]; + cont->cz += cont->verts[i*4+2]; + } + cont->cx /= cont->nverts; + cont->cy /= cont->nverts; + cont->cz /= cont->nverts;*/ + + cont->reg = reg; + } + } + } + } + + // Check and merge droppings. + // Sometimes the previous algorithms can fail and create several countours + // per area. This pass will try to merge the holes into the main region. + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + // Check if the contour is would backwards. + if (calcAreaOfPolygon2D(cont.verts, cont.nverts) < 0) + { + // Find another contour which has the same region ID. + int mergeIdx = -1; + for (int j = 0; j < cset.nconts; ++j) + { + if (i == j) continue; + if (cset.conts[j].nverts && cset.conts[j].reg == cont.reg) + { + // Make sure the polygon is correctly oriented. + if (calcAreaOfPolygon2D(cset.conts[j].verts, cset.conts[j].nverts)) + { + mergeIdx = j; + break; + } + } + } + if (mergeIdx == -1) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "rcBuildContours: Could not find merge target for bad contour %d.", i); + } + else + { + rcContour& mcont = cset.conts[mergeIdx]; + // Merge by closest points. + int ia, ib; + getClosestIndices(mcont.verts, mcont.nverts, cont.verts, cont.nverts, ia, ib); + if (!mergeContours(mcont, cont, ia, ib)) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "rcBuildContours: Failed to merge contours %d and %d.", i, mergeIdx); + } + } + } + } + + + delete [] flags; + + rcTimeVal simplifyEndTime = rcGetPerformanceTimer(); + + rcTimeVal endTime = rcGetPerformanceTimer(); + +// if (rcGetLog()) +// { +// rcGetLog()->log(RC_LOG_PROGRESS, "Create contours: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); +// rcGetLog()->log(RC_LOG_PROGRESS, " - boundary: %.3f ms", rcGetDeltaTimeUsec(boundaryStartTime, boundaryEndTime)/1000.0f); +// rcGetLog()->log(RC_LOG_PROGRESS, " - contour: %.3f ms", rcGetDeltaTimeUsec(contourStartTime, contourEndTime)/1000.0f); +// } + + if (rcGetBuildTimes()) + { + rcGetBuildTimes()->buildContours += rcGetDeltaTimeUsec(startTime, endTime); + rcGetBuildTimes()->buildContoursTrace += rcGetDeltaTimeUsec(traceStartTime, traceEndTime); + rcGetBuildTimes()->buildContoursSimplify += rcGetDeltaTimeUsec(simplifyStartTime, simplifyEndTime); + } + + return true; +} diff --git a/extern/recastnavigation/Recast/Source/RecastFilter.cpp b/extern/recastnavigation/Recast/Source/RecastFilter.cpp new file mode 100644 index 00000000000..ebe60714a18 --- /dev/null +++ b/extern/recastnavigation/Recast/Source/RecastFilter.cpp @@ -0,0 +1,249 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#define _USE_MATH_DEFINES +#include <math.h> +#include <stdio.h> +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +void rcFilterLedgeSpans(const int walkableHeight, + const int walkableClimb, + rcHeightfield& solid) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const int w = solid.width; + const int h = solid.height; + const int MAX_HEIGHT = 0xffff; + + // Mark border spans. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next) + { + // Skip non walkable spans. + if ((s->flags & RC_WALKABLE) == 0) + continue; + + const int bot = (int)s->smax; + const int top = s->next ? (int)s->next->smin : MAX_HEIGHT; + + // Find neighbours minimum height. + int minh = MAX_HEIGHT; + + for (int dir = 0; dir < 4; ++dir) + { + int dx = x + rcGetDirOffsetX(dir); + int dy = y + rcGetDirOffsetY(dir); + // Skip neighbours which are out of bounds. + if (dx < 0 || dy < 0 || dx >= w || dy >= h) + { + minh = rcMin(minh, -walkableClimb - bot); + continue; + } + + // From minus infinity to the first span. + rcSpan* ns = solid.spans[dx + dy*w]; + int nbot = -walkableClimb; + int ntop = ns ? (int)ns->smin : MAX_HEIGHT; + // Skip neightbour if the gap between the spans is too small. + if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight) + minh = rcMin(minh, nbot - bot); + + // Rest of the spans. + for (ns = solid.spans[dx + dy*w]; ns; ns = ns->next) + { + nbot = (int)ns->smax; + ntop = ns->next ? (int)ns->next->smin : MAX_HEIGHT; + // Skip neightbour if the gap between the spans is too small. + if (rcMin(top,ntop) - rcMax(bot,nbot) > walkableHeight) + minh = rcMin(minh, nbot - bot); + } + } + + // The current span is close to a ledge if the drop to any + // neighbour span is less than the walkableClimb. + if (minh < -walkableClimb) + s->flags &= ~RC_WALKABLE; + + } + } + } + + rcTimeVal endTime = rcGetPerformanceTimer(); +// if (rcGetLog()) +// rcGetLog()->log(RC_LOG_PROGRESS, "Filter border: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + if (rcGetBuildTimes()) + rcGetBuildTimes()->filterBorder += rcGetDeltaTimeUsec(startTime, endTime); +} + +void rcFilterWalkableLowHeightSpans(int walkableHeight, + rcHeightfield& solid) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const int w = solid.width; + const int h = solid.height; + const int MAX_HEIGHT = 0xffff; + + // Remove walkable flag from spans which do not have enough + // space above them for the agent to stand there. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + for (rcSpan* s = solid.spans[x + y*w]; s; s = s->next) + { + const int bot = (int)s->smax; + const int top = s->next ? (int)s->next->smin : MAX_HEIGHT; + if ((top - bot) <= walkableHeight) + s->flags &= ~RC_WALKABLE; + } + } + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + +// if (rcGetLog()) +// rcGetLog()->log(RC_LOG_PROGRESS, "Filter walkable: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + if (rcGetBuildTimes()) + rcGetBuildTimes()->filterWalkable += rcGetDeltaTimeUsec(startTime, endTime); +} + + +struct rcReachableSeed +{ + inline void set(int ix, int iy, rcSpan* is) + { + x = (unsigned short)ix; + y = (unsigned short)iy; + s = is; + } + unsigned short x, y; + rcSpan* s; +}; + +bool rcMarkReachableSpans(const int walkableHeight, + const int walkableClimb, + rcHeightfield& solid) +{ + const int w = solid.width; + const int h = solid.height; + const int MAX_HEIGHT = 0xffff; + + rcTimeVal startTime = rcGetPerformanceTimer(); + + // Build navigable space. + const int MAX_SEEDS = w*h; + rcReachableSeed* stack = new rcReachableSeed[MAX_SEEDS]; + if (!stack) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMarkReachableSpans: Out of memory 'stack' (%d).", MAX_SEEDS); + return false; + } + int stackSize = 0; + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + rcSpan* topSpan = solid.spans[x + y*w]; + if (!topSpan) + continue; + while (topSpan->next) + topSpan = topSpan->next; + + // If the span is not walkable, skip it. + if ((topSpan->flags & RC_WALKABLE) == 0) + continue; + // If the span has been visited already, skip it. + if (topSpan->flags & RC_REACHABLE) + continue; + + // Start flood fill. + topSpan->flags |= RC_REACHABLE; + stackSize = 0; + stack[stackSize].set(x, y, topSpan); + stackSize++; + + while (stackSize) + { + // Pop a seed from the stack. + stackSize--; + rcReachableSeed cur = stack[stackSize]; + + const int bot = (int)cur.s->smax; + const int top = cur.s->next ? (int)cur.s->next->smin : MAX_HEIGHT; + + // Visit neighbours in all 4 directions. + for (int dir = 0; dir < 4; ++dir) + { + int dx = (int)cur.x + rcGetDirOffsetX(dir); + int dy = (int)cur.y + rcGetDirOffsetY(dir); + // Skip neighbour which are out of bounds. + if (dx < 0 || dy < 0 || dx >= w || dy >= h) + continue; + for (rcSpan* ns = solid.spans[dx + dy*w]; ns; ns = ns->next) + { + // Skip neighbour if it is not walkable. + if ((ns->flags & RC_WALKABLE) == 0) + continue; + // Skip the neighbour if it has been visited already. + if (ns->flags & RC_REACHABLE) + continue; + + const int nbot = (int)ns->smax; + const int ntop = ns->next ? (int)ns->next->smin : MAX_HEIGHT; + // Skip neightbour if the gap between the spans is too small. + if (rcMin(top,ntop) - rcMax(bot,nbot) < walkableHeight) + continue; + // Skip neightbour if the climb height to the neighbour is too high. + if (rcAbs(nbot - bot) >= walkableClimb) + continue; + + // This neighbour has not been visited yet. + // Mark it as reachable and add it to the seed stack. + ns->flags |= RC_REACHABLE; + if (stackSize < MAX_SEEDS) + { + stack[stackSize].set(dx, dy, ns); + stackSize++; + } + } + } + } + } + } + + delete [] stack; + + rcTimeVal endTime = rcGetPerformanceTimer(); + +// if (rcGetLog()) +// rcGetLog()->log(RC_LOG_PROGRESS, "Mark reachable: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + if (rcGetBuildTimes()) + rcGetBuildTimes()->filterMarkReachable += rcGetDeltaTimeUsec(startTime, endTime); + + return true; +} diff --git a/extern/recastnavigation/Recast/Source/RecastLog.cpp b/extern/recastnavigation/Recast/Source/RecastLog.cpp new file mode 100644 index 00000000000..27868042a1c --- /dev/null +++ b/extern/recastnavigation/Recast/Source/RecastLog.cpp @@ -0,0 +1,77 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include "RecastLog.h" +#include <stdio.h> +#include <stdarg.h> + +static rcLog* g_log = 0; +static rcBuildTimes* g_btimes = 0; + +rcLog::rcLog() : + m_messageCount(0), + m_textPoolSize(0) +{ +} + +rcLog::~rcLog() +{ + if (g_log == this) + g_log = 0; +} + +void rcLog::log(rcLogCategory category, const char* format, ...) +{ + if (m_messageCount >= MAX_MESSAGES) + return; + char* dst = &m_textPool[m_textPoolSize]; + int n = TEXT_POOL_SIZE - m_textPoolSize; + if (n < 2) + return; + // Store category + *dst = (char)category; + n--; + // Store message + va_list ap; + va_start(ap, format); + int ret = vsnprintf(dst+1, n-1, format, ap); + va_end(ap); + if (ret > 0) + m_textPoolSize += ret+2; + m_messages[m_messageCount++] = dst; +} + +void rcSetLog(rcLog* log) +{ + g_log = log; +} + +rcLog* rcGetLog() +{ + return g_log; +} + +void rcSetBuildTimes(rcBuildTimes* btimes) +{ + g_btimes = btimes; +} + +rcBuildTimes* rcGetBuildTimes() +{ + return g_btimes; +} diff --git a/extern/recastnavigation/Recast/Source/RecastMesh.cpp b/extern/recastnavigation/Recast/Source/RecastMesh.cpp new file mode 100644 index 00000000000..38d62904213 --- /dev/null +++ b/extern/recastnavigation/Recast/Source/RecastMesh.cpp @@ -0,0 +1,1218 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#define _USE_MATH_DEFINES +#include <math.h> +#include <string.h> +#include <stdio.h> +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +struct rcEdge +{ + unsigned short vert[2]; + unsigned short polyEdge[2]; + unsigned short poly[2]; +}; + +/*static */bool buildMeshAdjacency(unsigned short* polys, const int npolys, + const int nverts, const int vertsPerPoly) +{ + // Based on code by Eric Lengyel from: + // http://www.terathon.com/code/edges.php + + int maxEdgeCount = npolys*vertsPerPoly; + unsigned short* firstEdge = new unsigned short[nverts + maxEdgeCount]; + if (!firstEdge) + return false; + unsigned short* nextEdge = firstEdge + nverts; + int edgeCount = 0; + + rcEdge* edges = new rcEdge[maxEdgeCount]; + if (!edges) + return false; + + for (int i = 0; i < nverts; i++) + firstEdge[i] = 0xffff; + + // Invalida indices are marked as 0xffff, the following code + // handles them just fine. + + for (int i = 0; i < npolys; ++i) + { + unsigned short* t = &polys[i*vertsPerPoly*2]; + for (int j = 0; j < vertsPerPoly; ++j) + { + unsigned short v0 = t[j]; + unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == 0xffff) ? t[0] : t[j+1]; + if (v0 < v1) + { + rcEdge& edge = edges[edgeCount]; + edge.vert[0] = v0; + edge.vert[1] = v1; + edge.poly[0] = (unsigned short)i; + edge.polyEdge[0] = (unsigned short)j; + edge.poly[1] = (unsigned short)i; + edge.polyEdge[1] = 0; + // Insert edge + nextEdge[edgeCount] = firstEdge[v0]; + firstEdge[v0] = edgeCount; + edgeCount++; + } + } + } + + for (int i = 0; i < npolys; ++i) + { + unsigned short* t = &polys[i*vertsPerPoly*2]; + for (int j = 0; j < vertsPerPoly; ++j) + { + unsigned short v0 = t[j]; + unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == 0xffff) ? t[0] : t[j+1]; + if (v0 > v1) + { + for (unsigned short e = firstEdge[v1]; e != 0xffff; e = nextEdge[e]) + { + rcEdge& edge = edges[e]; + if (edge.vert[1] == v0 && edge.poly[0] == edge.poly[1]) + { + edge.poly[1] = (unsigned short)i; + edge.polyEdge[1] = (unsigned short)j; + break; + } + } + } + } + } + + // Store adjacency + for (int i = 0; i < edgeCount; ++i) + { + const rcEdge& e = edges[i]; + if (e.poly[0] != e.poly[1]) + { + unsigned short* p0 = &polys[e.poly[0]*vertsPerPoly*2]; + unsigned short* p1 = &polys[e.poly[1]*vertsPerPoly*2]; + p0[vertsPerPoly + e.polyEdge[0]] = e.poly[1]; + p1[vertsPerPoly + e.polyEdge[1]] = e.poly[0]; + } + } + + delete [] firstEdge; + delete [] edges; + + return true; +} + + +static const int VERTEX_BUCKET_COUNT = (1<<12); + +inline int computeVertexHash(int x, int y, int z) +{ + const unsigned int h1 = 0x8da6b343; // Large multiplicative constants; + const unsigned int h2 = 0xd8163841; // here arbitrarily chosen primes + const unsigned int h3 = 0xcb1ab31f; + unsigned int n = h1 * x + h2 * y + h3 * z; + return (int)(n & (VERTEX_BUCKET_COUNT-1)); +} + +static int addVertex(unsigned short x, unsigned short y, unsigned short z, + unsigned short* verts, int* firstVert, int* nextVert, int& nv) +{ + int bucket = computeVertexHash(x, 0, z); + int i = firstVert[bucket]; + + while (i != -1) + { + const unsigned short* v = &verts[i*3]; + if (v[0] == x && (rcAbs(v[1] - y) <= 2) && v[2] == z) + return i; + i = nextVert[i]; // next + } + + // Could not find, create new. + i = nv; nv++; + unsigned short* v = &verts[i*3]; + v[0] = x; + v[1] = y; + v[2] = z; + nextVert[i] = firstVert[bucket]; + firstVert[bucket] = i; + + return i; +} + +inline int prev(int i, int n) { return i-1 >= 0 ? i-1 : n-1; } +inline int next(int i, int n) { return i+1 < n ? i+1 : 0; } + +inline int area2(const int* a, const int* b, const int* c) +{ + return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]); +} + +// Exclusive or: true iff exactly one argument is true. +// The arguments are negated to ensure that they are 0/1 +// values. Then the bitwise Xor operator may apply. +// (This idea is due to Michael Baldwin.) +inline bool xorb(bool x, bool y) +{ + return !x ^ !y; +} + +// Returns true iff c is strictly to the left of the directed +// line through a to b. +inline bool left(const int* a, const int* b, const int* c) +{ + return area2(a, b, c) < 0; +} + +inline bool leftOn(const int* a, const int* b, const int* c) +{ + return area2(a, b, c) <= 0; +} + +inline bool collinear(const int* a, const int* b, const int* c) +{ + return area2(a, b, c) == 0; +} + +// Returns true iff ab properly intersects cd: they share +// a point interior to both segments. The properness of the +// intersection is ensured by using strict leftness. +bool intersectProp(const int* a, const int* b, const int* c, const int* d) +{ + // Eliminate improper cases. + if (collinear(a,b,c) || collinear(a,b,d) || + collinear(c,d,a) || collinear(c,d,b)) + return false; + + return xorb(left(a,b,c), left(a,b,d)) && xorb(left(c,d,a), left(c,d,b)); +} + +// Returns T iff (a,b,c) are collinear and point c lies +// on the closed segement ab. +static bool between(const int* a, const int* b, const int* c) +{ + if (!collinear(a, b, c)) + return false; + // If ab not vertical, check betweenness on x; else on y. + if (a[0] != b[0]) + return ((a[0] <= c[0]) && (c[0] <= b[0])) || ((a[0] >= c[0]) && (c[0] >= b[0])); + else + return ((a[2] <= c[2]) && (c[2] <= b[2])) || ((a[2] >= c[2]) && (c[2] >= b[2])); +} + +// Returns true iff segments ab and cd intersect, properly or improperly. +static bool intersect(const int* a, const int* b, const int* c, const int* d) +{ + if (intersectProp(a, b, c, d)) + return true; + else if (between(a, b, c) || between(a, b, d) || + between(c, d, a) || between(c, d, b)) + return true; + else + return false; +} + +static bool vequal(const int* a, const int* b) +{ + return a[0] == b[0] && a[2] == b[2]; +} + +// Returns T iff (v_i, v_j) is a proper internal *or* external +// diagonal of P, *ignoring edges incident to v_i and v_j*. +static bool diagonalie(int i, int j, int n, const int* verts, int* indices) +{ + const int* d0 = &verts[(indices[i] & 0x0fffffff) * 4]; + const int* d1 = &verts[(indices[j] & 0x0fffffff) * 4]; + + // For each edge (k,k+1) of P + for (int k = 0; k < n; k++) + { + int k1 = next(k, n); + // Skip edges incident to i or j + if (!((k == i) || (k1 == i) || (k == j) || (k1 == j))) + { + const int* p0 = &verts[(indices[k] & 0x0fffffff) * 4]; + const int* p1 = &verts[(indices[k1] & 0x0fffffff) * 4]; + + if (vequal(d0, p0) || vequal(d1, p0) || vequal(d0, p1) || vequal(d1, p1)) + continue; + + if (intersect(d0, d1, p0, p1)) + return false; + } + } + return true; +} + +// Returns true iff the diagonal (i,j) is strictly internal to the +// polygon P in the neighborhood of the i endpoint. +static bool inCone(int i, int j, int n, const int* verts, int* indices) +{ + const int* pi = &verts[(indices[i] & 0x0fffffff) * 4]; + const int* pj = &verts[(indices[j] & 0x0fffffff) * 4]; + const int* pi1 = &verts[(indices[next(i, n)] & 0x0fffffff) * 4]; + const int* pin1 = &verts[(indices[prev(i, n)] & 0x0fffffff) * 4]; + + // If P[i] is a convex vertex [ i+1 left or on (i-1,i) ]. + if (leftOn(pin1, pi, pi1)) + return left(pi, pj, pin1) && left(pj, pi, pi1); + // Assume (i-1,i,i+1) not collinear. + // else P[i] is reflex. + return !(leftOn(pi, pj, pi1) && leftOn(pj, pi, pin1)); +} + +// Returns T iff (v_i, v_j) is a proper internal +// diagonal of P. +static bool diagonal(int i, int j, int n, const int* verts, int* indices) +{ + return inCone(i, j, n, verts, indices) && diagonalie(i, j, n, verts, indices); +} + +int triangulate(int n, const int* verts, int* indices, int* tris) +{ + int ntris = 0; + int* dst = tris; + + // The last bit of the index is used to indicate if the vertex can be removed. + for (int i = 0; i < n; i++) + { + int i1 = next(i, n); + int i2 = next(i1, n); + if (diagonal(i, i2, n, verts, indices)) + indices[i1] |= 0x80000000; + } + + while (n > 3) + { + int minLen = -1; + int mini = -1; + for (int i = 0; i < n; i++) + { + int i1 = next(i, n); + if (indices[i1] & 0x80000000) + { + const int* p0 = &verts[(indices[i] & 0x0fffffff) * 4]; + const int* p2 = &verts[(indices[next(i1, n)] & 0x0fffffff) * 4]; + + int dx = p2[0] - p0[0]; + int dy = p2[2] - p0[2]; + int len = dx*dx + dy*dy; + + if (minLen < 0 || len < minLen) + { + minLen = len; + mini = i; + } + } + } + + if (mini == -1) + { + // Should not happen. + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "triangulate: Failed to triangulate polygon."); +/* printf("mini == -1 ntris=%d n=%d\n", ntris, n); + for (int i = 0; i < n; i++) + { + printf("%d ", indices[i] & 0x0fffffff); + } + printf("\n");*/ + return -ntris; + } + + int i = mini; + int i1 = next(i, n); + int i2 = next(i1, n); + + *dst++ = indices[i] & 0x0fffffff; + *dst++ = indices[i1] & 0x0fffffff; + *dst++ = indices[i2] & 0x0fffffff; + ntris++; + + // Removes P[i1] by copying P[i+1]...P[n-1] left one index. + n--; + for (int k = i1; k < n; k++) + indices[k] = indices[k+1]; + + if (i1 >= n) i1 = 0; + i = prev(i1,n); + // Update diagonal flags. + if (diagonal(prev(i, n), i1, n, verts, indices)) + indices[i] |= 0x80000000; + else + indices[i] &= 0x0fffffff; + + if (diagonal(i, next(i1, n), n, verts, indices)) + indices[i1] |= 0x80000000; + else + indices[i1] &= 0x0fffffff; + } + + // Append the remaining triangle. + *dst++ = indices[0] & 0x0fffffff; + *dst++ = indices[1] & 0x0fffffff; + *dst++ = indices[2] & 0x0fffffff; + ntris++; + + return ntris; +} + +static int countPolyVerts(const unsigned short* p, const int nvp) +{ + for (int i = 0; i < nvp; ++i) + if (p[i] == 0xffff) + return i; + return nvp; +} + +inline bool uleft(const unsigned short* a, const unsigned short* b, const unsigned short* c) +{ + return ((int)b[0] - (int)a[0]) * ((int)c[2] - (int)a[2]) - + ((int)c[0] - (int)a[0]) * ((int)b[2] - (int)a[2]) < 0; +} + +static int getPolyMergeValue(unsigned short* pa, unsigned short* pb, + const unsigned short* verts, int& ea, int& eb, + const int nvp) +{ + const int na = countPolyVerts(pa, nvp); + const int nb = countPolyVerts(pb, nvp); + + // If the merged polygon would be too big, do not merge. + if (na+nb-2 > nvp) + return -1; + + // Check if the polygons share an edge. + ea = -1; + eb = -1; + + for (int i = 0; i < na; ++i) + { + unsigned short va0 = pa[i]; + unsigned short va1 = pa[(i+1) % na]; + if (va0 > va1) + rcSwap(va0, va1); + for (int j = 0; j < nb; ++j) + { + unsigned short vb0 = pb[j]; + unsigned short vb1 = pb[(j+1) % nb]; + if (vb0 > vb1) + rcSwap(vb0, vb1); + if (va0 == vb0 && va1 == vb1) + { + ea = i; + eb = j; + break; + } + } + } + + // No common edge, cannot merge. + if (ea == -1 || eb == -1) + return -1; + + // Check to see if the merged polygon would be convex. + unsigned short va, vb, vc; + + va = pa[(ea+na-1) % na]; + vb = pa[ea]; + vc = pb[(eb+2) % nb]; + if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3])) + return -1; + + va = pb[(eb+nb-1) % nb]; + vb = pb[eb]; + vc = pa[(ea+2) % na]; + if (!uleft(&verts[va*3], &verts[vb*3], &verts[vc*3])) + return -1; + + va = pa[ea]; + vb = pa[(ea+1)%na]; + + int dx = (int)verts[va*3+0] - (int)verts[vb*3+0]; + int dy = (int)verts[va*3+2] - (int)verts[vb*3+2]; + + return dx*dx + dy*dy; +} + +static void mergePolys(unsigned short* pa, unsigned short* pb, + const unsigned short* verts, int ea, int eb, + unsigned short* tmp, const int nvp) +{ + const int na = countPolyVerts(pa, nvp); + const int nb = countPolyVerts(pb, nvp); + + // Merge polygons. + memset(tmp, 0xff, sizeof(unsigned short)*nvp); + int n = 0; + // Add pa + for (int i = 0; i < na-1; ++i) + tmp[n++] = pa[(ea+1+i) % na]; + // Add pb + for (int i = 0; i < nb-1; ++i) + tmp[n++] = pb[(eb+1+i) % nb]; + + memcpy(pa, tmp, sizeof(unsigned short)*nvp); +} + +static void pushFront(int v, int* arr, int& an) +{ + an++; + for (int i = an-1; i > 0; --i) arr[i] = arr[i-1]; + arr[0] = v; +} + +static void pushBack(int v, int* arr, int& an) +{ + arr[an] = v; + an++; +} + +static bool removeVertex(rcPolyMesh& mesh, const unsigned short rem, const int maxTris) +{ + unsigned short* tmpPoly; + int ntris; + + static const int nvp = mesh.nvp; + + int* edges = 0; + int nedges = 0; + int* hole = 0; + int nhole = 0; + int* hreg = 0; + int nhreg = 0; + int* tris = 0; + int* tverts = 0; + int* thole = 0; + unsigned short* polys = 0; + unsigned short* pregs = 0; + int npolys = 0; + + // Count number of polygons to remove. + int nrem = 0; + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*nvp*2]; + for (int j = 0; j < nvp; ++j) + if (p[j] == rem) { nrem++; break; } + } + + edges = new int[nrem*nvp*3]; + if (!edges) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'edges' (%d).", nrem*nvp*3); + goto failure; + } + + hole = new int[nrem*nvp]; + if (!hole) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hole' (%d).", nrem*nvp); + goto failure; + } + hreg = new int[nrem*nvp]; + if (!hreg) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hreg' (%d).", nrem*nvp); + goto failure; + } + + + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*nvp*2]; + const int nv = countPolyVerts(p, nvp); + bool hasRem = false; + for (int j = 0; j < nv; ++j) + if (p[j] == rem) hasRem = true; + if (hasRem) + { + // Collect edges which does not touch the removed vertex. + for (int j = 0, k = nv-1; j < nv; k = j++) + { + if (p[j] != rem && p[k] != rem) + { + int* e = &edges[nedges*3]; + e[0] = p[k]; + e[1] = p[j]; + e[2] = mesh.regs[i]; + nedges++; + } + } + // Remove the polygon. + unsigned short* p2 = &mesh.polys[(mesh.npolys-1)*nvp*2]; + memcpy(p,p2,sizeof(unsigned short)*nvp); + mesh.regs[i] = mesh.regs[mesh.npolys-1]; + mesh.npolys--; + --i; + } + } + + // Remove vertex. + for (int i = (int)rem; i < mesh.nverts; ++i) + { + mesh.verts[i*3+0] = mesh.verts[(i+1)*3+0]; + mesh.verts[i*3+1] = mesh.verts[(i+1)*3+1]; + mesh.verts[i*3+2] = mesh.verts[(i+1)*3+2]; + } + mesh.nverts--; + + // Adjust indices to match the removed vertex layout. + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*nvp*2]; + const int nv = countPolyVerts(p, nvp); + for (int j = 0; j < nv; ++j) + if (p[j] > rem) p[j]--; + } + for (int i = 0; i < nedges; ++i) + { + if (edges[i*3+0] > rem) edges[i*3+0]--; + if (edges[i*3+1] > rem) edges[i*3+1]--; + } + + if (nedges == 0) + return true; + + hole[nhole] = edges[0]; + hreg[nhole] = edges[2]; + nhole++; + + while (nedges) + { + bool match = false; + + for (int i = 0; i < nedges; ++i) + { + const int ea = edges[i*3+0]; + const int eb = edges[i*3+1]; + const int r = edges[i*3+2]; + bool add = false; + if (hole[0] == eb) + { + pushFront(ea, hole, nhole); + pushFront(r, hreg, nhreg); + add = true; + } + else if (hole[nhole-1] == ea) + { + pushBack(eb, hole, nhole); + pushBack(r, hreg, nhreg); + add = true; + } + if (add) + { + // Remove edge. + edges[i*3+0] = edges[(nedges-1)*3+0]; + edges[i*3+1] = edges[(nedges-1)*3+1]; + edges[i*3+2] = edges[(nedges-1)*3+2]; + --nedges; + match = true; + --i; + } + } + + if (!match) + break; + } + + tris = new int[nhole*3]; + if (!tris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tris' (%d).", nhole*3); + goto failure; + } + + tverts = new int[nhole*4]; + if (!tverts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tverts' (%d).", nhole*4); + goto failure; + } + + thole = new int[nhole]; + if (!tverts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'thole' (%d).", nhole); + goto failure; + } + + // Generate temp vertex array for triangulation. + for (int i = 0; i < nhole; ++i) + { + const int pi = hole[i]; + tverts[i*4+0] = mesh.verts[pi*3+0]; + tverts[i*4+1] = mesh.verts[pi*3+1]; + tverts[i*4+2] = mesh.verts[pi*3+2]; + tverts[i*4+3] = 0; + thole[i] = i; + } + + // Triangulate the hole. + ntris = triangulate(nhole, &tverts[0], &thole[0], tris); + + // Merge the hole triangles back to polygons. + polys = new unsigned short[(ntris+1)*nvp]; + if (!polys) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'polys' (%d).", (ntris+1)*nvp); + goto failure; + } + pregs = new unsigned short[ntris]; + if (!pregs) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_WARNING, "removeVertex: Out of memory 'pregs' (%d).", ntris); + goto failure; + } + + tmpPoly = &polys[ntris*nvp]; + + // Build initial polygons. + memset(polys, 0xff, ntris*nvp*sizeof(unsigned short)); + for (int j = 0; j < ntris; ++j) + { + int* t = &tris[j*3]; + if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2]) + { + polys[npolys*nvp+0] = (unsigned short)hole[t[0]]; + polys[npolys*nvp+1] = (unsigned short)hole[t[1]]; + polys[npolys*nvp+2] = (unsigned short)hole[t[2]]; + pregs[npolys] = hreg[t[0]]; + npolys++; + } + } + if (!npolys) + return true; + + // Merge polygons. + if (nvp > 3) + { + while (true) + { + // Find best polygons to merge. + int bestMergeVal = 0; + int bestPa, bestPb, bestEa, bestEb; + + for (int j = 0; j < npolys-1; ++j) + { + unsigned short* pj = &polys[j*nvp]; + for (int k = j+1; k < npolys; ++k) + { + unsigned short* pk = &polys[k*nvp]; + int ea, eb; + int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb, nvp); + if (v > bestMergeVal) + { + bestMergeVal = v; + bestPa = j; + bestPb = k; + bestEa = ea; + bestEb = eb; + } + } + } + + if (bestMergeVal > 0) + { + // Found best, merge. + unsigned short* pa = &polys[bestPa*nvp]; + unsigned short* pb = &polys[bestPb*nvp]; + mergePolys(pa, pb, mesh.verts, bestEa, bestEb, tmpPoly, nvp); + memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp); + pregs[bestPb] = pregs[npolys-1]; + npolys--; + } + else + { + // Could not merge any polygons, stop. + break; + } + } + } + + // Store polygons. + for (int i = 0; i < npolys; ++i) + { + if (mesh.npolys >= maxTris) break; + unsigned short* p = &mesh.polys[mesh.npolys*nvp*2]; + memset(p,0xff,sizeof(unsigned short)*nvp*2); + for (int j = 0; j < nvp; ++j) + p[j] = polys[i*nvp+j]; + mesh.regs[mesh.npolys] = pregs[i]; + mesh.npolys++; + } + + delete [] edges; + delete [] hole; + delete [] hreg; + delete [] tris; + delete [] thole; + delete [] tverts; + delete [] polys; + delete [] pregs; + + return true; + +failure: + delete [] edges; + delete [] hole; + delete [] hreg; + delete [] tris; + delete [] thole; + delete [] tverts; + delete [] polys; + delete [] pregs; + + return false; +} + + +bool rcBuildPolyMesh(rcContourSet& cset, int nvp, rcPolyMesh& mesh) +{ + unsigned short* tmpPoly; + rcTimeVal startTime = rcGetPerformanceTimer(); + rcTimeVal endTime; + + vcopy(mesh.bmin, cset.bmin); + vcopy(mesh.bmax, cset.bmax); + mesh.cs = cset.cs; + mesh.ch = cset.ch; + + int maxVertices = 0; + int maxTris = 0; + int maxVertsPerCont = 0; + for (int i = 0; i < cset.nconts; ++i) + { + maxVertices += cset.conts[i].nverts; + maxTris += cset.conts[i].nverts - 2; + maxVertsPerCont = rcMax(maxVertsPerCont, cset.conts[i].nverts); + } + + if (maxVertices >= 0xfffe) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Too many vertices %d.", maxVertices); + return false; + } + + unsigned char* vflags = 0; + int* nextVert = 0; + int* firstVert = 0; + int* indices = 0; + int* tris = 0; + unsigned short* polys = 0; + + vflags = new unsigned char[maxVertices]; + if (!vflags) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices); + goto failure; + } + memset(vflags, 0, maxVertices); + + mesh.verts = new unsigned short[maxVertices*3]; + if (!mesh.verts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices); + goto failure; + } + mesh.polys = new unsigned short[maxTris*nvp*2]; + if (!mesh.polys) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.polys' (%d).", maxTris*nvp*2); + goto failure; + } + mesh.regs = new unsigned short[maxTris]; + if (!mesh.regs) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.regs' (%d).", maxTris); + goto failure; + } + mesh.nverts = 0; + mesh.npolys = 0; + mesh.nvp = nvp; + + memset(mesh.verts, 0, sizeof(unsigned short)*maxVertices*3); + memset(mesh.polys, 0xff, sizeof(unsigned short)*maxTris*nvp*2); + memset(mesh.regs, 0, sizeof(unsigned short)*maxTris); + + nextVert = new int[maxVertices]; + if (!nextVert) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'nextVert' (%d).", maxVertices); + goto failure; + } + memset(nextVert, 0, sizeof(int)*maxVertices); + + firstVert = new int[VERTEX_BUCKET_COUNT]; + if (!firstVert) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT); + goto failure; + } + for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i) + firstVert[i] = -1; + + indices = new int[maxVertsPerCont]; + if (!indices) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'indices' (%d).", maxVertsPerCont); + goto failure; + } + tris = new int[maxVertsPerCont*3]; + if (!tris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'tris' (%d).", maxVertsPerCont*3); + goto failure; + } + polys = new unsigned short[(maxVertsPerCont+1)*nvp]; + if (!polys) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'polys' (%d).", maxVertsPerCont*nvp); + goto failure; + } + tmpPoly = &polys[maxVertsPerCont*nvp]; + + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + + // Skip empty contours. + if (cont.nverts < 3) + continue; + + // Triangulate contour + for (int j = 0; j < cont.nverts; ++j) + indices[j] = j; + + int ntris = triangulate(cont.nverts, cont.verts, &indices[0], &tris[0]); + if (ntris <= 0) + { + // Bad triangulation, should not happen. +/* for (int k = 0; k < cont.nverts; ++k) + { + const int* v = &cont.verts[k*4]; + printf("\t\t%d,%d,%d,%d,\n", v[0], v[1], v[2], v[3]); + if (nBadPos < 100) + { + badPos[nBadPos*3+0] = v[0]; + badPos[nBadPos*3+1] = v[1]; + badPos[nBadPos*3+2] = v[2]; + nBadPos++; + } + }*/ + ntris = -ntris; + } + // Add and merge vertices. + for (int j = 0; j < cont.nverts; ++j) + { + const int* v = &cont.verts[j*4]; + indices[j] = addVertex((unsigned short)v[0], (unsigned short)v[1], (unsigned short)v[2], + mesh.verts, firstVert, nextVert, mesh.nverts); + if (v[3] & RC_BORDER_VERTEX) + { + // This vertex should be removed. + vflags[indices[j]] = 1; + } + } + + // Build initial polygons. + int npolys = 0; + memset(polys, 0xff, maxVertsPerCont*nvp*sizeof(unsigned short)); + for (int j = 0; j < ntris; ++j) + { + int* t = &tris[j*3]; + if (t[0] != t[1] && t[0] != t[2] && t[1] != t[2]) + { + polys[npolys*nvp+0] = (unsigned short)indices[t[0]]; + polys[npolys*nvp+1] = (unsigned short)indices[t[1]]; + polys[npolys*nvp+2] = (unsigned short)indices[t[2]]; + npolys++; + } + } + if (!npolys) + continue; + + // Merge polygons. + if (nvp > 3) + { + while (true) + { + // Find best polygons to merge. + int bestMergeVal = 0; + int bestPa, bestPb, bestEa, bestEb; + + for (int j = 0; j < npolys-1; ++j) + { + unsigned short* pj = &polys[j*nvp]; + for (int k = j+1; k < npolys; ++k) + { + unsigned short* pk = &polys[k*nvp]; + int ea, eb; + int v = getPolyMergeValue(pj, pk, mesh.verts, ea, eb, nvp); + if (v > bestMergeVal) + { + bestMergeVal = v; + bestPa = j; + bestPb = k; + bestEa = ea; + bestEb = eb; + } + } + } + + if (bestMergeVal > 0) + { + // Found best, merge. + unsigned short* pa = &polys[bestPa*nvp]; + unsigned short* pb = &polys[bestPb*nvp]; + mergePolys(pa, pb, mesh.verts, bestEa, bestEb, tmpPoly, nvp); + memcpy(pb, &polys[(npolys-1)*nvp], sizeof(unsigned short)*nvp); + npolys--; + } + else + { + // Could not merge any polygons, stop. + break; + } + } + } + + + // Store polygons. + for (int j = 0; j < npolys; ++j) + { + unsigned short* p = &mesh.polys[mesh.npolys*nvp*2]; + unsigned short* q = &polys[j*nvp]; + for (int k = 0; k < nvp; ++k) + p[k] = q[k]; + mesh.regs[mesh.npolys] = cont.reg; + mesh.npolys++; + } + } + + + // Remove edge vertices. + for (int i = 0; i < mesh.nverts; ++i) + { + if (vflags[i]) + { + if (!removeVertex(mesh, i, maxTris)) + goto failure; + for (int j = i; j < mesh.nverts-1; ++j) + vflags[j] = vflags[j+1]; + --i; + } + } + + delete [] vflags; + delete [] firstVert; + delete [] nextVert; + delete [] indices; + delete [] tris; + + // Calculate adjacency. + if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, nvp)) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMesh: Adjacency failed."); + return false; + } + + endTime = rcGetPerformanceTimer(); + +// if (rcGetLog()) +// rcGetLog()->log(RC_LOG_PROGRESS, "Build polymesh: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + if (rcGetBuildTimes()) + rcGetBuildTimes()->buildPolymesh += rcGetDeltaTimeUsec(startTime, endTime); + + return true; + +failure: + delete [] vflags; + delete [] tmpPoly; + delete [] firstVert; + delete [] nextVert; + delete [] indices; + delete [] tris; + + return false; +} + +bool rcMergePolyMeshes(rcPolyMesh** meshes, const int nmeshes, rcPolyMesh& mesh) +{ + if (!nmeshes || !meshes) + return true; + + rcTimeVal startTime = rcGetPerformanceTimer(); + rcTimeVal endTime; + + int* nextVert = 0; + int* firstVert = 0; + unsigned short* vremap = 0; + + mesh.nvp = meshes[0]->nvp; + mesh.cs = meshes[0]->cs; + mesh.ch = meshes[0]->ch; + vcopy(mesh.bmin, meshes[0]->bmin); + vcopy(mesh.bmax, meshes[0]->bmax); + + int maxVerts = 0; + int maxPolys = 0; + int maxVertsPerMesh = 0; + for (int i = 0; i < nmeshes; ++i) + { + vmin(mesh.bmin, meshes[i]->bmin); + vmax(mesh.bmax, meshes[i]->bmax); + maxVertsPerMesh = rcMax(maxVertsPerMesh, meshes[i]->nverts); + maxVerts += meshes[i]->nverts; + maxPolys += meshes[i]->npolys; + } + + mesh.nverts = 0; + mesh.verts = new unsigned short[maxVerts*3]; + if (!mesh.verts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.verts' (%d).", maxVerts*3); + return false; + } + + mesh.npolys = 0; + mesh.polys = new unsigned short[maxPolys*2*mesh.nvp]; + if (!mesh.polys) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.polys' (%d).", maxPolys*2*mesh.nvp); + return false; + } + memset(mesh.polys, 0xff, sizeof(unsigned short)*maxPolys*2*mesh.nvp); + + mesh.regs = new unsigned short[maxPolys]; + if (!mesh.regs) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.regs' (%d).", maxPolys); + return false; + } + memset(mesh.regs, 0, sizeof(unsigned short)*maxPolys); + + nextVert = new int[maxVerts]; + if (!nextVert) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'nextVert' (%d).", maxVerts); + goto failure; + } + memset(nextVert, 0, sizeof(int)*maxVerts); + + firstVert = new int[VERTEX_BUCKET_COUNT]; + if (!firstVert) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT); + goto failure; + } + for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i) + firstVert[i] = -1; + + vremap = new unsigned short[maxVertsPerMesh]; + if (!vremap) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'vremap' (%d).", maxVertsPerMesh); + goto failure; + } + memset(nextVert, 0, sizeof(int)*maxVerts); + + for (int i = 0; i < nmeshes; ++i) + { + const rcPolyMesh* pmesh = meshes[i]; + + const unsigned short ox = (unsigned short)floorf((pmesh->bmin[0]-mesh.bmin[0])/mesh.cs+0.5f); + const unsigned short oz = (unsigned short)floorf((pmesh->bmin[2]-mesh.bmin[2])/mesh.cs+0.5f); + + for (int j = 0; j < pmesh->nverts; ++j) + { + unsigned short* v = &pmesh->verts[j*3]; + vremap[j] = addVertex(v[0]+ox, v[1], v[2]+oz, + mesh.verts, firstVert, nextVert, mesh.nverts); + } + + for (int j = 0; j < pmesh->npolys; ++j) + { + unsigned short* tgt = &mesh.polys[mesh.npolys*2*mesh.nvp]; + unsigned short* src = &pmesh->polys[j*2*mesh.nvp]; + mesh.regs[mesh.npolys] = pmesh->regs[j]; + mesh.npolys++; + for (int k = 0; k < mesh.nvp; ++k) + { + if (src[k] == 0xffff) break; + tgt[k] = vremap[src[k]]; + } + } + } + + // Calculate adjacency. + if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, mesh.nvp)) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcMergePolyMeshes: Adjacency failed."); + return false; + } + + + delete [] firstVert; + delete [] nextVert; + delete [] vremap; + + endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->mergePolyMesh += rcGetDeltaTimeUsec(startTime, endTime); + + return true; + +failure: + delete [] firstVert; + delete [] nextVert; + delete [] vremap; + + return false; +} diff --git a/extern/recastnavigation/Recast/Source/RecastMeshDetail.cpp b/extern/recastnavigation/Recast/Source/RecastMeshDetail.cpp new file mode 100644 index 00000000000..dab94eaa775 --- /dev/null +++ b/extern/recastnavigation/Recast/Source/RecastMeshDetail.cpp @@ -0,0 +1,993 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include <float.h> +#define _USE_MATH_DEFINES +#include <math.h> +#include <string.h> +#include <stdlib.h> +#include <stdio.h> +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +struct rcHeightPatch +{ + inline rcHeightPatch() : data(0) {} + inline ~rcHeightPatch() { delete [] data; } + unsigned short* data; + int xmin, ymin, width, height; +}; + + +static int circumCircle(const float xp, const float yp, + const float x1, const float y1, + const float x2, const float y2, + const float x3, const float y3, + float& xc, float& yc, float& rsqr) +{ + static const float EPSILON = 1e-6f; + + const float fabsy1y2 = rcAbs(y1-y2); + const float fabsy2y3 = rcAbs(y2-y3); + + /* Check for coincident points */ + if (fabsy1y2 < EPSILON && fabsy2y3 < EPSILON) + return 0; + + if (fabsy1y2 < EPSILON) + { + const float m2 = - (x3-x2) / (y3-y2); + const float mx2 = (x2 + x3) / 2.0f; + const float my2 = (y2 + y3) / 2.0f; + xc = (x2 + x1) / 2.0f; + yc = m2 * (xc - mx2) + my2; + } + else if (fabsy2y3 < EPSILON) + { + const float m1 = - (x2-x1) / (y2-y1); + const float mx1 = (x1 + x2) / 2.0f; + const float my1 = (y1 + y2) / 2.0f; + xc = (x3 + x2) / 2.0f; + yc = m1 * (xc - mx1) + my1; + } + else + { + const float m1 = - (x2-x1) / (y2-y1); + const float m2 = - (x3-x2) / (y3-y2); + const float mx1 = (x1 + x2) / 2.0f; + const float mx2 = (x2 + x3) / 2.0f; + const float my1 = (y1 + y2) / 2.0f; + const float my2 = (y2 + y3) / 2.0f; + xc = (m1 * mx1 - m2 * mx2 + my2 - my1) / (m1 - m2); + if (fabsy1y2 > fabsy2y3) + yc = m1 * (xc - mx1) + my1; + else + yc = m2 * (xc - mx2) + my2; + } + + float dx,dy; + + dx = x2 - xc; + dy = y2 - yc; + rsqr = dx*dx + dy*dy; + + dx = xp - xc; + dy = yp - yc; + const float drsqr = dx*dx + dy*dy; + + return (drsqr <= rsqr) ? 1 : 0; +} + +#if defined(_MSC_VER) +static int ptcmp(void* up, const void *v1, const void *v2) +#elif defined(__APPLE__) || defined(__FreeBSD__) +static int ptcmp(void* up, const void *v1, const void *v2) +#else +static int ptcmp(const void *v1, const void *v2, void* up) +#endif +{ + const float* verts = (const float*)up; + const float* p1 = &verts[(*(const int*)v1)*3]; + const float* p2 = &verts[(*(const int*)v2)*3]; + if (p1[0] < p2[0]) + return -1; + else if (p1[0] > p2[0]) + return 1; + else + return 0; +} + +// Based on Paul Bourke's triangulate.c +// http://astronomy.swin.edu.au/~pbourke/terrain/triangulate/triangulate.c +static void delaunay(const int nv, float *verts, rcIntArray& idx, rcIntArray& tris, rcIntArray& edges) +{ + // Sort vertices + idx.resize(nv); + for (int i = 0; i < nv; ++i) + idx[i] = i; +#if defined(_MSC_VER) + qsort_s(&idx[0], idx.size(), sizeof(int), ptcmp, verts); +#elif defined(__APPLE__) || defined(__FreeBSD__) + qsort_r(&idx[0], idx.size(), sizeof(int), verts, ptcmp); +#else + qsort_r(&idx[0], idx.size(), sizeof(int), ptcmp, verts); +#endif + + // Find the maximum and minimum vertex bounds. + // This is to allow calculation of the bounding triangle + float xmin = verts[0]; + float ymin = verts[2]; + float xmax = xmin; + float ymax = ymin; + for (int i = 1; i < nv; ++i) + { + xmin = rcMin(xmin, verts[i*3+0]); + xmax = rcMax(xmax, verts[i*3+0]); + ymin = rcMin(ymin, verts[i*3+2]); + ymax = rcMax(ymax, verts[i*3+2]); + } + float dx = xmax - xmin; + float dy = ymax - ymin; + float dmax = (dx > dy) ? dx : dy; + float xmid = (xmax + xmin) / 2.0f; + float ymid = (ymax + ymin) / 2.0f; + + // Set up the supertriangle + // This is a triangle which encompasses all the sample points. + // The supertriangle coordinates are added to the end of the + // vertex list. The supertriangle is the first triangle in + // the triangle list. + float sv[3*3]; + + sv[0] = xmid - 20 * dmax; + sv[1] = 0; + sv[2] = ymid - dmax; + + sv[3] = xmid; + sv[4] = 0; + sv[5] = ymid + 20 * dmax; + + sv[6] = xmid + 20 * dmax; + sv[7] = 0; + sv[8] = ymid - dmax; + + tris.push(-3); + tris.push(-2); + tris.push(-1); + tris.push(0); // not completed + + for (int i = 0; i < nv; ++i) + { + const float xp = verts[idx[i]*3+0]; + const float yp = verts[idx[i]*3+2]; + + edges.resize(0); + + // Set up the edge buffer. + // If the point (xp,yp) lies inside the circumcircle then the + // three edges of that triangle are added to the edge buffer + // and that triangle is removed. + for (int j = 0; j < tris.size()/4; ++j) + { + int* t = &tris[j*4]; + if (t[3]) // completed? + continue; + const float* v1 = t[0] < 0 ? &sv[(t[0]+3)*3] : &verts[idx[t[0]]*3]; + const float* v2 = t[1] < 0 ? &sv[(t[1]+3)*3] : &verts[idx[t[1]]*3]; + const float* v3 = t[2] < 0 ? &sv[(t[2]+3)*3] : &verts[idx[t[2]]*3]; + float xc,yc,rsqr; + int inside = circumCircle(xp,yp, v1[0],v1[2], v2[0],v2[2], v3[0],v3[2], xc,yc,rsqr); + if (xc < xp && rcSqr(xp-xc) > rsqr) + t[3] = 1; + if (inside) + { + // Collect triangle edges. + edges.push(t[0]); + edges.push(t[1]); + edges.push(t[1]); + edges.push(t[2]); + edges.push(t[2]); + edges.push(t[0]); + // Remove triangle j. + t[0] = tris[tris.size()-4]; + t[1] = tris[tris.size()-3]; + t[2] = tris[tris.size()-2]; + t[3] = tris[tris.size()-1]; + tris.resize(tris.size()-4); + j--; + } + } + + // Remove duplicate edges. + const int ne = edges.size()/2; + for (int j = 0; j < ne-1; ++j) + { + for (int k = j+1; k < ne; ++k) + { + // Dupe?, make null. + if ((edges[j*2+0] == edges[k*2+1]) && (edges[j*2+1] == edges[k*2+0])) + { + edges[j*2+0] = 0; + edges[j*2+1] = 0; + edges[k*2+0] = 0; + edges[k*2+1] = 0; + } + } + } + + // Form new triangles for the current point + // Skipping over any null. + // All edges are arranged in clockwise order. + for (int j = 0; j < ne; ++j) + { + if (edges[j*2+0] == edges[j*2+1]) continue; + tris.push(edges[j*2+0]); + tris.push(edges[j*2+1]); + tris.push(i); + tris.push(0); // not completed + } + } + + // Remove triangles with supertriangle vertices + // These are triangles which have a vertex number greater than nv + for (int i = 0; i < tris.size()/4; ++i) + { + int* t = &tris[i*4]; + if (t[0] < 0 || t[1] < 0 || t[2] < 0) + { + t[0] = tris[tris.size()-4]; + t[1] = tris[tris.size()-3]; + t[2] = tris[tris.size()-2]; + t[3] = tris[tris.size()-1]; + tris.resize(tris.size()-4); + i--; + } + } + // Triangle vertices are pointing to sorted vertices, remap indices. + for (int i = 0; i < tris.size(); ++i) + tris[i] = idx[tris[i]]; +} + +inline float vdot2(const float* a, const float* b) +{ + return a[0]*b[0] + a[2]*b[2]; +} + +static float distPtTri(const float* p, const float* a, const float* b, const float* c) +{ + float v0[3], v1[3], v2[3]; + vsub(v0, c,a); + vsub(v1, b,a); + vsub(v2, p,a); + + const float dot00 = vdot2(v0, v0); + const float dot01 = vdot2(v0, v1); + const float dot02 = vdot2(v0, v2); + const float dot11 = vdot2(v1, v1); + const float dot12 = vdot2(v1, v2); + + // Compute barycentric coordinates + float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + float u = (dot11 * dot02 - dot01 * dot12) * invDenom; + float v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + // If point lies inside the triangle, return interpolated y-coord. + static const float EPS = 1e-4f; + if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS) + { + float y = a[1] + v0[1]*u + v1[1]*v; + return fabsf(y-p[1]); + } + return FLT_MAX; +} + +static float distancePtSeg(const float* pt, const float* p, const float* q) +{ + float pqx = q[0] - p[0]; + float pqy = q[1] - p[1]; + float pqz = q[2] - p[2]; + float dx = pt[0] - p[0]; + float dy = pt[1] - p[1]; + float dz = pt[2] - p[2]; + float d = pqx*pqx + pqy*pqy + pqz*pqz; + float t = pqx*dx + pqy*dy + pqz*dz; + if (d > 0) + t /= d; + if (t < 0) + t = 0; + else if (t > 1) + t = 1; + + dx = p[0] + t*pqx - pt[0]; + dy = p[1] + t*pqy - pt[1]; + dz = p[2] + t*pqz - pt[2]; + + return dx*dx + dy*dy + dz*dz; +} + +static float distancePtSeg2d(const float* pt, const float* p, const float* q) +{ + float pqx = q[0] - p[0]; + float pqz = q[2] - p[2]; + float dx = pt[0] - p[0]; + float dz = pt[2] - p[2]; + float d = pqx*pqx + pqz*pqz; + float t = pqx*dx + pqz*dz; + if (d > 0) + t /= d; + if (t < 0) + t = 0; + else if (t > 1) + t = 1; + + dx = p[0] + t*pqx - pt[0]; + dz = p[2] + t*pqz - pt[2]; + + return dx*dx + dz*dz; +} + +static float distToTriMesh(const float* p, const float* verts, int nverts, const int* tris, int ntris) +{ + float dmin = FLT_MAX; + for (int i = 0; i < ntris; ++i) + { + const float* va = &verts[tris[i*4+0]*3]; + const float* vb = &verts[tris[i*4+1]*3]; + const float* vc = &verts[tris[i*4+2]*3]; + float d = distPtTri(p, va,vb,vc); + if (d < dmin) + dmin = d; + } + if (dmin == FLT_MAX) return -1; + return dmin; +} + +static float distToPoly(int nvert, const float* verts, const float* p) +{ + + float dmin = FLT_MAX; + int i, j, c = 0; + for (i = 0, j = nvert-1; i < nvert; j = i++) + { + const float* vi = &verts[i*3]; + const float* vj = &verts[j*3]; + if (((vi[2] > p[2]) != (vj[2] > p[2])) && + (p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) + c = !c; + dmin = rcMin(dmin, distancePtSeg2d(p, vj, vi)); + } + return c ? -dmin : dmin; +} + + +static unsigned short getHeight(const float* pos, const float* bmin, const float ics, const rcHeightPatch& hp) +{ + int ix = (int)floorf((pos[0]-bmin[0])*ics + 0.01f); + int iz = (int)floorf((pos[2]-bmin[2])*ics + 0.01f); + ix = rcClamp(ix-hp.xmin, 0, hp.width); + iz = rcClamp(iz-hp.ymin, 0, hp.height); + unsigned short h = hp.data[ix+iz*hp.width]; + return h; +} + +static bool buildPolyDetail(const float* in, const int nin, unsigned short reg, + const float sampleDist, const float sampleMaxError, + const rcCompactHeightfield& chf, const rcHeightPatch& hp, + float* verts, int& nverts, rcIntArray& tris, + rcIntArray& edges, rcIntArray& idx, rcIntArray& samples) +{ + static const int MAX_VERTS = 256; + static const int MAX_EDGE = 64; + float edge[(MAX_EDGE+1)*3]; + + nverts = 0; + + for (int i = 0; i < nin; ++i) + vcopy(&verts[i*3], &in[i*3]); + nverts = nin; + + const float ics = 1.0f/chf.cs; + + // Tesselate outlines. + // This is done in separate pass in order to ensure + // seamless height values across the ply boundaries. + if (sampleDist > 0) + { + for (int i = 0, j = nin-1; i < nin; j=i++) + { + const float* vj = &in[j*3]; + const float* vi = &in[i*3]; + // Make sure the segments are always handled in same order + // using lexological sort or else there will be seams. + if (fabsf(vj[0]-vi[0]) < 1e-6f) + { + if (vj[2] > vi[2]) + rcSwap(vj,vi); + } + else + { + if (vj[0] > vi[0]) + rcSwap(vj,vi); + } + // Create samples along the edge. + float dx = vi[0] - vj[0]; + float dy = vi[1] - vj[1]; + float dz = vi[2] - vj[2]; + float d = sqrtf(dx*dx + dz*dz); + int nn = 1 + (int)floorf(d/sampleDist); + if (nn > MAX_EDGE) nn = MAX_EDGE; + if (nverts+nn >= MAX_VERTS) + nn = MAX_VERTS-1-nverts; + for (int k = 0; k <= nn; ++k) + { + float u = (float)k/(float)nn; + float* pos = &edge[k*3]; + pos[0] = vj[0] + dx*u; + pos[1] = vj[1] + dy*u; + pos[2] = vj[2] + dz*u; + pos[1] = chf.bmin[1] + getHeight(pos, chf.bmin, ics, hp)*chf.ch; + } + // Simplify samples. + int idx[MAX_EDGE] = {0,nn}; + int nidx = 2; + for (int k = 0; k < nidx-1; ) + { + const int a = idx[k]; + const int b = idx[k+1]; + const float* va = &edge[a*3]; + const float* vb = &edge[b*3]; + // Find maximum deviation along the segment. + float maxd = 0; + int maxi = -1; + for (int m = a+1; m < b; ++m) + { + float d = distancePtSeg(&edge[m*3],va,vb); + if (d > maxd) + { + maxd = d; + maxi = m; + } + } + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1 && maxd > rcSqr(sampleMaxError)) + { + for (int m = nidx; m > k; --m) + idx[m] = idx[m-1]; + idx[k+1] = maxi; + nidx++; + } + else + { + ++k; + } + } + // Add new vertices. + for (int k = 1; k < nidx-1; ++k) + { + vcopy(&verts[nverts*3], &edge[idx[k]*3]); + nverts++; + } + } + } + + // Tesselate the base mesh. + edges.resize(0); + tris.resize(0); + idx.resize(0); + delaunay(nverts, verts, idx, tris, edges); + + if (sampleDist > 0) + { + // Create sample locations in a grid. + float bmin[3], bmax[3]; + vcopy(bmin, in); + vcopy(bmax, in); + for (int i = 1; i < nin; ++i) + { + vmin(bmin, &in[i*3]); + vmax(bmax, &in[i*3]); + } + int x0 = (int)floorf(bmin[0]/sampleDist); + int x1 = (int)ceilf(bmax[0]/sampleDist); + int z0 = (int)floorf(bmin[2]/sampleDist); + int z1 = (int)ceilf(bmax[2]/sampleDist); + samples.resize(0); + for (int z = z0; z < z1; ++z) + { + for (int x = x0; x < x1; ++x) + { + float pt[3]; + pt[0] = x*sampleDist; + pt[2] = z*sampleDist; + // Make sure the samples are not too close to the edges. + if (distToPoly(nin,in,pt) > -sampleDist/2) continue; + samples.push(x); + samples.push(getHeight(pt, chf.bmin, ics, hp)); + samples.push(z); + } + } + + // Add the samples starting from the one that has the most + // error. The procedure stops when all samples are added + // or when the max error is within treshold. + const int nsamples = samples.size()/3; + for (int iter = 0; iter < nsamples; ++iter) + { + // Find sample with most error. + float bestpt[3]; + float bestd = 0; + for (int i = 0; i < nsamples; ++i) + { + float pt[3]; + pt[0] = samples[i*3+0]*sampleDist; + pt[1] = chf.bmin[1] + samples[i*3+1]*chf.ch; + pt[2] = samples[i*3+2]*sampleDist; + float d = distToTriMesh(pt, verts, nverts, &tris[0], tris.size()/4); + if (d < 0) continue; // did not hit the mesh. + if (d > bestd) + { + bestd = d; + vcopy(bestpt,pt); + } + } + // If the max error is within accepted threshold, stop tesselating. + if (bestd <= sampleMaxError) + break; + + // Add the new sample point. + vcopy(&verts[nverts*3],bestpt); + nverts++; + + // Create new triangulation. + // TODO: Incremental add instead of full rebuild. + edges.resize(0); + tris.resize(0); + idx.resize(0); + delaunay(nverts, verts, idx, tris, edges); + + if (nverts >= MAX_VERTS) + break; + } + } + + return true; +} + +static void getHeightData(const rcCompactHeightfield& chf, + const unsigned short* poly, const int npoly, + const unsigned short* verts, + rcHeightPatch& hp, rcIntArray& stack) +{ + // Floodfill the heightfield to get 2D height data, + // starting at vertex locations as seeds. + + memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height); + + stack.resize(0); + + // Use poly vertices as seed points for the flood fill. + for (int j = 0; j < npoly; ++j) + { + const int ax = (int)verts[poly[j]*3+0]; + const int ay = (int)verts[poly[j]*3+1]; + const int az = (int)verts[poly[j]*3+2]; + if (ax < hp.xmin || ax >= hp.xmin+hp.width || + az < hp.ymin || az >= hp.ymin+hp.height) + continue; + + const rcCompactCell& c = chf.cells[ax+az*chf.width]; + int dmin = 0xffff; + int ai = -1; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + int d = rcAbs(ay - (int)s.y); + if (d < dmin) + { + ai = i; + dmin = d; + } + } + if (ai != -1) + { + stack.push(ax); + stack.push(az); + stack.push(ai); + } + } + + while (stack.size() > 0) + { + int ci = stack.pop(); + int cy = stack.pop(); + int cx = stack.pop(); + + // Skip already visited locations. + int idx = cx-hp.xmin+(cy-hp.ymin)*hp.width; + if (hp.data[idx] != 0xffff) + continue; + + const rcCompactSpan& cs = chf.spans[ci]; + hp.data[idx] = cs.y; + + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(cs, dir) == 0xf) continue; + + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + + if (ax < hp.xmin || ax >= (hp.xmin+hp.width) || + ay < hp.ymin || ay >= (hp.ymin+hp.height)) + continue; + + if (hp.data[ax-hp.xmin+(ay-hp.ymin)*hp.width] != 0xffff) + continue; + + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(cs, dir); + + stack.push(ax); + stack.push(ay); + stack.push(ai); + } + } +} + +static unsigned char getEdgeFlags(const float* va, const float* vb, + const float* vpoly, const int npoly) +{ + // Return true if edge (va,vb) is part of the polygon. + static const float thrSqr = rcSqr(0.001f); + for (int i = 0, j = npoly-1; i < npoly; j=i++) + { + if (distancePtSeg2d(va, &vpoly[j*3], &vpoly[i*3]) < thrSqr && + distancePtSeg2d(vb, &vpoly[j*3], &vpoly[i*3]) < thrSqr) + return 1; + } + return 0; +} + +static unsigned char getTriFlags(const float* va, const float* vb, const float* vc, + const float* vpoly, const int npoly) +{ + unsigned char flags = 0; + flags |= getEdgeFlags(va,vb,vpoly,npoly) << 0; + flags |= getEdgeFlags(vb,vc,vpoly,npoly) << 2; + flags |= getEdgeFlags(vc,va,vpoly,npoly) << 4; + return flags; +} + + + +bool rcBuildPolyMeshDetail(const rcPolyMesh& mesh, const rcCompactHeightfield& chf, + const float sampleDist, const float sampleMaxError, + rcPolyMeshDetail& dmesh) +{ + if (mesh.nverts == 0 || mesh.npolys == 0) + return true; + + rcTimeVal startTime = rcGetPerformanceTimer(); + rcTimeVal endTime; + + int vcap; + int tcap; + + const int nvp = mesh.nvp; + const float cs = mesh.cs; + const float ch = mesh.ch; + const float* orig = mesh.bmin; + + rcIntArray edges(64); + rcIntArray tris(512); + rcIntArray idx(512); + rcIntArray stack(512); + rcIntArray samples(512); + float verts[256*3]; + float* poly = 0; + int* bounds = 0; + rcHeightPatch hp; + int nPolyVerts = 0; + int maxhw = 0, maxhh = 0; + + bounds = new int[mesh.npolys*4]; + if (!bounds) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'bounds' (%d).", mesh.npolys*4); + goto failure; + } + poly = new float[nvp*3]; + if (!bounds) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'poly' (%d).", nvp*3); + goto failure; + } + + // Find max size for a polygon area. + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + int& xmin = bounds[i*4+0]; + int& xmax = bounds[i*4+1]; + int& ymin = bounds[i*4+2]; + int& ymax = bounds[i*4+3]; + xmin = chf.width; + xmax = 0; + ymin = chf.height; + ymax = 0; + for (int j = 0; j < nvp; ++j) + { + if(p[j] == 0xffff) break; + const unsigned short* v = &mesh.verts[p[j]*3]; + xmin = rcMin(xmin, (int)v[0]); + xmax = rcMax(xmax, (int)v[0]); + ymin = rcMin(ymin, (int)v[2]); + ymax = rcMax(ymax, (int)v[2]); + nPolyVerts++; + } + xmin = rcMax(0,xmin-1); + xmax = rcMin(chf.width,xmax+1); + ymin = rcMax(0,ymin-1); + ymax = rcMin(chf.height,ymax+1); + if (xmin >= xmax || ymin >= ymax) continue; + maxhw = rcMax(maxhw, xmax-xmin); + maxhh = rcMax(maxhh, ymax-ymin); + } + + hp.data = new unsigned short[maxhw*maxhh]; + if (!hp.data) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'hp.data' (%d).", maxhw*maxhh); + goto failure; + } + + dmesh.nmeshes = mesh.npolys; + dmesh.nverts = 0; + dmesh.ntris = 0; + dmesh.meshes = new unsigned short[dmesh.nmeshes*4]; + if (!dmesh.meshes) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.meshes' (%d).", dmesh.nmeshes*4); + goto failure; + } + + vcap = nPolyVerts+nPolyVerts/2; + tcap = vcap*2; + + dmesh.nverts = 0; + dmesh.verts = new float[vcap*3]; + if (!dmesh.verts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", vcap*3); + goto failure; + } + dmesh.ntris = 0; + dmesh.tris = new unsigned char[tcap*4]; + if (!dmesh.tris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", tcap*4); + goto failure; + } + + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + + // Find polygon bounding box. + int npoly = 0; + for (int j = 0; j < nvp; ++j) + { + if(p[j] == 0xffff) break; + const unsigned short* v = &mesh.verts[p[j]*3]; + poly[j*3+0] = orig[0] + v[0]*cs; + poly[j*3+1] = orig[1] + v[1]*ch; + poly[j*3+2] = orig[2] + v[2]*cs; + npoly++; + } + + // Get the height data from the area of the polygon. + hp.xmin = bounds[i*4+0]; + hp.ymin = bounds[i*4+2]; + hp.width = bounds[i*4+1]-bounds[i*4+0]; + hp.height = bounds[i*4+3]-bounds[i*4+2]; + getHeightData(chf, p, npoly, mesh.verts, hp, stack); + + // Build detail mesh. + int nverts = 0; + if (!buildPolyDetail(poly, npoly, mesh.regs[i], + sampleDist, sampleMaxError, + chf, hp, verts, nverts, tris, + edges, idx, samples)) + { + goto failure; + } + + // Offset detail vertices, unnecassary? + for (int j = 0; j < nverts; ++j) + verts[j*3+1] += chf.ch; + + // Store detail submesh. + const int ntris = tris.size()/4; + + dmesh.meshes[i*4+0] = dmesh.nverts; + dmesh.meshes[i*4+1] = (unsigned short)nverts; + dmesh.meshes[i*4+2] = dmesh.ntris; + dmesh.meshes[i*4+3] = (unsigned short)ntris; + + // Store vertices, allocate more memory if necessary. + if (dmesh.nverts+nverts > vcap) + { + while (dmesh.nverts+nverts > vcap) + vcap += 256; + + float* newv = new float[vcap*3]; + if (!newv) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newv' (%d).", vcap*3); + goto failure; + } + if (dmesh.nverts) + memcpy(newv, dmesh.verts, sizeof(float)*3*dmesh.nverts); + delete [] dmesh.verts; + dmesh.verts = newv; + } + for (int j = 0; j < nverts; ++j) + { + dmesh.verts[dmesh.nverts*3+0] = verts[j*3+0]; + dmesh.verts[dmesh.nverts*3+1] = verts[j*3+1]; + dmesh.verts[dmesh.nverts*3+2] = verts[j*3+2]; + dmesh.nverts++; + } + + // Store triangles, allocate more memory if necessary. + if (dmesh.ntris+ntris > tcap) + { + while (dmesh.ntris+ntris > tcap) + tcap += 256; + unsigned char* newt = new unsigned char[tcap*4]; + if (!newt) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newt' (%d).", tcap*4); + goto failure; + } + if (dmesh.ntris) + memcpy(newt, dmesh.tris, sizeof(unsigned char)*4*dmesh.ntris); + delete [] dmesh.tris; + dmesh.tris = newt; + } + for (int j = 0; j < ntris; ++j) + { + const int* t = &tris[j*4]; + dmesh.tris[dmesh.ntris*4+0] = (unsigned char)t[0]; + dmesh.tris[dmesh.ntris*4+1] = (unsigned char)t[1]; + dmesh.tris[dmesh.ntris*4+2] = (unsigned char)t[2]; + dmesh.tris[dmesh.ntris*4+3] = getTriFlags(&verts[t[0]*3], &verts[t[1]*3], &verts[t[2]*3], poly, npoly); + dmesh.ntris++; + } + } + + delete [] bounds; + delete [] poly; + + endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->buildDetailMesh += rcGetDeltaTimeUsec(startTime, endTime); + + return true; + +failure: + + delete [] bounds; + delete [] poly; + + return false; +} + +bool rcMergePolyMeshDetails(rcPolyMeshDetail** meshes, const int nmeshes, rcPolyMeshDetail& mesh) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + int maxVerts = 0; + int maxTris = 0; + int maxMeshes = 0; + + for (int i = 0; i < nmeshes; ++i) + { + if (!meshes[i]) continue; + maxVerts += meshes[i]->nverts; + maxTris += meshes[i]->ntris; + maxMeshes += meshes[i]->nmeshes; + } + + mesh.nmeshes = 0; + mesh.meshes = new unsigned short[maxMeshes*4]; + if (!mesh.meshes) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'pmdtl.meshes' (%d).", maxMeshes*4); + return false; + } + + mesh.ntris = 0; + mesh.tris = new unsigned char[maxTris*4]; + if (!mesh.tris) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", maxTris*4); + return false; + } + + mesh.nverts = 0; + mesh.verts = new float[maxVerts*3]; + if (!mesh.verts) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", maxVerts*3); + return false; + } + + // Merge datas. + for (int i = 0; i < nmeshes; ++i) + { + rcPolyMeshDetail* dm = meshes[i]; + if (!dm) continue; + for (int j = 0; j < dm->nmeshes; ++j) + { + unsigned short* dst = &mesh.meshes[mesh.nmeshes*4]; + unsigned short* src = &dm->meshes[j*4]; + dst[0] = mesh.nverts+src[0]; + dst[1] = src[1]; + dst[2] = mesh.ntris+src[2]; + dst[3] = src[3]; + mesh.nmeshes++; + } + + for (int k = 0; k < dm->nverts; ++k) + { + vcopy(&mesh.verts[mesh.nverts*3], &dm->verts[k*3]); + mesh.nverts++; + } + for (int k = 0; k < dm->ntris; ++k) + { + mesh.tris[mesh.ntris*4+0] = dm->tris[k*4+0]; + mesh.tris[mesh.ntris*4+1] = dm->tris[k*4+1]; + mesh.tris[mesh.ntris*4+2] = dm->tris[k*4+2]; + mesh.tris[mesh.ntris*4+3] = dm->tris[k*4+3]; + mesh.ntris++; + } + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->mergePolyMeshDetail += rcGetDeltaTimeUsec(startTime, endTime); + + return true; +} + diff --git a/extern/recastnavigation/Recast/Source/RecastRasterization.cpp b/extern/recastnavigation/Recast/Source/RecastRasterization.cpp new file mode 100644 index 00000000000..658b0e1fb51 --- /dev/null +++ b/extern/recastnavigation/Recast/Source/RecastRasterization.cpp @@ -0,0 +1,308 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#define _USE_MATH_DEFINES +#include <math.h> +#include <stdio.h> +#include "Recast.h" +#include "RecastTimer.h" +#include "RecastLog.h" + +inline bool overlapBounds(const float* amin, const float* amax, const float* bmin, const float* bmax) +{ + bool overlap = true; + overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap; + overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap; + overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; + return overlap; +} + +inline bool overlapInterval(unsigned short amin, unsigned short amax, + unsigned short bmin, unsigned short bmax) +{ + if (amax < bmin) return false; + if (amin > bmax) return false; + return true; +} + + +static rcSpan* allocSpan(rcHeightfield& hf) +{ + // If running out of memory, allocate new page and update the freelist. + if (!hf.freelist || !hf.freelist->next) + { + // Create new page. + // Allocate memory for the new pool. + const int size = (sizeof(rcSpanPool)-sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL; + rcSpanPool* pool = reinterpret_cast<rcSpanPool*>(new unsigned char[size]); + if (!pool) return 0; + pool->next = 0; + // Add the pool into the list of pools. + pool->next = hf.pools; + hf.pools = pool; + // Add new items to the free list. + rcSpan* freelist = hf.freelist; + rcSpan* head = &pool->items[0]; + rcSpan* it = &pool->items[RC_SPANS_PER_POOL]; + do + { + --it; + it->next = freelist; + freelist = it; + } + while (it != head); + hf.freelist = it; + } + + // Pop item from in front of the free list. + rcSpan* it = hf.freelist; + hf.freelist = hf.freelist->next; + return it; +} + +static void freeSpan(rcHeightfield& hf, rcSpan* ptr) +{ + if (!ptr) return; + // Add the node in front of the free list. + ptr->next = hf.freelist; + hf.freelist = ptr; +} + +static void addSpan(rcHeightfield& hf, int x, int y, + unsigned short smin, unsigned short smax, + unsigned short flags) +{ + int idx = x + y*hf.width; + + rcSpan* s = allocSpan(hf); + s->smin = smin; + s->smax = smax; + s->flags = flags; + s->next = 0; + + // Empty cell, add he first span. + if (!hf.spans[idx]) + { + hf.spans[idx] = s; + return; + } + rcSpan* prev = 0; + rcSpan* cur = hf.spans[idx]; + + // Insert and merge spans. + while (cur) + { + if (cur->smin > s->smax) + { + // Current span is further than the new span, break. + break; + } + else if (cur->smax < s->smin) + { + // Current span is before the new span advance. + prev = cur; + cur = cur->next; + } + else + { + // Merge spans. + if (cur->smin < s->smin) + s->smin = cur->smin; + if (cur->smax > s->smax) + s->smax = cur->smax; + + // Merge flags. +// if (s->smax == cur->smax) + if (rcAbs((int)s->smax - (int)cur->smax) <= 1) + s->flags |= cur->flags; + + // Remove current span. + rcSpan* next = cur->next; + freeSpan(hf, cur); + if (prev) + prev->next = next; + else + hf.spans[idx] = next; + cur = next; + } + } + + // Insert new span. + if (prev) + { + s->next = prev->next; + prev->next = s; + } + else + { + s->next = hf.spans[idx]; + hf.spans[idx] = s; + } +} + +static int clipPoly(const float* in, int n, float* out, float pnx, float pnz, float pd) +{ + float d[12]; + for (int i = 0; i < n; ++i) + d[i] = pnx*in[i*3+0] + pnz*in[i*3+2] + pd; + + int m = 0; + for (int i = 0, j = n-1; i < n; j=i, ++i) + { + bool ina = d[j] >= 0; + bool inb = d[i] >= 0; + if (ina != inb) + { + float s = d[j] / (d[j] - d[i]); + out[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s; + out[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s; + out[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s; + m++; + } + if (inb) + { + out[m*3+0] = in[i*3+0]; + out[m*3+1] = in[i*3+1]; + out[m*3+2] = in[i*3+2]; + m++; + } + } + return m; +} + +static void rasterizeTri(const float* v0, const float* v1, const float* v2, + unsigned char flags, rcHeightfield& hf, + const float* bmin, const float* bmax, + const float cs, const float ics, const float ich) +{ + const int w = hf.width; + const int h = hf.height; + float tmin[3], tmax[3]; + const float by = bmax[1] - bmin[1]; + + // Calculate the bounding box of the triangle. + vcopy(tmin, v0); + vcopy(tmax, v0); + vmin(tmin, v1); + vmin(tmin, v2); + vmax(tmax, v1); + vmax(tmax, v2); + + // If the triangle does not touch the bbox of the heightfield, skip the triagle. + if (!overlapBounds(bmin, bmax, tmin, tmax)) + return; + + // Calculate the footpring of the triangle on the grid. + int x0 = (int)((tmin[0] - bmin[0])*ics); + int y0 = (int)((tmin[2] - bmin[2])*ics); + int x1 = (int)((tmax[0] - bmin[0])*ics); + int y1 = (int)((tmax[2] - bmin[2])*ics); + x0 = rcClamp(x0, 0, w-1); + y0 = rcClamp(y0, 0, h-1); + x1 = rcClamp(x1, 0, w-1); + y1 = rcClamp(y1, 0, h-1); + + // Clip the triangle into all grid cells it touches. + float in[7*3], out[7*3], inrow[7*3]; + + for (int y = y0; y <= y1; ++y) + { + // Clip polygon to row. + vcopy(&in[0], v0); + vcopy(&in[1*3], v1); + vcopy(&in[2*3], v2); + int nvrow = 3; + const float cz = bmin[2] + y*cs; + nvrow = clipPoly(in, nvrow, out, 0, 1, -cz); + if (nvrow < 3) continue; + nvrow = clipPoly(out, nvrow, inrow, 0, -1, cz+cs); + if (nvrow < 3) continue; + + for (int x = x0; x <= x1; ++x) + { + // Clip polygon to column. + int nv = nvrow; + const float cx = bmin[0] + x*cs; + nv = clipPoly(inrow, nv, out, 1, 0, -cx); + if (nv < 3) continue; + nv = clipPoly(out, nv, in, -1, 0, cx+cs); + if (nv < 3) continue; + + // Calculate min and max of the span. + float smin = in[1], smax = in[1]; + for (int i = 1; i < nv; ++i) + { + smin = rcMin(smin, in[i*3+1]); + smax = rcMax(smax, in[i*3+1]); + } + smin -= bmin[1]; + smax -= bmin[1]; + // Skip the span if it is outside the heightfield bbox + if (smax < 0.0f) continue; + if (smin > by) continue; + // Clamp the span to the heightfield bbox. + if (smin < 0.0f) smin = bmin[1]; + if (smax > by) smax = bmax[1]; + + // Snap the span to the heightfield height grid. + unsigned short ismin = (unsigned short)rcClamp((int)floorf(smin * ich), 0, 0x7fff); + unsigned short ismax = (unsigned short)rcClamp((int)ceilf(smax * ich), 0, 0x7fff); + + addSpan(hf, x, y, ismin, ismax, flags); + } + } +} + +void rcRasterizeTriangle(const float* v0, const float* v1, const float* v2, + unsigned char flags, rcHeightfield& solid) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const float ics = 1.0f/solid.cs; + const float ich = 1.0f/solid.ch; + rasterizeTri(v0, v1, v2, flags, solid, solid.bmin, solid.bmax, solid.cs, ics, ich); + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->rasterizeTriangles += rcGetDeltaTimeUsec(startTime, endTime); +} + +void rcRasterizeTriangles(const float* verts, int nv, + const int* tris, const unsigned char* flags, int nt, + rcHeightfield& solid) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const float ics = 1.0f/solid.cs; + const float ich = 1.0f/solid.ch; + // Rasterize triangles. + for (int i = 0; i < nt; ++i) + { + const float* v0 = &verts[tris[i*3+0]*3]; + const float* v1 = &verts[tris[i*3+1]*3]; + const float* v2 = &verts[tris[i*3+2]*3]; + // Rasterize. + rasterizeTri(v0, v1, v2, flags[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich); + } + + rcTimeVal endTime = rcGetPerformanceTimer(); + + if (rcGetBuildTimes()) + rcGetBuildTimes()->rasterizeTriangles += rcGetDeltaTimeUsec(startTime, endTime); +} diff --git a/extern/recastnavigation/Recast/Source/RecastRegion.cpp b/extern/recastnavigation/Recast/Source/RecastRegion.cpp new file mode 100644 index 00000000000..5c557cf0681 --- /dev/null +++ b/extern/recastnavigation/Recast/Source/RecastRegion.cpp @@ -0,0 +1,1081 @@ +// +// Copyright (c) 2009 Mikko Mononen memon@inside.org +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would be +// appreciated but is not required. +// 2. Altered source versions must be plainly marked as such, and must not be +// misrepresented as being the original software. +// 3. This notice may not be removed or altered from any source distribution. +// + +#include <float.h> +#define _USE_MATH_DEFINES +#include <math.h> +#include <string.h> +#include <stdlib.h> +#include <stdio.h> +#include "Recast.h" +#include "RecastLog.h" +#include "RecastTimer.h" + + +static unsigned short* calculateDistanceField(rcCompactHeightfield& chf, + unsigned short* src, unsigned short* dst, + unsigned short& maxDist) +{ + const int w = chf.width; + const int h = chf.height; + + // Init distance and points. + for (int i = 0; i < chf.spanCount; ++i) + src[i] = 0xffff; + + // Mark boundary cells. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + int nc = 0; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != 0xf) + nc++; + } + if (nc != 4) + src[i] = 0; + } + } + } + + // Pass 1 + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + + if (rcGetCon(s, 0) != 0xf) + { + // (-1,0) + const int ax = x + rcGetDirOffsetX(0); + const int ay = y + rcGetDirOffsetY(0); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0); + const rcCompactSpan& as = chf.spans[ai]; + if (src[ai]+2 < src[i]) + src[i] = src[ai]+2; + + // (-1,-1) + if (rcGetCon(as, 3) != 0xf) + { + const int aax = ax + rcGetDirOffsetX(3); + const int aay = ay + rcGetDirOffsetY(3); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3); + if (src[aai]+3 < src[i]) + src[i] = src[aai]+3; + } + } + if (rcGetCon(s, 3) != 0xf) + { + // (0,-1) + const int ax = x + rcGetDirOffsetX(3); + const int ay = y + rcGetDirOffsetY(3); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3); + const rcCompactSpan& as = chf.spans[ai]; + if (src[ai]+2 < src[i]) + src[i] = src[ai]+2; + + // (1,-1) + if (rcGetCon(as, 2) != 0xf) + { + const int aax = ax + rcGetDirOffsetX(2); + const int aay = ay + rcGetDirOffsetY(2); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2); + if (src[aai]+3 < src[i]) + src[i] = src[aai]+3; + } + } + } + } + } + + // Pass 2 + for (int y = h-1; y >= 0; --y) + { + for (int x = w-1; x >= 0; --x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + + if (rcGetCon(s, 2) != 0xf) + { + // (1,0) + const int ax = x + rcGetDirOffsetX(2); + const int ay = y + rcGetDirOffsetY(2); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2); + const rcCompactSpan& as = chf.spans[ai]; + if (src[ai]+2 < src[i]) + src[i] = src[ai]+2; + + // (1,1) + if (rcGetCon(as, 1) != 0xf) + { + const int aax = ax + rcGetDirOffsetX(1); + const int aay = ay + rcGetDirOffsetY(1); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1); + if (src[aai]+3 < src[i]) + src[i] = src[aai]+3; + } + } + if (rcGetCon(s, 1) != 0xf) + { + // (0,1) + const int ax = x + rcGetDirOffsetX(1); + const int ay = y + rcGetDirOffsetY(1); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1); + const rcCompactSpan& as = chf.spans[ai]; + if (src[ai]+2 < src[i]) + src[i] = src[ai]+2; + + // (-1,1) + if (rcGetCon(as, 0) != 0xf) + { + const int aax = ax + rcGetDirOffsetX(0); + const int aay = ay + rcGetDirOffsetY(0); + const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0); + if (src[aai]+3 < src[i]) + src[i] = src[aai]+3; + } + } + } + } + } + + maxDist = 0; + for (int i = 0; i < chf.spanCount; ++i) + maxDist = rcMax(src[i], maxDist); + + return src; + +} + +static unsigned short* boxBlur(rcCompactHeightfield& chf, int thr, + unsigned short* src, unsigned short* dst) +{ + const int w = chf.width; + const int h = chf.height; + + thr *= 2; + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + int cd = (int)src[i]; + if (cd <= thr) + { + dst[i] = cd; + continue; + } + + int d = cd; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != 0xf) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); + d += (int)src[ai]; + + const rcCompactSpan& as = chf.spans[ai]; + const int dir2 = (dir+1) & 0x3; + if (rcGetCon(as, dir2) != 0xf) + { + const int ax2 = ax + rcGetDirOffsetX(dir2); + const int ay2 = ay + rcGetDirOffsetY(dir2); + const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2); + d += (int)src[ai2]; + } + else + { + d += cd; + } + } + else + { + d += cd*2; + } + } + dst[i] = (unsigned short)((d+5)/9); + } + } + } + return dst; +} + + +static bool floodRegion(int x, int y, int i, + unsigned short level, unsigned short minLevel, unsigned short r, + rcCompactHeightfield& chf, + unsigned short* src, + rcIntArray& stack) +{ + const int w = chf.width; + + // Flood fill mark region. + stack.resize(0); + stack.push((int)x); + stack.push((int)y); + stack.push((int)i); + src[i*2] = r; + src[i*2+1] = 0; + + unsigned short lev = level >= minLevel+2 ? level-2 : minLevel; + int count = 0; + + while (stack.size() > 0) + { + int ci = stack.pop(); + int cy = stack.pop(); + int cx = stack.pop(); + + const rcCompactSpan& cs = chf.spans[ci]; + + // Check if any of the neighbours already have a valid region set. + unsigned short ar = 0; + for (int dir = 0; dir < 4; ++dir) + { + // 8 connected + if (rcGetCon(cs, dir) != 0xf) + { + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(cs, dir); + unsigned short nr = src[ai*2]; + if (nr != 0 && nr != r) + ar = nr; + + const rcCompactSpan& as = chf.spans[ai]; + + const int dir2 = (dir+1) & 0x3; + if (rcGetCon(as, dir2) != 0xf) + { + const int ax2 = ax + rcGetDirOffsetX(dir2); + const int ay2 = ay + rcGetDirOffsetY(dir2); + const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2); + + unsigned short nr = src[ai2*2]; + if (nr != 0 && nr != r) + ar = nr; + } + } + } + if (ar != 0) + { + src[ci*2] = 0; + continue; + } + count++; + + // Expand neighbours. + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(cs, dir) != 0xf) + { + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(cs, dir); + if (chf.spans[ai].dist >= lev) + { + if (src[ai*2] == 0) + { + src[ai*2] = r; + src[ai*2+1] = 0; + stack.push(ax); + stack.push(ay); + stack.push(ai); + } + } + } + } + } + + return count > 0; +} + +static unsigned short* expandRegions(int maxIter, unsigned short level, + rcCompactHeightfield& chf, + unsigned short* src, + unsigned short* dst, + rcIntArray& stack) +{ + const int w = chf.width; + const int h = chf.height; + + // Find cells revealed by the raised level. + stack.resize(0); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (chf.spans[i].dist >= level && src[i*2] == 0) + { + stack.push(x); + stack.push(y); + stack.push(i); + } + } + } + } + + int iter = 0; + while (stack.size() > 0) + { + int failed = 0; + + memcpy(dst, src, sizeof(unsigned short)*chf.spanCount*2); + + for (int j = 0; j < stack.size(); j += 3) + { + int x = stack[j+0]; + int y = stack[j+1]; + int i = stack[j+2]; + if (i < 0) + { + failed++; + continue; + } + + unsigned short r = src[i*2]; + unsigned short d2 = 0xffff; + const rcCompactSpan& s = chf.spans[i]; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) == 0xf) continue; + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); + if (src[ai*2] > 0 && (src[ai*2] & RC_BORDER_REG) == 0) + { + if ((int)src[ai*2+1]+2 < (int)d2) + { + r = src[ai*2]; + d2 = src[ai*2+1]+2; + } + } + } + if (r) + { + stack[j+2] = -1; // mark as used + dst[i*2] = r; + dst[i*2+1] = d2; + } + else + { + failed++; + } + } + + // rcSwap source and dest. + rcSwap(src, dst); + + if (failed*3 == stack.size()) + break; + + if (level > 0) + { + ++iter; + if (iter >= maxIter) + break; + } + } + + return src; +} + + +struct rcRegion +{ + inline rcRegion() : count(0), id(0), remap(false) {} + + int count; + unsigned short id; + bool remap; + rcIntArray connections; + rcIntArray floors; +}; + +static void removeAdjacentNeighbours(rcRegion& reg) +{ + // Remove adjacent duplicates. + for (int i = 0; i < reg.connections.size() && reg.connections.size() > 1; ) + { + int ni = (i+1) % reg.connections.size(); + if (reg.connections[i] == reg.connections[ni]) + { + // Remove duplicate + for (int j = i; j < reg.connections.size()-1; ++j) + reg.connections[j] = reg.connections[j+1]; + reg.connections.pop(); + } + else + ++i; + } +} + +static void replaceNeighbour(rcRegion& reg, unsigned short oldId, unsigned short newId) +{ + bool neiChanged = false; + for (int i = 0; i < reg.connections.size(); ++i) + { + if (reg.connections[i] == oldId) + { + reg.connections[i] = newId; + neiChanged = true; + } + } + for (int i = 0; i < reg.floors.size(); ++i) + { + if (reg.floors[i] == oldId) + reg.floors[i] = newId; + } + if (neiChanged) + removeAdjacentNeighbours(reg); +} + +static bool canMergeWithRegion(rcRegion& reg, unsigned short id) +{ + int n = 0; + for (int i = 0; i < reg.connections.size(); ++i) + { + if (reg.connections[i] == id) + n++; + } + if (n > 1) + return false; + for (int i = 0; i < reg.floors.size(); ++i) + { + if (reg.floors[i] == id) + return false; + } + return true; +} + +static void addUniqueFloorRegion(rcRegion& reg, unsigned short n) +{ + for (int i = 0; i < reg.floors.size(); ++i) + if (reg.floors[i] == n) + return; + reg.floors.push(n); +} + +static bool mergeRegions(rcRegion& rega, rcRegion& regb) +{ + unsigned short aid = rega.id; + unsigned short bid = regb.id; + + // Duplicate current neighbourhood. + rcIntArray acon; + acon.resize(rega.connections.size()); + for (int i = 0; i < rega.connections.size(); ++i) + acon[i] = rega.connections[i]; + rcIntArray& bcon = regb.connections; + + // Find insertion point on A. + int insa = -1; + for (int i = 0; i < acon.size(); ++i) + { + if (acon[i] == bid) + { + insa = i; + break; + } + } + if (insa == -1) + return false; + + // Find insertion point on B. + int insb = -1; + for (int i = 0; i < bcon.size(); ++i) + { + if (bcon[i] == aid) + { + insb = i; + break; + } + } + if (insb == -1) + return false; + + // Merge neighbours. + rega.connections.resize(0); + for (int i = 0, ni = acon.size(); i < ni-1; ++i) + rega.connections.push(acon[(insa+1+i) % ni]); + + for (int i = 0, ni = bcon.size(); i < ni-1; ++i) + rega.connections.push(bcon[(insb+1+i) % ni]); + + removeAdjacentNeighbours(rega); + + for (int j = 0; j < regb.floors.size(); ++j) + addUniqueFloorRegion(rega, regb.floors[j]); + rega.count += regb.count; + regb.count = 0; + regb.connections.resize(0); + + return true; +} + +static bool isRegionConnectedToBorder(const rcRegion& reg) +{ + // Region is connected to border if + // one of the neighbours is null id. + for (int i = 0; i < reg.connections.size(); ++i) + { + if (reg.connections[i] == 0) + return true; + } + return false; +} + +static bool isSolidEdge(rcCompactHeightfield& chf, unsigned short* src, + int x, int y, int i, int dir) +{ + const rcCompactSpan& s = chf.spans[i]; + unsigned short r = 0; + if (rcGetCon(s, dir) != 0xf) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir); + r = src[ai*2]; + } + if (r == src[i*2]) + return false; + return true; +} + +static void walkContour(int x, int y, int i, int dir, + rcCompactHeightfield& chf, + unsigned short* src, + rcIntArray& cont) +{ + int startDir = dir; + int starti = i; + + const rcCompactSpan& ss = chf.spans[i]; + unsigned short curReg = 0; + if (rcGetCon(ss, dir) != 0xf) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(ss, dir); + curReg = src[ai*2]; + } + cont.push(curReg); + + int iter = 0; + while (++iter < 40000) + { + const rcCompactSpan& s = chf.spans[i]; + + if (isSolidEdge(chf, src, x, y, i, dir)) + { + // Choose the edge corner + unsigned short r = 0; + if (rcGetCon(s, dir) != 0xf) + { + const int ax = x + rcGetDirOffsetX(dir); + const int ay = y + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*chf.width].index + rcGetCon(s, dir); + r = src[ai*2]; + } + if (r != curReg) + { + curReg = r; + cont.push(curReg); + } + + dir = (dir+1) & 0x3; // Rotate CW + } + else + { + int ni = -1; + const int nx = x + rcGetDirOffsetX(dir); + const int ny = y + rcGetDirOffsetY(dir); + if (rcGetCon(s, dir) != 0xf) + { + const rcCompactCell& nc = chf.cells[nx+ny*chf.width]; + ni = (int)nc.index + rcGetCon(s, dir); + } + if (ni == -1) + { + // Should not happen. + return; + } + x = nx; + y = ny; + i = ni; + dir = (dir+3) & 0x3; // Rotate CCW + } + + if (starti == i && startDir == dir) + { + break; + } + } + + // Remove adjacent duplicates. + if (cont.size() > 1) + { + for (int i = 0; i < cont.size(); ) + { + int ni = (i+1) % cont.size(); + if (cont[i] == cont[ni]) + { + for (int j = i; j < cont.size()-1; ++j) + cont[j] = cont[j+1]; + cont.pop(); + } + else + ++i; + } + } +} + +static bool filterSmallRegions(int minRegionSize, int mergeRegionSize, + unsigned short& maxRegionId, + rcCompactHeightfield& chf, + unsigned short* src) +{ + const int w = chf.width; + const int h = chf.height; + + int nreg = maxRegionId+1; + rcRegion* regions = new rcRegion[nreg]; + if (!regions) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "filterSmallRegions: Out of memory 'regions' (%d).", nreg); + return false; + } + + for (int i = 0; i < nreg; ++i) + regions[i].id = (unsigned short)i; + + // Find edge of a region and find connections around the contour. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + unsigned short r = src[i*2]; + if (r == 0 || r >= nreg) + continue; + + rcRegion& reg = regions[r]; + reg.count++; + + + // Update floors. + for (int j = (int)c.index; j < ni; ++j) + { + if (i == j) continue; + unsigned short floorId = src[j*2]; + if (floorId == 0 || floorId >= nreg) + continue; + addUniqueFloorRegion(reg, floorId); + } + + // Have found contour + if (reg.connections.size() > 0) + continue; + + // Check if this cell is next to a border. + int ndir = -1; + for (int dir = 0; dir < 4; ++dir) + { + if (isSolidEdge(chf, src, x, y, i, dir)) + { + ndir = dir; + break; + } + } + + if (ndir != -1) + { + // The cell is at border. + // Walk around the contour to find all the neighbours. + walkContour(x, y, i, ndir, chf, src, reg.connections); + } + } + } + } + + // Remove too small unconnected regions. + for (int i = 0; i < nreg; ++i) + { + rcRegion& reg = regions[i]; + if (reg.id == 0 || (reg.id & RC_BORDER_REG)) + continue; + if (reg.count == 0) + continue; + + if (reg.connections.size() == 1 && reg.connections[0] == 0) + { + if (reg.count < minRegionSize) + { + // Non-connected small region, remove. + reg.count = 0; + reg.id = 0; + } + } + } + + + // Merge too small regions to neighbour regions. + int mergeCount = 0 ; + do + { + mergeCount = 0; + for (int i = 0; i < nreg; ++i) + { + rcRegion& reg = regions[i]; + if (reg.id == 0 || (reg.id & RC_BORDER_REG)) + continue; + if (reg.count == 0) + continue; + + // Check to see if the region should be merged. + if (reg.count > mergeRegionSize && isRegionConnectedToBorder(reg)) + continue; + + // Small region with more than 1 connection. + // Or region which is not connected to a border at all. + // Find smallest neighbour region that connects to this one. + int smallest = 0xfffffff; + unsigned short mergeId = reg.id; + for (int j = 0; j < reg.connections.size(); ++j) + { + if (reg.connections[j] & RC_BORDER_REG) continue; + rcRegion& mreg = regions[reg.connections[j]]; + if (mreg.id == 0 || (mreg.id & RC_BORDER_REG)) continue; + if (mreg.count < smallest && + canMergeWithRegion(reg, mreg.id) && + canMergeWithRegion(mreg, reg.id)) + { + smallest = mreg.count; + mergeId = mreg.id; + } + } + // Found new id. + if (mergeId != reg.id) + { + unsigned short oldId = reg.id; + rcRegion& target = regions[mergeId]; + + // Merge neighbours. + if (mergeRegions(target, reg)) + { + // Fixup regions pointing to current region. + for (int j = 0; j < nreg; ++j) + { + if (regions[j].id == 0 || (regions[j].id & RC_BORDER_REG)) continue; + // If another region was already merged into current region + // change the nid of the previous region too. + if (regions[j].id == oldId) + regions[j].id = mergeId; + // Replace the current region with the new one if the + // current regions is neighbour. + replaceNeighbour(regions[j], oldId, mergeId); + } + mergeCount++; + } + } + } + } + while (mergeCount > 0); + + // Compress region Ids. + for (int i = 0; i < nreg; ++i) + { + regions[i].remap = false; + if (regions[i].id == 0) continue; // Skip nil regions. + if (regions[i].id & RC_BORDER_REG) continue; // Skip external regions. + regions[i].remap = true; + } + + unsigned short regIdGen = 0; + for (int i = 0; i < nreg; ++i) + { + if (!regions[i].remap) + continue; + unsigned short oldId = regions[i].id; + unsigned short newId = ++regIdGen; + for (int j = i; j < nreg; ++j) + { + if (regions[j].id == oldId) + { + regions[j].id = newId; + regions[j].remap = false; + } + } + } + maxRegionId = regIdGen; + + // Remap regions. + for (int i = 0; i < chf.spanCount; ++i) + { + if ((src[i*2] & RC_BORDER_REG) == 0) + src[i*2] = regions[src[i*2]].id; + } + + delete [] regions; + + return true; +} + +bool rcBuildDistanceField(rcCompactHeightfield& chf) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + unsigned short* dist0 = new unsigned short[chf.spanCount]; + if (!dist0) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'dist0' (%d).", chf.spanCount); + return false; + } + unsigned short* dist1 = new unsigned short[chf.spanCount]; + if (!dist1) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'dist1' (%d).", chf.spanCount); + delete [] dist0; + return false; + } + + unsigned short* src = dist0; + unsigned short* dst = dist1; + + unsigned short maxDist = 0; + + rcTimeVal distStartTime = rcGetPerformanceTimer(); + + if (calculateDistanceField(chf, src, dst, maxDist) != src) + rcSwap(src, dst); + + chf.maxDistance = maxDist; + + rcTimeVal distEndTime = rcGetPerformanceTimer(); + + rcTimeVal blurStartTime = rcGetPerformanceTimer(); + + // Blur + if (boxBlur(chf, 1, src, dst) != src) + rcSwap(src, dst); + + // Store distance. + for (int i = 0; i < chf.spanCount; ++i) + chf.spans[i].dist = src[i]; + + rcTimeVal blurEndTime = rcGetPerformanceTimer(); + + delete [] dist0; + delete [] dist1; + + rcTimeVal endTime = rcGetPerformanceTimer(); + +/* if (rcGetLog()) + { + rcGetLog()->log(RC_LOG_PROGRESS, "Build distance field: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - dist: %.3f ms", rcGetDeltaTimeUsec(distStartTime, distEndTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - blur: %.3f ms", rcGetDeltaTimeUsec(blurStartTime, blurEndTime)/1000.0f); + }*/ + if (rcGetBuildTimes()) + { + rcGetBuildTimes()->buildDistanceField += rcGetDeltaTimeUsec(startTime, endTime); + rcGetBuildTimes()->buildDistanceFieldDist += rcGetDeltaTimeUsec(distStartTime, distEndTime); + rcGetBuildTimes()->buildDistanceFieldBlur += rcGetDeltaTimeUsec(blurStartTime, blurEndTime); + } + + return true; +} + +static void paintRectRegion(int minx, int maxx, int miny, int maxy, + unsigned short regId, unsigned short minLevel, + rcCompactHeightfield& chf, unsigned short* src) +{ + const int w = chf.width; + for (int y = miny; y < maxy; ++y) + { + for (int x = minx; x < maxx; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (chf.spans[i].dist >= minLevel) + src[i*2] = regId; + } + } + } +} + +bool rcBuildRegions(rcCompactHeightfield& chf, + int walkableRadius, int borderSize, + int minRegionSize, int mergeRegionSize) +{ + rcTimeVal startTime = rcGetPerformanceTimer(); + + const int w = chf.width; + const int h = chf.height; + + unsigned short* tmp1 = new unsigned short[chf.spanCount*2]; + if (!tmp1) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'tmp1' (%d).", chf.spanCount*2); + return false; + } + unsigned short* tmp2 = new unsigned short[chf.spanCount*2]; + if (!tmp2) + { + if (rcGetLog()) + rcGetLog()->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'tmp2' (%d).", chf.spanCount*2); + delete [] tmp1; + return false; + } + + rcTimeVal regStartTime = rcGetPerformanceTimer(); + + rcIntArray stack(1024); + rcIntArray visited(1024); + + unsigned short* src = tmp1; + unsigned short* dst = tmp2; + + memset(src, 0, sizeof(unsigned short) * chf.spanCount*2); + + unsigned short regionId = 1; + unsigned short level = (chf.maxDistance+1) & ~1; + + unsigned short minLevel = (unsigned short)(walkableRadius*2); + + const int expandIters = 4 + walkableRadius * 2; + + // Mark border regions. + paintRectRegion(0, borderSize, 0, h, regionId|RC_BORDER_REG, minLevel, chf, src); regionId++; + paintRectRegion(w-borderSize, w, 0, h, regionId|RC_BORDER_REG, minLevel, chf, src); regionId++; + paintRectRegion(0, w, 0, borderSize, regionId|RC_BORDER_REG, minLevel, chf, src); regionId++; + paintRectRegion(0, w, h-borderSize, h, regionId|RC_BORDER_REG, minLevel, chf, src); regionId++; + + rcTimeVal expTime = 0; + rcTimeVal floodTime = 0; + + while (level > minLevel) + { + level = level >= 2 ? level-2 : 0; + + rcTimeVal expStartTime = rcGetPerformanceTimer(); + + // Expand current regions until no empty connected cells found. + if (expandRegions(expandIters, level, chf, src, dst, stack) != src) + rcSwap(src, dst); + + expTime += rcGetPerformanceTimer() - expStartTime; + + rcTimeVal floodStartTime = rcGetPerformanceTimer(); + + // Mark new regions with IDs. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + if (chf.spans[i].dist < level || src[i*2] != 0) + continue; + + if (floodRegion(x, y, i, minLevel, level, regionId, chf, src, stack)) + regionId++; + } + } + } + + floodTime += rcGetPerformanceTimer() - floodStartTime; + + } + + // Expand current regions until no empty connected cells found. + if (expandRegions(expandIters*8, minLevel, chf, src, dst, stack) != src) + rcSwap(src, dst); + + rcTimeVal regEndTime = rcGetPerformanceTimer(); + + rcTimeVal filterStartTime = rcGetPerformanceTimer(); + + // Filter out small regions. + chf.maxRegions = regionId; + if (!filterSmallRegions(minRegionSize, mergeRegionSize, chf.maxRegions, chf, src)) + return false; + + rcTimeVal filterEndTime = rcGetPerformanceTimer(); + + // Write the result out. + for (int i = 0; i < chf.spanCount; ++i) + chf.spans[i].reg = src[i*2]; + + delete [] tmp1; + delete [] tmp2; + + rcTimeVal endTime = rcGetPerformanceTimer(); + +/* if (rcGetLog()) + { + rcGetLog()->log(RC_LOG_PROGRESS, "Build regions: %.3f ms", rcGetDeltaTimeUsec(startTime, endTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - reg: %.3f ms", rcGetDeltaTimeUsec(regStartTime, regEndTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - exp: %.3f ms", rcGetDeltaTimeUsec(0, expTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - flood: %.3f ms", rcGetDeltaTimeUsec(0, floodTime)/1000.0f); + rcGetLog()->log(RC_LOG_PROGRESS, " - filter: %.3f ms", rcGetDeltaTimeUsec(filterStartTime, filterEndTime)/1000.0f); + } +*/ + if (rcGetBuildTimes()) + { + rcGetBuildTimes()->buildRegions += rcGetDeltaTimeUsec(startTime, endTime); + rcGetBuildTimes()->buildRegionsReg += rcGetDeltaTimeUsec(regStartTime, regEndTime); + rcGetBuildTimes()->buildRegionsExp += rcGetDeltaTimeUsec(0, expTime); + rcGetBuildTimes()->buildRegionsFlood += rcGetDeltaTimeUsec(0, floodTime); + rcGetBuildTimes()->buildRegionsFilter += rcGetDeltaTimeUsec(filterStartTime, filterEndTime); + } + + return true; +} + + diff --git a/extern/recastnavigation/Recast/Source/RecastTimer.cpp b/extern/recastnavigation/Recast/Source/RecastTimer.cpp new file mode 100644 index 00000000000..51ffb7d3160 --- /dev/null +++ b/extern/recastnavigation/Recast/Source/RecastTimer.cpp @@ -0,0 +1,58 @@ +#include "RecastTimer.h" + +#if defined(WIN32) + +// Win32 +#include <windows.h> + +rcTimeVal rcGetPerformanceTimer() +{ + __int64 count; + QueryPerformanceCounter((LARGE_INTEGER*)&count); + return count; +} + +int rcGetDeltaTimeUsec(rcTimeVal start, rcTimeVal end) +{ + static __int64 freq = 0; + if (freq == 0) + QueryPerformanceFrequency((LARGE_INTEGER*)&freq); + __int64 elapsed = end - start; + return (int)(elapsed*1000000 / freq); +} + +#elif defined(__MACH__) + +// OSX +#include <mach/mach_time.h> + +rcTimeVal rcGetPerformanceTimer() +{ + return mach_absolute_time(); +} + +int rcGetDeltaTimeUsec(rcTimeVal start, rcTimeVal end) +{ + static mach_timebase_info_data_t timebaseInfo; + if (timebaseInfo.denom == 0) + mach_timebase_info(&timebaseInfo); + uint64_t elapsed = end - start; + uint64_t nanosec = elapsed * timebaseInfo.numer / timebaseInfo.denom; + return (int)(nanosec / 1000); +} + +#else + +// TODO: Linux, etc + +rcTimeVal rcGetPerformanceTimer() +{ + return 0; +} + +int rcGetDeltaTimeUsec(rcTimeVal start, rcTimeVal end) +{ + return 0; +} + +#endif
\ No newline at end of file |