From ca6fed5371a0250d1a62c384f23ff24b57ad1758 Mon Sep 17 00:00:00 2001 From: frank Date: Mon, 8 Jan 2018 20:55:50 +0100 Subject: [PATCH] worked on grid-walking worked on grid-generation added helper library for nav-meshes started working on nav meshes --- grid/Grid.h | 18 + grid/factory/v3/GridFactory3.h | 33 +- grid/walk/v3/Walker.h | 2 + lib/Recast/Recast.cpp | 511 ++++ lib/Recast/Recast.h | 1206 ++++++++ lib/Recast/RecastAlloc.cpp | 86 + lib/Recast/RecastAlloc.h | 146 + lib/Recast/RecastArea.cpp | 591 ++++ lib/Recast/RecastAssert.cpp | 35 + lib/Recast/RecastAssert.h | 56 + lib/Recast/RecastContour.cpp | 1105 ++++++++ lib/Recast/RecastFilter.cpp | 202 ++ lib/Recast/RecastLayers.cpp | 644 +++++ lib/Recast/RecastMesh.cpp | 1552 +++++++++++ lib/Recast/RecastMeshDetail.cpp | 1462 ++++++++++ lib/Recast/RecastRasterization.cpp | 454 +++ lib/Recast/RecastRegion.cpp | 1824 ++++++++++++ lib/gpc/gpc.cpp.h | 3080 +++++++++++---------- main.cpp | 4 +- navMesh/NavMesh.h | 99 + navMesh/NavMeshDebug.h | 75 + navMesh/NavMeshFactory.h | 573 ++++ navMesh/NavMeshPoly.h | 123 + navMesh/NavMeshRandom.h | 49 + navMesh/NavMeshTriangle.h | 126 + navMesh/walk/NavMeshWalkSimple.h | 43 + sensors/imu/MagnetometerData.h | 2 +- tests/math/filter/TestButter.cpp | 298 +- tests/navMesh/TestNavMeshFactory.cpp | 38 + tests/navMesh/TestNavMeshTriangle.cpp | 35 + tests/sensors/imu/TestMotionDetection.cpp | 226 +- tests/sensors/imu/TestTurnDetection.cpp | 38 +- tests/sensors/pressure/TestBarometer.cpp | 168 +- 33 files changed, 12991 insertions(+), 1913 deletions(-) create mode 100644 lib/Recast/Recast.cpp create mode 100644 lib/Recast/Recast.h create mode 100644 lib/Recast/RecastAlloc.cpp create mode 100644 lib/Recast/RecastAlloc.h create mode 100644 lib/Recast/RecastArea.cpp create mode 100644 lib/Recast/RecastAssert.cpp create mode 100644 lib/Recast/RecastAssert.h create mode 100644 lib/Recast/RecastContour.cpp create mode 100644 lib/Recast/RecastFilter.cpp create mode 100644 lib/Recast/RecastLayers.cpp create mode 100644 lib/Recast/RecastMesh.cpp create mode 100644 lib/Recast/RecastMeshDetail.cpp create mode 100644 lib/Recast/RecastRasterization.cpp create mode 100644 lib/Recast/RecastRegion.cpp create mode 100644 navMesh/NavMesh.h create mode 100644 navMesh/NavMeshDebug.h create mode 100644 navMesh/NavMeshFactory.h create mode 100644 navMesh/NavMeshPoly.h create mode 100644 navMesh/NavMeshRandom.h create mode 100644 navMesh/NavMeshTriangle.h create mode 100644 navMesh/walk/NavMeshWalkSimple.h create mode 100644 tests/navMesh/TestNavMeshFactory.cpp create mode 100644 tests/navMesh/TestNavMeshTriangle.cpp diff --git a/grid/Grid.h b/grid/Grid.h index 4340e8b..fa74144 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -16,6 +16,11 @@ #include "../geo/BBox3.h" #include "../misc/Debug.h" +#define GM_BOX 1 +#define GM_HOBEYCOMB 2 +#define GRID_MODE GM_BOX + + /** * grid of a given-size, storing some user-data-value which * - extends GridPoint and GridNode @@ -240,9 +245,18 @@ public: const uint64_t center = 1 << 19; // build +#if (GRID_MODE == GM_HOBEYCOMB) + const int xx = ((int)std::round(p.y_cm / gridSize_cm) % 2 == 0) ? (0) : (gridSize_cm/2); + const uint64_t x = center + (int64_t) idxX(p.x_cm-xx); + const uint64_t y = center + (int64_t) idxY(p.y_cm); + const uint64_t z = center + (int64_t) idxZ(p.z_cm); +#elif (GRID_MODE == GM_BOX) const uint64_t x = center + (int64_t) idxX(p.x_cm); const uint64_t y = center + (int64_t) idxY(p.y_cm); const uint64_t z = center + (int64_t) idxZ(p.z_cm); +#endif + + return (z << 40) | (y << 20) | (x << 0); @@ -527,9 +541,13 @@ private: /** asssert that the given element is aligned to the grid */ void assertAligned(const T& elem) { +#if (GRID_MODE == GM_HOBEYCOMB) + +#elif (GRID_MODE == GM_BOX) if (((int)elem.x_cm % gridSize_cm) != 0) {throw Exception("element's x is not aligned!");} if (((int)elem.y_cm % gridSize_cm) != 0) {throw Exception("element's y is not aligned!");} //if (((int)elem.z_cm % gridSize_cm) != 0) {throw Exception("element's z is not aligned!");} +#endif } }; diff --git a/grid/factory/v3/GridFactory3.h b/grid/factory/v3/GridFactory3.h index cd68c86..9914638 100644 --- a/grid/factory/v3/GridFactory3.h +++ b/grid/factory/v3/GridFactory3.h @@ -4,7 +4,20 @@ #include "../../Grid.h" #include "../../../floorplan/v2/Floorplan.h" #include "HelperPoly3.h" +#include +#if (GRID_MODE == GM_BOX) + +#define GF3_ITER_XY for (int y = y1; y <= y2; y += gs_cm) { for (int x = x1; x <= x2; x += gs_cm) { + +#elif (GRID_MODE == GM_HOBEYCOMB) + +#define GF3_ITER_XY\ +for (int y = y1; y <= y2; y += gs_cm) {\ + const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2);\ + for (int x = x1-xx; x <= x2; x += gs_cm) { + +#endif template class GridFactory3 { @@ -125,6 +138,7 @@ public: if (n1 == n2) {continue;} + // stair with floor if ( (n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_FLOOR) || (n2.getType() == GridNode::TYPE_STAIR && n1.getType() == GridNode::TYPE_FLOOR) @@ -132,25 +146,28 @@ public: const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy()); const float distz_cm = std::abs(n1.z_cm - n2.z_cm); - if (distxy > 0 && distxy < gs_cm * 1.5 / 100.0f && distz_cm < gs_cm) { + if (distxy > 0 && distxy < gs_cm * 1.2 / 100.0f && distz_cm < gs_cm) { // [1.85] if (n1.fullyConnected()) {continue;} if (n2.fullyConnected()) {continue;} grid.connectUniDir(n1, n2); } + // floor with floor } else if (n1.getType() == GridNode::TYPE_FLOOR && n2.getType() == GridNode::TYPE_FLOOR) { - if (n1.getDistanceInCM(n2) < gs_cm * 1.45 && !isBlocked(map, n1, n2)) { + if (n1.getDistanceInCM(n2) < gs_cm * 1.2 && !isBlocked(map, n1, n2)) { // [1.2 | 1.845] if (n1.fullyConnected()) {continue;} if (n2.fullyConnected()) {continue;} grid.connectUniDir(n1, n2); } + + // stair with stair } else if (n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_STAIR) { const float distxy = n1.inMeter().xy().getDistance(n2.inMeter().xy()); const float distz_cm = std::abs(n1.z_cm - n2.z_cm); // if (n1.getDistanceInCM(n2) < gs_cm * 1.45 && !isBlocked(map, n1, n2)) { - if (distxy < gs_cm * 1.45 / 100.0f && distz_cm <= gs_cm) { + if (distxy < gs_cm * 1.2 / 100.0f && distz_cm <= gs_cm) { // [1.845] if (n1.fullyConnected()) {continue;} if (n2.fullyConnected()) {continue;} grid.connectUniDir(n1, n2); @@ -328,8 +345,7 @@ public: polygons.push_back(Combo(pol, poly)); } - for (int y = y1; y <= y2; y += gs_cm) { - for (int x = x1; x <= x2; x += gs_cm) { + GF3_ITER_XY int type = GridNode::TYPE_FLOOR; bool remove = false; @@ -429,8 +445,11 @@ public: //int zFloor = floor->atHeight * 100; - for (int y = y1; y <= y2; y += gs_cm) { - for (int x = x1; x <= x2; x += gs_cm) { +// for (int y = y1; y <= y2; y += gs_cm) { +// const int xx = (y / gs_cm % 2 == 0) ? (0) : (gs_cm/2); +// for (int x = x1-xx; x <= x2; x += gs_cm) { + + GF3_ITER_XY int z = 0; Point2 p(x/100.0f, y/100.0f); diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index 48a3c2e..f40a542 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -184,7 +184,9 @@ namespace GW3 { } +#if (GRID_MODE == GM_BOX) Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid"); +#endif return res; diff --git a/lib/Recast/Recast.cpp b/lib/Recast/Recast.cpp new file mode 100644 index 0000000..29316e4 --- /dev/null +++ b/lib/Recast/Recast.cpp @@ -0,0 +1,511 @@ +// +// Copyright (c) 2009-2010 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 +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastAssert.h" + +float rcSqrt(float x) +{ + return sqrtf(x); +} + +/// @class rcContext +/// @par +/// +/// This class does not provide logging or timer functionality on its +/// own. Both must be provided by a concrete implementation +/// by overriding the protected member functions. Also, this class does not +/// provide an interface for extracting log messages. (Only adding them.) +/// So concrete implementations must provide one. +/// +/// If no logging or timers are required, just pass an instance of this +/// class through the Recast build process. +/// + +/// @par +/// +/// Example: +/// @code +/// // Where ctx is an instance of rcContext and filepath is a char array. +/// ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath); +/// @endcode +void rcContext::log(const rcLogCategory category, const char* format, ...) +{ + if (!m_logEnabled) + return; + static const int MSG_SIZE = 512; + char msg[MSG_SIZE]; + va_list ap; + va_start(ap, format); + int len = vsnprintf(msg, MSG_SIZE, format, ap); + if (len >= MSG_SIZE) + { + len = MSG_SIZE-1; + msg[MSG_SIZE-1] = '\0'; + } + va_end(ap); + doLog(category, msg, len); +} + +rcHeightfield* rcAllocHeightfield() +{ + return new (rcAlloc(sizeof(rcHeightfield), RC_ALLOC_PERM)) rcHeightfield; +} + +rcHeightfield::rcHeightfield() + : width() + , height() + , bmin() + , bmax() + , cs() + , ch() + , spans() + , pools() + , freelist() +{ +} + +rcHeightfield::~rcHeightfield() +{ + // Delete span array. + rcFree(spans); + // Delete span pools. + while (pools) + { + rcSpanPool* next = pools->next; + rcFree(pools); + pools = next; + } +} + +void rcFreeHeightField(rcHeightfield* hf) +{ + if (!hf) return; + hf->~rcHeightfield(); + rcFree(hf); +} + +rcCompactHeightfield* rcAllocCompactHeightfield() +{ + rcCompactHeightfield* chf = (rcCompactHeightfield*)rcAlloc(sizeof(rcCompactHeightfield), RC_ALLOC_PERM); + memset(chf, 0, sizeof(rcCompactHeightfield)); + return chf; +} + +void rcFreeCompactHeightfield(rcCompactHeightfield* chf) +{ + if (!chf) return; + rcFree(chf->cells); + rcFree(chf->spans); + rcFree(chf->dist); + rcFree(chf->areas); + rcFree(chf); +} + +rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet() +{ + rcHeightfieldLayerSet* lset = (rcHeightfieldLayerSet*)rcAlloc(sizeof(rcHeightfieldLayerSet), RC_ALLOC_PERM); + memset(lset, 0, sizeof(rcHeightfieldLayerSet)); + return lset; +} + +void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset) +{ + if (!lset) return; + for (int i = 0; i < lset->nlayers; ++i) + { + rcFree(lset->layers[i].heights); + rcFree(lset->layers[i].areas); + rcFree(lset->layers[i].cons); + } + rcFree(lset->layers); + rcFree(lset); +} + + +rcContourSet* rcAllocContourSet() +{ + rcContourSet* cset = (rcContourSet*)rcAlloc(sizeof(rcContourSet), RC_ALLOC_PERM); + memset(cset, 0, sizeof(rcContourSet)); + return cset; +} + +void rcFreeContourSet(rcContourSet* cset) +{ + if (!cset) return; + for (int i = 0; i < cset->nconts; ++i) + { + rcFree(cset->conts[i].verts); + rcFree(cset->conts[i].rverts); + } + rcFree(cset->conts); + rcFree(cset); +} + +rcPolyMesh* rcAllocPolyMesh() +{ + rcPolyMesh* pmesh = (rcPolyMesh*)rcAlloc(sizeof(rcPolyMesh), RC_ALLOC_PERM); + memset(pmesh, 0, sizeof(rcPolyMesh)); + return pmesh; +} + +void rcFreePolyMesh(rcPolyMesh* pmesh) +{ + if (!pmesh) return; + rcFree(pmesh->verts); + rcFree(pmesh->polys); + rcFree(pmesh->regs); + rcFree(pmesh->flags); + rcFree(pmesh->areas); + rcFree(pmesh); +} + +rcPolyMeshDetail* rcAllocPolyMeshDetail() +{ + rcPolyMeshDetail* dmesh = (rcPolyMeshDetail*)rcAlloc(sizeof(rcPolyMeshDetail), RC_ALLOC_PERM); + memset(dmesh, 0, sizeof(rcPolyMeshDetail)); + return dmesh; +} + +void rcFreePolyMeshDetail(rcPolyMeshDetail* dmesh) +{ + if (!dmesh) return; + rcFree(dmesh->meshes); + rcFree(dmesh->verts); + rcFree(dmesh->tris); + rcFree(dmesh); +} + +void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax) +{ + // Calculate bounding box. + rcVcopy(bmin, verts); + rcVcopy(bmax, verts); + for (int i = 1; i < nv; ++i) + { + const float* v = &verts[i*3]; + rcVmin(bmin, v); + rcVmax(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); +} + +/// @par +/// +/// See the #rcConfig documentation for more information on the configuration parameters. +/// +/// @see rcAllocHeightfield, rcHeightfield +bool rcCreateHeightfield(rcContext* ctx, rcHeightfield& hf, int width, int height, + const float* bmin, const float* bmax, + float cs, float ch) +{ + rcIgnoreUnused(ctx); + + hf.width = width; + hf.height = height; + rcVcopy(hf.bmin, bmin); + rcVcopy(hf.bmax, bmax); + hf.cs = cs; + hf.ch = ch; + hf.spans = (rcSpan**)rcAlloc(sizeof(rcSpan*)*hf.width*hf.height, RC_ALLOC_PERM); + 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]; + rcVsub(e0, v1, v0); + rcVsub(e1, v2, v0); + rcVcross(norm, e0, e1); + rcVnormalize(norm); +} + +/// @par +/// +/// Only sets the area id's for the walkable triangles. Does not alter the +/// area id's for unwalkable triangles. +/// +/// See the #rcConfig documentation for more information on the configuration parameters. +/// +/// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles +void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, + const float* verts, int nv, + const int* tris, int nt, + unsigned char* areas) +{ + rcIgnoreUnused(ctx); + rcIgnoreUnused(nv); + + const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI); + + float norm[3]; + + for (int i = 0; i < nt; ++i) + { + const int* tri = &tris[i*3]; + int a = tri[0]; + int b = tri[1]; + int c = tri[2]; + float aa = verts[6]; + float bb = verts[7]; + float cc = verts[8]; + + calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm); + // Check if the face is walkable. + if (norm[1] > walkableThr) + areas[i] = RC_WALKABLE_AREA; + } +} + +/// @par +/// +/// Only sets the area id's for the unwalkable triangles. Does not alter the +/// area id's for walkable triangles. +/// +/// See the #rcConfig documentation for more information on the configuration parameters. +/// +/// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles +void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, + const float* verts, int /*nv*/, + const int* tris, int nt, + unsigned char* areas) +{ + rcIgnoreUnused(ctx); + + const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_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) + areas[i] = RC_NULL_AREA; + } +} + +int rcGetHeightFieldSpanCount(rcContext* ctx, rcHeightfield& hf) +{ + rcIgnoreUnused(ctx); + + 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->area != RC_NULL_AREA) + spanCount++; + } + } + } + return spanCount; +} + +/// @par +/// +/// This is just the beginning of the process of fully building a compact heightfield. +/// Various filters may be applied, then the distance field and regions built. +/// E.g: #rcBuildDistanceField and #rcBuildRegions +/// +/// See the #rcConfig documentation for more information on the configuration parameters. +/// +/// @see rcAllocCompactHeightfield, rcHeightfield, rcCompactHeightfield, rcConfig +bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const int walkableClimb, + rcHeightfield& hf, rcCompactHeightfield& chf) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_BUILD_COMPACTHEIGHTFIELD); + + const int w = hf.width; + const int h = hf.height; + const int spanCount = rcGetHeightFieldSpanCount(ctx, hf); + + // Fill in header. + chf.width = w; + chf.height = h; + chf.spanCount = spanCount; + chf.walkableHeight = walkableHeight; + chf.walkableClimb = walkableClimb; + chf.maxRegions = 0; + rcVcopy(chf.bmin, hf.bmin); + rcVcopy(chf.bmax, hf.bmax); + chf.bmax[1] += walkableHeight*hf.ch; + chf.cs = hf.cs; + chf.ch = hf.ch; + chf.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell)*w*h, RC_ALLOC_PERM); + if (!chf.cells) + { + ctx->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 = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan)*spanCount, RC_ALLOC_PERM); + if (!chf.spans) + { + ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount); + return false; + } + memset(chf.spans, 0, sizeof(rcCompactSpan)*spanCount); + chf.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*spanCount, RC_ALLOC_PERM); + if (!chf.areas) + { + ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.areas' (%d)", spanCount); + return false; + } + memset(chf.areas, RC_NULL_AREA, sizeof(unsigned char)*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->area != RC_NULL_AREA) + { + 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); + chf.areas[idx] = s->area; + idx++; + c.count++; + } + s = s->next; + } + } + } + + // Find neighbour connections. + const int MAX_LAYERS = RC_NOT_CONNECTED-1; + int tooHighNeighbour = 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) + { + rcCompactSpan& s = chf.spans[i]; + + for (int dir = 0; dir < 4; ++dir) + { + rcSetCon(s, dir, RC_NOT_CONNECTED); + 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. + const int lidx = k - (int)nc.index; + if (lidx < 0 || lidx > MAX_LAYERS) + { + tooHighNeighbour = rcMax(tooHighNeighbour, lidx); + continue; + } + rcSetCon(s, dir, lidx); + break; + } + } + + } + } + } + } + + if (tooHighNeighbour > MAX_LAYERS) + { + ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Heightfield has too many layers %d (max: %d)", + tooHighNeighbour, MAX_LAYERS); + } + + 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/lib/Recast/Recast.h b/lib/Recast/Recast.h new file mode 100644 index 0000000..4eab5d6 --- /dev/null +++ b/lib/Recast/Recast.h @@ -0,0 +1,1206 @@ +// +// Copyright (c) 2009-2010 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 + +#ifndef RECAST_H +#define RECAST_H + +/// The value of PI used by Recast. +static const float RC_PI = 3.14159265f; + +/// Recast log categories. +/// @see rcContext +enum rcLogCategory +{ + RC_LOG_PROGRESS = 1, ///< A progress log entry. + RC_LOG_WARNING, ///< A warning log entry. + RC_LOG_ERROR, ///< An error log entry. +}; + +/// Recast performance timer categories. +/// @see rcContext +enum rcTimerLabel +{ + /// The user defined total time of the build. + RC_TIMER_TOTAL, + /// A user defined build time. + RC_TIMER_TEMP, + /// The time to rasterize the triangles. (See: #rcRasterizeTriangle) + RC_TIMER_RASTERIZE_TRIANGLES, + /// The time to build the compact heightfield. (See: #rcBuildCompactHeightfield) + RC_TIMER_BUILD_COMPACTHEIGHTFIELD, + /// The total time to build the contours. (See: #rcBuildContours) + RC_TIMER_BUILD_CONTOURS, + /// The time to trace the boundaries of the contours. (See: #rcBuildContours) + RC_TIMER_BUILD_CONTOURS_TRACE, + /// The time to simplify the contours. (See: #rcBuildContours) + RC_TIMER_BUILD_CONTOURS_SIMPLIFY, + /// The time to filter ledge spans. (See: #rcFilterLedgeSpans) + RC_TIMER_FILTER_BORDER, + /// The time to filter low height spans. (See: #rcFilterWalkableLowHeightSpans) + RC_TIMER_FILTER_WALKABLE, + /// The time to apply the median filter. (See: #rcMedianFilterWalkableArea) + RC_TIMER_MEDIAN_AREA, + /// The time to filter low obstacles. (See: #rcFilterLowHangingWalkableObstacles) + RC_TIMER_FILTER_LOW_OBSTACLES, + /// The time to build the polygon mesh. (See: #rcBuildPolyMesh) + RC_TIMER_BUILD_POLYMESH, + /// The time to merge polygon meshes. (See: #rcMergePolyMeshes) + RC_TIMER_MERGE_POLYMESH, + /// The time to erode the walkable area. (See: #rcErodeWalkableArea) + RC_TIMER_ERODE_AREA, + /// The time to mark a box area. (See: #rcMarkBoxArea) + RC_TIMER_MARK_BOX_AREA, + /// The time to mark a cylinder area. (See: #rcMarkCylinderArea) + RC_TIMER_MARK_CYLINDER_AREA, + /// The time to mark a convex polygon area. (See: #rcMarkConvexPolyArea) + RC_TIMER_MARK_CONVEXPOLY_AREA, + /// The total time to build the distance field. (See: #rcBuildDistanceField) + RC_TIMER_BUILD_DISTANCEFIELD, + /// The time to build the distances of the distance field. (See: #rcBuildDistanceField) + RC_TIMER_BUILD_DISTANCEFIELD_DIST, + /// The time to blur the distance field. (See: #rcBuildDistanceField) + RC_TIMER_BUILD_DISTANCEFIELD_BLUR, + /// The total time to build the regions. (See: #rcBuildRegions, #rcBuildRegionsMonotone) + RC_TIMER_BUILD_REGIONS, + /// The total time to apply the watershed algorithm. (See: #rcBuildRegions) + RC_TIMER_BUILD_REGIONS_WATERSHED, + /// The time to expand regions while applying the watershed algorithm. (See: #rcBuildRegions) + RC_TIMER_BUILD_REGIONS_EXPAND, + /// The time to flood regions while applying the watershed algorithm. (See: #rcBuildRegions) + RC_TIMER_BUILD_REGIONS_FLOOD, + /// The time to filter out small regions. (See: #rcBuildRegions, #rcBuildRegionsMonotone) + RC_TIMER_BUILD_REGIONS_FILTER, + /// The time to build heightfield layers. (See: #rcBuildHeightfieldLayers) + RC_TIMER_BUILD_LAYERS, + /// The time to build the polygon mesh detail. (See: #rcBuildPolyMeshDetail) + RC_TIMER_BUILD_POLYMESHDETAIL, + /// The time to merge polygon mesh details. (See: #rcMergePolyMeshDetails) + RC_TIMER_MERGE_POLYMESHDETAIL, + /// The maximum number of timers. (Used for iterating timers.) + RC_MAX_TIMERS +}; + +/// Provides an interface for optional logging and performance tracking of the Recast +/// build process. +/// @ingroup recast +class rcContext +{ +public: + + /// Contructor. + /// @param[in] state TRUE if the logging and performance timers should be enabled. [Default: true] + inline rcContext(bool state = true) : m_logEnabled(state), m_timerEnabled(state) {} + virtual ~rcContext() {} + + /// Enables or disables logging. + /// @param[in] state TRUE if logging should be enabled. + inline void enableLog(bool state) { m_logEnabled = state; } + + /// Clears all log entries. + inline void resetLog() { if (m_logEnabled) doResetLog(); } + + /// Logs a message. + /// @param[in] category The category of the message. + /// @param[in] format The message. + void log(const rcLogCategory category, const char* format, ...); + + /// Enables or disables the performance timers. + /// @param[in] state TRUE if timers should be enabled. + inline void enableTimer(bool state) { m_timerEnabled = state; } + + /// Clears all peformance timers. (Resets all to unused.) + inline void resetTimers() { if (m_timerEnabled) doResetTimers(); } + + /// Starts the specified performance timer. + /// @param label The category of the timer. + inline void startTimer(const rcTimerLabel label) { if (m_timerEnabled) doStartTimer(label); } + + /// Stops the specified performance timer. + /// @param label The category of the timer. + inline void stopTimer(const rcTimerLabel label) { if (m_timerEnabled) doStopTimer(label); } + + /// Returns the total accumulated time of the specified performance timer. + /// @param label The category of the timer. + /// @return The accumulated time of the timer, or -1 if timers are disabled or the timer has never been started. + inline int getAccumulatedTime(const rcTimerLabel label) const { return m_timerEnabled ? doGetAccumulatedTime(label) : -1; } + +protected: + + /// Clears all log entries. + virtual void doResetLog() {} + + /// Logs a message. + /// @param[in] category The category of the message. + /// @param[in] msg The formatted message. + /// @param[in] len The length of the formatted message. + //virtual void doLog(const rcLogCategory /*category*/, const char* /*msg*/, const int /*len*/) {} + + virtual void doLog(const rcLogCategory /*category*/, const char* msg, const int ) { + std::cout << msg << std::endl; + } + + /// Clears all timers. (Resets all to unused.) + virtual void doResetTimers() {} + + /// Starts the specified performance timer. + /// @param[in] label The category of timer. + virtual void doStartTimer(const rcTimerLabel /*label*/) {} + + /// Stops the specified performance timer. + /// @param[in] label The category of the timer. + virtual void doStopTimer(const rcTimerLabel /*label*/) {} + + /// Returns the total accumulated time of the specified performance timer. + /// @param[in] label The category of the timer. + /// @return The accumulated time of the timer, or -1 if timers are disabled or the timer has never been started. + virtual int doGetAccumulatedTime(const rcTimerLabel /*label*/) const { return -1; } + + /// True if logging is enabled. + bool m_logEnabled; + + /// True if the performance timers are enabled. + bool m_timerEnabled; +}; + +/// A helper to first start a timer and then stop it when this helper goes out of scope. +/// @see rcContext +class rcScopedTimer +{ +public: + /// Constructs an instance and starts the timer. + /// @param[in] ctx The context to use. + /// @param[in] label The category of the timer. + inline rcScopedTimer(rcContext* ctx, const rcTimerLabel label) : m_ctx(ctx), m_label(label) { m_ctx->startTimer(m_label); } + inline ~rcScopedTimer() { m_ctx->stopTimer(m_label); } + +private: + // Explicitly disabled copy constructor and copy assignment operator. + rcScopedTimer(const rcScopedTimer&); + rcScopedTimer& operator=(const rcScopedTimer&); + + rcContext* const m_ctx; + const rcTimerLabel m_label; +}; + +/// Specifies a configuration to use when performing Recast builds. +/// @ingroup recast +struct rcConfig +{ + /// The width of the field along the x-axis. [Limit: >= 0] [Units: vx] + int width; + + /// The height of the field along the z-axis. [Limit: >= 0] [Units: vx] + int height; + + /// The width/height size of tile's on the xz-plane. [Limit: >= 0] [Units: vx] + int tileSize; + + /// The size of the non-navigable border around the heightfield. [Limit: >=0] [Units: vx] + int borderSize; + + /// The xz-plane cell size to use for fields. [Limit: > 0] [Units: wu] + float cs; + + /// The y-axis cell size to use for fields. [Limit: > 0] [Units: wu] + float ch; + + /// The minimum bounds of the field's AABB. [(x, y, z)] [Units: wu] + float bmin[3]; + + /// The maximum bounds of the field's AABB. [(x, y, z)] [Units: wu] + float bmax[3]; + + /// The maximum slope that is considered walkable. [Limits: 0 <= value < 90] [Units: Degrees] + float walkableSlopeAngle; + + /// Minimum floor to 'ceiling' height that will still allow the floor area to + /// be considered walkable. [Limit: >= 3] [Units: vx] + int walkableHeight; + + /// Maximum ledge height that is considered to still be traversable. [Limit: >=0] [Units: vx] + int walkableClimb; + + /// The distance to erode/shrink the walkable area of the heightfield away from + /// obstructions. [Limit: >=0] [Units: vx] + int walkableRadius; + + /// The maximum allowed length for contour edges along the border of the mesh. [Limit: >=0] [Units: vx] + int maxEdgeLen; + + /// The maximum distance a simplfied contour's border edges should deviate + /// the original raw contour. [Limit: >=0] [Units: vx] + float maxSimplificationError; + + /// The minimum number of cells allowed to form isolated island areas. [Limit: >=0] [Units: vx] + int minRegionArea; + + /// Any regions with a span count smaller than this value will, if possible, + /// be merged with larger regions. [Limit: >=0] [Units: vx] + int mergeRegionArea; + + /// The maximum number of vertices allowed for polygons generated during the + /// contour to polygon conversion process. [Limit: >= 3] + int maxVertsPerPoly; + + /// Sets the sampling distance to use when generating the detail mesh. + /// (For height detail only.) [Limits: 0 or >= 0.9] [Units: wu] + float detailSampleDist; + + /// The maximum distance the detail mesh surface should deviate from heightfield + /// data. (For height detail only.) [Limit: >=0] [Units: wu] + float detailSampleMaxError; +}; + +/// Defines the number of bits allocated to rcSpan::smin and rcSpan::smax. +static const int RC_SPAN_HEIGHT_BITS = 13; +/// Defines the maximum value for rcSpan::smin and rcSpan::smax. +static const int RC_SPAN_MAX_HEIGHT = (1 << RC_SPAN_HEIGHT_BITS) - 1; + +/// The number of spans allocated per span spool. +/// @see rcSpanPool +static const int RC_SPANS_PER_POOL = 2048; + +/// Represents a span in a heightfield. +/// @see rcHeightfield +struct rcSpan +{ + unsigned int smin : RC_SPAN_HEIGHT_BITS; ///< The lower limit of the span. [Limit: < #smax] + unsigned int smax : RC_SPAN_HEIGHT_BITS; ///< The upper limit of the span. [Limit: <= #RC_SPAN_MAX_HEIGHT] + unsigned int area : 6; ///< The area id assigned to the span. + rcSpan* next; ///< The next span higher up in column. +}; + +/// A memory pool used for quick allocation of spans within a heightfield. +/// @see rcHeightfield +struct rcSpanPool +{ + rcSpanPool* next; ///< The next span pool. + rcSpan items[RC_SPANS_PER_POOL]; ///< Array of spans in the pool. +}; + +/// A dynamic heightfield representing obstructed space. +/// @ingroup recast +struct rcHeightfield +{ + rcHeightfield(); + ~rcHeightfield(); + + int width; ///< The width of the heightfield. (Along the x-axis in cell units.) + int height; ///< The height of the heightfield. (Along the z-axis in cell units.) + float bmin[3]; ///< The minimum bounds in world space. [(x, y, z)] + float bmax[3]; ///< The maximum bounds in world space. [(x, y, z)] + float cs; ///< The size of each cell. (On the xz-plane.) + float ch; ///< The height of each cell. (The minimum increment along the y-axis.) + rcSpan** spans; ///< Heightfield of spans (width*height). + rcSpanPool* pools; ///< Linked list of span pools. + rcSpan* freelist; ///< The next free span. + +private: + // Explicitly-disabled copy constructor and copy assignment operator. + rcHeightfield(const rcHeightfield&); + rcHeightfield& operator=(const rcHeightfield&); +}; + +/// Provides information on the content of a cell column in a compact heightfield. +struct rcCompactCell +{ + unsigned int index : 24; ///< Index to the first span in the column. + unsigned int count : 8; ///< Number of spans in the column. +}; + +/// Represents a span of unobstructed space within a compact heightfield. +struct rcCompactSpan +{ + unsigned short y; ///< The lower extent of the span. (Measured from the heightfield's base.) + unsigned short reg; ///< The id of the region the span belongs to. (Or zero if not in a region.) + unsigned int con : 24; ///< Packed neighbor connection data. + unsigned int h : 8; ///< The height of the span. (Measured from #y.) +}; + +/// A compact, static heightfield representing unobstructed space. +/// @ingroup recast +struct rcCompactHeightfield +{ + int width; ///< The width of the heightfield. (Along the x-axis in cell units.) + int height; ///< The height of the heightfield. (Along the z-axis in cell units.) + int spanCount; ///< The number of spans in the heightfield. + int walkableHeight; ///< The walkable height used during the build of the field. (See: rcConfig::walkableHeight) + int walkableClimb; ///< The walkable climb used during the build of the field. (See: rcConfig::walkableClimb) + int borderSize; ///< The AABB border size used during the build of the field. (See: rcConfig::borderSize) + unsigned short maxDistance; ///< The maximum distance value of any span within the field. + unsigned short maxRegions; ///< The maximum region id of any span within the field. + float bmin[3]; ///< The minimum bounds in world space. [(x, y, z)] + float bmax[3]; ///< The maximum bounds in world space. [(x, y, z)] + float cs; ///< The size of each cell. (On the xz-plane.) + float ch; ///< The height of each cell. (The minimum increment along the y-axis.) + rcCompactCell* cells; ///< Array of cells. [Size: #width*#height] + rcCompactSpan* spans; ///< Array of spans. [Size: #spanCount] + unsigned short* dist; ///< Array containing border distance data. [Size: #spanCount] + unsigned char* areas; ///< Array containing area id data. [Size: #spanCount] +}; + +/// Represents a heightfield layer within a layer set. +/// @see rcHeightfieldLayerSet +struct rcHeightfieldLayer +{ + float bmin[3]; ///< The minimum bounds in world space. [(x, y, z)] + float bmax[3]; ///< The maximum bounds in world space. [(x, y, z)] + float cs; ///< The size of each cell. (On the xz-plane.) + float ch; ///< The height of each cell. (The minimum increment along the y-axis.) + int width; ///< The width of the heightfield. (Along the x-axis in cell units.) + int height; ///< The height of the heightfield. (Along the z-axis in cell units.) + int minx; ///< The minimum x-bounds of usable data. + int maxx; ///< The maximum x-bounds of usable data. + int miny; ///< The minimum y-bounds of usable data. (Along the z-axis.) + int maxy; ///< The maximum y-bounds of usable data. (Along the z-axis.) + int hmin; ///< The minimum height bounds of usable data. (Along the y-axis.) + int hmax; ///< The maximum height bounds of usable data. (Along the y-axis.) + unsigned char* heights; ///< The heightfield. [Size: width * height] + unsigned char* areas; ///< Area ids. [Size: Same as #heights] + unsigned char* cons; ///< Packed neighbor connection information. [Size: Same as #heights] +}; + +/// Represents a set of heightfield layers. +/// @ingroup recast +/// @see rcAllocHeightfieldLayerSet, rcFreeHeightfieldLayerSet +struct rcHeightfieldLayerSet +{ + rcHeightfieldLayer* layers; ///< The layers in the set. [Size: #nlayers] + int nlayers; ///< The number of layers in the set. +}; + +/// Represents a simple, non-overlapping contour in field space. +struct rcContour +{ + int* verts; ///< Simplified contour vertex and connection data. [Size: 4 * #nverts] + int nverts; ///< The number of vertices in the simplified contour. + int* rverts; ///< Raw contour vertex and connection data. [Size: 4 * #nrverts] + int nrverts; ///< The number of vertices in the raw contour. + unsigned short reg; ///< The region id of the contour. + unsigned char area; ///< The area id of the contour. +}; + +/// Represents a group of related contours. +/// @ingroup recast +struct rcContourSet +{ + rcContour* conts; ///< An array of the contours in the set. [Size: #nconts] + int nconts; ///< The number of contours in the set. + float bmin[3]; ///< The minimum bounds in world space. [(x, y, z)] + float bmax[3]; ///< The maximum bounds in world space. [(x, y, z)] + float cs; ///< The size of each cell. (On the xz-plane.) + float ch; ///< The height of each cell. (The minimum increment along the y-axis.) + int width; ///< The width of the set. (Along the x-axis in cell units.) + int height; ///< The height of the set. (Along the z-axis in cell units.) + int borderSize; ///< The AABB border size used to generate the source data from which the contours were derived. + float maxError; ///< The max edge error that this contour set was simplified with. +}; + +/// Represents a polygon mesh suitable for use in building a navigation mesh. +/// @ingroup recast +struct rcPolyMesh +{ + unsigned short* verts; ///< The mesh vertices. [Form: (x, y, z) * #nverts] + unsigned short* polys; ///< Polygon and neighbor data. [Length: #maxpolys * 2 * #nvp] + unsigned short* regs; ///< The region id assigned to each polygon. [Length: #maxpolys] + unsigned short* flags; ///< The user defined flags for each polygon. [Length: #maxpolys] + unsigned char* areas; ///< The area id assigned to each polygon. [Length: #maxpolys] + int nverts; ///< The number of vertices. + int npolys; ///< The number of polygons. + int maxpolys; ///< The number of allocated polygons. + int nvp; ///< The maximum number of vertices per polygon. + float bmin[3]; ///< The minimum bounds in world space. [(x, y, z)] + float bmax[3]; ///< The maximum bounds in world space. [(x, y, z)] + float cs; ///< The size of each cell. (On the xz-plane.) + float ch; ///< The height of each cell. (The minimum increment along the y-axis.) + int borderSize; ///< The AABB border size used to generate the source data from which the mesh was derived. + float maxEdgeError; ///< The max error of the polygon edges in the mesh. +}; + +/// Contains triangle meshes that represent detailed height data associated +/// with the polygons in its associated polygon mesh object. +/// @ingroup recast +struct rcPolyMeshDetail +{ + unsigned int* meshes; ///< The sub-mesh data. [Size: 4*#nmeshes] + float* verts; ///< The mesh vertices. [Size: 3*#nverts] + unsigned char* tris; ///< The mesh triangles. [Size: 4*#ntris] + int nmeshes; ///< The number of sub-meshes defined by #meshes. + int nverts; ///< The number of vertices in #verts. + int ntris; ///< The number of triangles in #tris. +}; + +/// @name Allocation Functions +/// Functions used to allocate and de-allocate Recast objects. +/// @see rcAllocSetCustom +/// @{ + +/// Allocates a heightfield object using the Recast allocator. +/// @return A heightfield that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcCreateHeightfield, rcFreeHeightField +rcHeightfield* rcAllocHeightfield(); + +/// Frees the specified heightfield object using the Recast allocator. +/// @param[in] hf A heightfield allocated using #rcAllocHeightfield +/// @ingroup recast +/// @see rcAllocHeightfield +void rcFreeHeightField(rcHeightfield* hf); + +/// Allocates a compact heightfield object using the Recast allocator. +/// @return A compact heightfield that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcBuildCompactHeightfield, rcFreeCompactHeightfield +rcCompactHeightfield* rcAllocCompactHeightfield(); + +/// Frees the specified compact heightfield object using the Recast allocator. +/// @param[in] chf A compact heightfield allocated using #rcAllocCompactHeightfield +/// @ingroup recast +/// @see rcAllocCompactHeightfield +void rcFreeCompactHeightfield(rcCompactHeightfield* chf); + +/// Allocates a heightfield layer set using the Recast allocator. +/// @return A heightfield layer set that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcBuildHeightfieldLayers, rcFreeHeightfieldLayerSet +rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet(); + +/// Frees the specified heightfield layer set using the Recast allocator. +/// @param[in] lset A heightfield layer set allocated using #rcAllocHeightfieldLayerSet +/// @ingroup recast +/// @see rcAllocHeightfieldLayerSet +void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset); + +/// Allocates a contour set object using the Recast allocator. +/// @return A contour set that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcBuildContours, rcFreeContourSet +rcContourSet* rcAllocContourSet(); + +/// Frees the specified contour set using the Recast allocator. +/// @param[in] cset A contour set allocated using #rcAllocContourSet +/// @ingroup recast +/// @see rcAllocContourSet +void rcFreeContourSet(rcContourSet* cset); + +/// Allocates a polygon mesh object using the Recast allocator. +/// @return A polygon mesh that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcBuildPolyMesh, rcFreePolyMesh +rcPolyMesh* rcAllocPolyMesh(); + +/// Frees the specified polygon mesh using the Recast allocator. +/// @param[in] pmesh A polygon mesh allocated using #rcAllocPolyMesh +/// @ingroup recast +/// @see rcAllocPolyMesh +void rcFreePolyMesh(rcPolyMesh* pmesh); + +/// Allocates a detail mesh object using the Recast allocator. +/// @return A detail mesh that is ready for initialization, or null on failure. +/// @ingroup recast +/// @see rcBuildPolyMeshDetail, rcFreePolyMeshDetail +rcPolyMeshDetail* rcAllocPolyMeshDetail(); + +/// Frees the specified detail mesh using the Recast allocator. +/// @param[in] dmesh A detail mesh allocated using #rcAllocPolyMeshDetail +/// @ingroup recast +/// @see rcAllocPolyMeshDetail +void rcFreePolyMeshDetail(rcPolyMeshDetail* dmesh); + +/// @} + +/// Heighfield border flag. +/// If a heightfield region ID has this bit set, then the region is a border +/// region and its spans are considered unwalkable. +/// (Used during the region and contour build process.) +/// @see rcCompactSpan::reg +static const unsigned short RC_BORDER_REG = 0x8000; + +/// Polygon touches multiple regions. +/// If a polygon has this region ID it was merged with or created +/// from polygons of different regions during the polymesh +/// build step that removes redundant border vertices. +/// (Used during the polymesh and detail polymesh build processes) +/// @see rcPolyMesh::regs +static const unsigned short RC_MULTIPLE_REGS = 0; + +/// Border vertex flag. +/// If a region ID has this bit set, then the associated element lies on +/// a tile border. If a contour vertex's region ID has this bit set, the +/// vertex will later be removed in order to match the segments and vertices +/// at tile boundaries. +/// (Used during the build process.) +/// @see rcCompactSpan::reg, #rcContour::verts, #rcContour::rverts +static const int RC_BORDER_VERTEX = 0x10000; + +/// Area border flag. +/// If a region ID has this bit set, then the associated element lies on +/// the border of an area. +/// (Used during the region and contour build process.) +/// @see rcCompactSpan::reg, #rcContour::verts, #rcContour::rverts +static const int RC_AREA_BORDER = 0x20000; + +/// Contour build flags. +/// @see rcBuildContours +enum rcBuildContoursFlags +{ + RC_CONTOUR_TESS_WALL_EDGES = 0x01, ///< Tessellate solid (impassable) edges during contour simplification. + RC_CONTOUR_TESS_AREA_EDGES = 0x02, ///< Tessellate edges between areas during contour simplification. +}; + +/// Applied to the region id field of contour vertices in order to extract the region id. +/// The region id field of a vertex may have several flags applied to it. So the +/// fields value can't be used directly. +/// @see rcContour::verts, rcContour::rverts +static const int RC_CONTOUR_REG_MASK = 0xffff; + +/// An value which indicates an invalid index within a mesh. +/// @note This does not necessarily indicate an error. +/// @see rcPolyMesh::polys +static const unsigned short RC_MESH_NULL_IDX = 0xffff; + +/// Represents the null area. +/// When a data element is given this value it is considered to no longer be +/// assigned to a usable area. (E.g. It is unwalkable.) +static const unsigned char RC_NULL_AREA = 0; + +/// The default area id used to indicate a walkable polygon. +/// This is also the maximum allowed area id, and the only non-null area id +/// recognized by some steps in the build process. +static const unsigned char RC_WALKABLE_AREA = 63; + +/// The value returned by #rcGetCon if the specified direction is not connected +/// to another span. (Has no neighbor.) +static const int RC_NOT_CONNECTED = 0x3f; + +/// @name General helper functions +/// @{ + +/// Used to ignore a function parameter. VS complains about unused parameters +/// and this silences the warning. +/// @param [in] _ Unused parameter +template void rcIgnoreUnused(const T&) { } + +/// Swaps the values of the two parameters. +/// @param[in,out] a Value A +/// @param[in,out] b Value B +template inline void rcSwap(T& a, T& b) { T t = a; a = b; b = t; } + +/// Returns the minimum of two values. +/// @param[in] a Value A +/// @param[in] b Value B +/// @return The minimum of the two values. +template inline T rcMin(T a, T b) { return a < b ? a : b; } + +/// Returns the maximum of two values. +/// @param[in] a Value A +/// @param[in] b Value B +/// @return The maximum of the two values. +template inline T rcMax(T a, T b) { return a > b ? a : b; } + +/// Returns the absolute value. +/// @param[in] a The value. +/// @return The absolute value of the specified value. +template inline T rcAbs(T a) { return a < 0 ? -a : a; } + +/// Returns the square of the value. +/// @param[in] a The value. +/// @return The square of the value. +template inline T rcSqr(T a) { return a*a; } + +/// Clamps the value to the specified range. +/// @param[in] v The value to clamp. +/// @param[in] mn The minimum permitted return value. +/// @param[in] mx The maximum permitted return value. +/// @return The value, clamped to the specified range. +template inline T rcClamp(T v, T mn, T mx) { return v < mn ? mn : (v > mx ? mx : v); } + +/// Returns the square root of the value. +/// @param[in] x The value. +/// @return The square root of the vlaue. +float rcSqrt(float x); + +/// @} +/// @name Vector helper functions. +/// @{ + +/// Derives the cross product of two vectors. (@p v1 x @p v2) +/// @param[out] dest The cross product. [(x, y, z)] +/// @param[in] v1 A Vector [(x, y, z)] +/// @param[in] v2 A vector [(x, y, z)] +inline void rcVcross(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[1]*v2[2] - v1[2]*v2[1]; + dest[1] = v1[2]*v2[0] - v1[0]*v2[2]; + dest[2] = v1[0]*v2[1] - v1[1]*v2[0]; +} + +/// Derives the dot product of two vectors. (@p v1 . @p v2) +/// @param[in] v1 A Vector [(x, y, z)] +/// @param[in] v2 A vector [(x, y, z)] +/// @return The dot product. +inline float rcVdot(const float* v1, const float* v2) +{ + return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]; +} + +/// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s)) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v1 The base vector. [(x, y, z)] +/// @param[in] v2 The vector to scale and add to @p v1. [(x, y, z)] +/// @param[in] s The amount to scale @p v2 by before adding to @p v1. +inline void rcVmad(float* dest, const float* v1, const float* v2, const float s) +{ + dest[0] = v1[0]+v2[0]*s; + dest[1] = v1[1]+v2[1]*s; + dest[2] = v1[2]+v2[2]*s; +} + +/// Performs a vector addition. (@p v1 + @p v2) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v1 The base vector. [(x, y, z)] +/// @param[in] v2 The vector to add to @p v1. [(x, y, z)] +inline void rcVadd(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[0]+v2[0]; + dest[1] = v1[1]+v2[1]; + dest[2] = v1[2]+v2[2]; +} + +/// Performs a vector subtraction. (@p v1 - @p v2) +/// @param[out] dest The result vector. [(x, y, z)] +/// @param[in] v1 The base vector. [(x, y, z)] +/// @param[in] v2 The vector to subtract from @p v1. [(x, y, z)] +inline void rcVsub(float* dest, const float* v1, const float* v2) +{ + dest[0] = v1[0]-v2[0]; + dest[1] = v1[1]-v2[1]; + dest[2] = v1[2]-v2[2]; +} + +/// Selects the minimum value of each element from the specified vectors. +/// @param[in,out] mn A vector. (Will be updated with the result.) [(x, y, z)] +/// @param[in] v A vector. [(x, y, z)] +inline void rcVmin(float* mn, const float* v) +{ + mn[0] = rcMin(mn[0], v[0]); + mn[1] = rcMin(mn[1], v[1]); + mn[2] = rcMin(mn[2], v[2]); +} + +/// Selects the maximum value of each element from the specified vectors. +/// @param[in,out] mx A vector. (Will be updated with the result.) [(x, y, z)] +/// @param[in] v A vector. [(x, y, z)] +inline void rcVmax(float* mx, const float* v) +{ + mx[0] = rcMax(mx[0], v[0]); + mx[1] = rcMax(mx[1], v[1]); + mx[2] = rcMax(mx[2], v[2]); +} + +/// Performs a vector copy. +/// @param[out] dest The result. [(x, y, z)] +/// @param[in] v The vector to copy. [(x, y, z)] +inline void rcVcopy(float* dest, const float* v) +{ + dest[0] = v[0]; + dest[1] = v[1]; + dest[2] = v[2]; +} + +/// Returns the distance between two points. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @return The distance between the two points. +inline float rcVdist(const float* v1, const float* v2) +{ + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return rcSqrt(dx*dx + dy*dy + dz*dz); +} + +/// Returns the square of the distance between two points. +/// @param[in] v1 A point. [(x, y, z)] +/// @param[in] v2 A point. [(x, y, z)] +/// @return The square of the distance between the two points. +inline float rcVdistSqr(const float* v1, const float* v2) +{ + float dx = v2[0] - v1[0]; + float dy = v2[1] - v1[1]; + float dz = v2[2] - v1[2]; + return dx*dx + dy*dy + dz*dz; +} + +/// Normalizes the vector. +/// @param[in,out] v The vector to normalize. [(x, y, z)] +inline void rcVnormalize(float* v) +{ + float d = 1.0f / rcSqrt(rcSqr(v[0]) + rcSqr(v[1]) + rcSqr(v[2])); + v[0] *= d; + v[1] *= d; + v[2] *= d; +} + +/// @} +/// @name Heightfield Functions +/// @see rcHeightfield +/// @{ + +/// Calculates the bounding box of an array of vertices. +/// @ingroup recast +/// @param[in] verts An array of vertices. [(x, y, z) * @p nv] +/// @param[in] nv The number of vertices in the @p verts array. +/// @param[out] bmin The minimum bounds of the AABB. [(x, y, z)] [Units: wu] +/// @param[out] bmax The maximum bounds of the AABB. [(x, y, z)] [Units: wu] +void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax); + +/// Calculates the grid size based on the bounding box and grid cell size. +/// @ingroup recast +/// @param[in] bmin The minimum bounds of the AABB. [(x, y, z)] [Units: wu] +/// @param[in] bmax The maximum bounds of the AABB. [(x, y, z)] [Units: wu] +/// @param[in] cs The xz-plane cell size. [Limit: > 0] [Units: wu] +/// @param[out] w The width along the x-axis. [Limit: >= 0] [Units: vx] +/// @param[out] h The height along the z-axis. [Limit: >= 0] [Units: vx] +void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h); + +/// Initializes a new heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in,out] hf The allocated heightfield to initialize. +/// @param[in] width The width of the field along the x-axis. [Limit: >= 0] [Units: vx] +/// @param[in] height The height of the field along the z-axis. [Limit: >= 0] [Units: vx] +/// @param[in] bmin The minimum bounds of the field's AABB. [(x, y, z)] [Units: wu] +/// @param[in] bmax The maximum bounds of the field's AABB. [(x, y, z)] [Units: wu] +/// @param[in] cs The xz-plane cell size to use for the field. [Limit: > 0] [Units: wu] +/// @param[in] ch The y-axis cell size to use for field. [Limit: > 0] [Units: wu] +/// @returns True if the operation completed successfully. +bool rcCreateHeightfield(rcContext* ctx, rcHeightfield& hf, int width, int height, + const float* bmin, const float* bmax, + float cs, float ch); + +/// Sets the area id of all triangles with a slope below the specified value +/// to #RC_WALKABLE_AREA. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] walkableSlopeAngle The maximum slope that is considered walkable. +/// [Limits: 0 <= value < 90] [Units: Degrees] +/// @param[in] verts The vertices. [(x, y, z) * @p nv] +/// @param[in] nv The number of vertices. +/// @param[in] tris The triangle vertex indices. [(vertA, vertB, vertC) * @p nt] +/// @param[in] nt The number of triangles. +/// @param[out] areas The triangle area ids. [Length: >= @p nt] +void rcMarkWalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, const float* verts, int nv, + const int* tris, int nt, unsigned char* areas); + +/// Sets the area id of all triangles with a slope greater than or equal to the specified value to #RC_NULL_AREA. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] walkableSlopeAngle The maximum slope that is considered walkable. +/// [Limits: 0 <= value < 90] [Units: Degrees] +/// @param[in] verts The vertices. [(x, y, z) * @p nv] +/// @param[in] nv The number of vertices. +/// @param[in] tris The triangle vertex indices. [(vertA, vertB, vertC) * @p nt] +/// @param[in] nt The number of triangles. +/// @param[out] areas The triangle area ids. [Length: >= @p nt] +void rcClearUnwalkableTriangles(rcContext* ctx, const float walkableSlopeAngle, const float* verts, int nv, + const int* tris, int nt, unsigned char* areas); + +/// Adds a span to the specified heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in,out] hf An initialized heightfield. +/// @param[in] x The width index where the span is to be added. +/// [Limits: 0 <= value < rcHeightfield::width] +/// @param[in] y The height index where the span is to be added. +/// [Limits: 0 <= value < rcHeightfield::height] +/// @param[in] smin The minimum height of the span. [Limit: < @p smax] [Units: vx] +/// @param[in] smax The maximum height of the span. [Limit: <= #RC_SPAN_MAX_HEIGHT] [Units: vx] +/// @param[in] area The area id of the span. [Limit: <= #RC_WALKABLE_AREA) +/// @param[in] flagMergeThr The merge theshold. [Limit: >= 0] [Units: vx] +/// @returns True if the operation completed successfully. +bool rcAddSpan(rcContext* ctx, rcHeightfield& hf, const int x, const int y, + const unsigned short smin, const unsigned short smax, + const unsigned char area, const int flagMergeThr); + +/// Rasterizes a triangle into the specified heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] v0 Triangle vertex 0 [(x, y, z)] +/// @param[in] v1 Triangle vertex 1 [(x, y, z)] +/// @param[in] v2 Triangle vertex 2 [(x, y, z)] +/// @param[in] area The area id of the triangle. [Limit: <= #RC_WALKABLE_AREA] +/// @param[in,out] solid An initialized heightfield. +/// @param[in] flagMergeThr The distance where the walkable flag is favored over the non-walkable flag. +/// [Limit: >= 0] [Units: vx] +/// @returns True if the operation completed successfully. +bool rcRasterizeTriangle(rcContext* ctx, const float* v0, const float* v1, const float* v2, + const unsigned char area, rcHeightfield& solid, + const int flagMergeThr = 1); + +/// Rasterizes an indexed triangle mesh into the specified heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] verts The vertices. [(x, y, z) * @p nv] +/// @param[in] nv The number of vertices. +/// @param[in] tris The triangle indices. [(vertA, vertB, vertC) * @p nt] +/// @param[in] areas The area id's of the triangles. [Limit: <= #RC_WALKABLE_AREA] [Size: @p nt] +/// @param[in] nt The number of triangles. +/// @param[in,out] solid An initialized heightfield. +/// @param[in] flagMergeThr The distance where the walkable flag is favored over the non-walkable flag. +/// [Limit: >= 0] [Units: vx] +/// @returns True if the operation completed successfully. +bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int nv, + const int* tris, const unsigned char* areas, const int nt, + rcHeightfield& solid, const int flagMergeThr = 1); + +/// Rasterizes an indexed triangle mesh into the specified heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] verts The vertices. [(x, y, z) * @p nv] +/// @param[in] nv The number of vertices. +/// @param[in] tris The triangle indices. [(vertA, vertB, vertC) * @p nt] +/// @param[in] areas The area id's of the triangles. [Limit: <= #RC_WALKABLE_AREA] [Size: @p nt] +/// @param[in] nt The number of triangles. +/// @param[in,out] solid An initialized heightfield. +/// @param[in] flagMergeThr The distance where the walkable flag is favored over the non-walkable flag. +/// [Limit: >= 0] [Units: vx] +/// @returns True if the operation completed successfully. +bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int nv, + const unsigned short* tris, const unsigned char* areas, const int nt, + rcHeightfield& solid, const int flagMergeThr = 1); + +/// Rasterizes triangles into the specified heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] verts The triangle vertices. [(ax, ay, az, bx, by, bz, cx, by, cx) * @p nt] +/// @param[in] areas The area id's of the triangles. [Limit: <= #RC_WALKABLE_AREA] [Size: @p nt] +/// @param[in] nt The number of triangles. +/// @param[in,out] solid An initialized heightfield. +/// @param[in] flagMergeThr The distance where the walkable flag is favored over the non-walkable flag. +/// [Limit: >= 0] [Units: vx] +/// @returns True if the operation completed successfully. +bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const unsigned char* areas, const int nt, + rcHeightfield& solid, const int flagMergeThr = 1); + +/// Marks non-walkable spans as walkable if their maximum is within @p walkableClimp of a walkable neighbor. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] walkableClimb Maximum ledge height that is considered to still be traversable. +/// [Limit: >=0] [Units: vx] +/// @param[in,out] solid A fully built heightfield. (All spans have been added.) +void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb, rcHeightfield& solid); + +/// Marks spans that are ledges as not-walkable. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] walkableHeight Minimum floor to 'ceiling' height that will still allow the floor area to +/// be considered walkable. [Limit: >= 3] [Units: vx] +/// @param[in] walkableClimb Maximum ledge height that is considered to still be traversable. +/// [Limit: >=0] [Units: vx] +/// @param[in,out] solid A fully built heightfield. (All spans have been added.) +void rcFilterLedgeSpans(rcContext* ctx, const int walkableHeight, + const int walkableClimb, rcHeightfield& solid); + +/// Marks walkable spans as not walkable if the clearence above the span is less than the specified height. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] walkableHeight Minimum floor to 'ceiling' height that will still allow the floor area to +/// be considered walkable. [Limit: >= 3] [Units: vx] +/// @param[in,out] solid A fully built heightfield. (All spans have been added.) +void rcFilterWalkableLowHeightSpans(rcContext* ctx, int walkableHeight, rcHeightfield& solid); + +/// Returns the number of spans contained in the specified heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] hf An initialized heightfield. +/// @returns The number of spans in the heightfield. +int rcGetHeightFieldSpanCount(rcContext* ctx, rcHeightfield& hf); + +/// @} +/// @name Compact Heightfield Functions +/// @see rcCompactHeightfield +/// @{ + +/// Builds a compact heightfield representing open space, from a heightfield representing solid space. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] walkableHeight Minimum floor to 'ceiling' height that will still allow the floor area +/// to be considered walkable. [Limit: >= 3] [Units: vx] +/// @param[in] walkableClimb Maximum ledge height that is considered to still be traversable. +/// [Limit: >=0] [Units: vx] +/// @param[in] hf The heightfield to be compacted. +/// @param[out] chf The resulting compact heightfield. (Must be pre-allocated.) +/// @returns True if the operation completed successfully. +bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const int walkableClimb, + rcHeightfield& hf, rcCompactHeightfield& chf); + +/// Erodes the walkable area within the heightfield by the specified radius. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] radius The radius of erosion. [Limits: 0 < value < 255] [Units: vx] +/// @param[in,out] chf The populated compact heightfield to erode. +/// @returns True if the operation completed successfully. +bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf); + +/// Applies a median filter to walkable area types (based on area id), removing noise. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in,out] chf A populated compact heightfield. +/// @returns True if the operation completed successfully. +bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf); + +/// Applies an area id to all spans within the specified bounding box. (AABB) +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] bmin The minimum of the bounding box. [(x, y, z)] +/// @param[in] bmax The maximum of the bounding box. [(x, y, z)] +/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] +/// @param[in,out] chf A populated compact heightfield. +void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId, + rcCompactHeightfield& chf); + +/// Applies the area id to the all spans within the specified convex polygon. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] verts The vertices of the polygon [Fomr: (x, y, z) * @p nverts] +/// @param[in] nverts The number of vertices in the polygon. +/// @param[in] hmin The height of the base of the polygon. +/// @param[in] hmax The height of the top of the polygon. +/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] +/// @param[in,out] chf A populated compact heightfield. +void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts, + const float hmin, const float hmax, unsigned char areaId, + rcCompactHeightfield& chf); + +/// Helper function to offset voncex polygons for rcMarkConvexPolyArea. +/// @ingroup recast +/// @param[in] verts The vertices of the polygon [Form: (x, y, z) * @p nverts] +/// @param[in] nverts The number of vertices in the polygon. +/// @param[out] outVerts The offset vertices (should hold up to 2 * @p nverts) [Form: (x, y, z) * return value] +/// @param[in] maxOutVerts The max number of vertices that can be stored to @p outVerts. +/// @returns Number of vertices in the offset polygon or 0 if too few vertices in @p outVerts. +int rcOffsetPoly(const float* verts, const int nverts, const float offset, + float* outVerts, const int maxOutVerts); + +/// Applies the area id to all spans within the specified cylinder. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] pos The center of the base of the cylinder. [Form: (x, y, z)] +/// @param[in] r The radius of the cylinder. +/// @param[in] h The height of the cylinder. +/// @param[in] areaId The area id to apply. [Limit: <= #RC_WALKABLE_AREA] +/// @param[in,out] chf A populated compact heightfield. +void rcMarkCylinderArea(rcContext* ctx, const float* pos, + const float r, const float h, unsigned char areaId, + rcCompactHeightfield& chf); + +/// Builds the distance field for the specified compact heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in,out] chf A populated compact heightfield. +/// @returns True if the operation completed successfully. +bool rcBuildDistanceField(rcContext* ctx, rcCompactHeightfield& chf); + +/// Builds region data for the heightfield using watershed partitioning. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in,out] chf A populated compact heightfield. +/// @param[in] borderSize The size of the non-navigable border around the heightfield. +/// [Limit: >=0] [Units: vx] +/// @param[in] minRegionArea The minimum number of cells allowed to form isolated island areas. +/// [Limit: >=0] [Units: vx]. +/// @param[in] mergeRegionArea Any regions with a span count smaller than this value will, if possible, +/// be merged with larger regions. [Limit: >=0] [Units: vx] +/// @returns True if the operation completed successfully. +bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf, + const int borderSize, const int minRegionArea, const int mergeRegionArea); + +/// Builds region data for the heightfield by partitioning the heightfield in non-overlapping layers. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in,out] chf A populated compact heightfield. +/// @param[in] borderSize The size of the non-navigable border around the heightfield. +/// [Limit: >=0] [Units: vx] +/// @param[in] minRegionArea The minimum number of cells allowed to form isolated island areas. +/// [Limit: >=0] [Units: vx]. +/// @returns True if the operation completed successfully. +bool rcBuildLayerRegions(rcContext* ctx, rcCompactHeightfield& chf, + const int borderSize, const int minRegionArea); + +/// Builds region data for the heightfield using simple monotone partitioning. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in,out] chf A populated compact heightfield. +/// @param[in] borderSize The size of the non-navigable border around the heightfield. +/// [Limit: >=0] [Units: vx] +/// @param[in] minRegionArea The minimum number of cells allowed to form isolated island areas. +/// [Limit: >=0] [Units: vx]. +/// @param[in] mergeRegionArea Any regions with a span count smaller than this value will, if possible, +/// be merged with larger regions. [Limit: >=0] [Units: vx] +/// @returns True if the operation completed successfully. +bool rcBuildRegionsMonotone(rcContext* ctx, rcCompactHeightfield& chf, + const int borderSize, const int minRegionArea, const int mergeRegionArea); + +/// Sets the neighbor connection data for the specified direction. +/// @param[in] s The span to update. +/// @param[in] dir The direction to set. [Limits: 0 <= value < 4] +/// @param[in] i The index of the neighbor span. +inline void rcSetCon(rcCompactSpan& s, int dir, int i) +{ + const unsigned int shift = (unsigned int)dir*6; + unsigned int con = s.con; + s.con = (con & ~(0x3f << shift)) | (((unsigned int)i & 0x3f) << shift); +} + +/// Gets neighbor connection data for the specified direction. +/// @param[in] s The span to check. +/// @param[in] dir The direction to check. [Limits: 0 <= value < 4] +/// @return The neighbor connection data for the specified direction, +/// or #RC_NOT_CONNECTED if there is no connection. +inline int rcGetCon(const rcCompactSpan& s, int dir) +{ + const unsigned int shift = (unsigned int)dir*6; + return (s.con >> shift) & 0x3f; +} + +/// Gets the standard width (x-axis) offset for the specified direction. +/// @param[in] dir The direction. [Limits: 0 <= value < 4] +/// @return The width offset to apply to the current cell position to move +/// in the direction. +inline int rcGetDirOffsetX(int dir) +{ + static const int offset[4] = { -1, 0, 1, 0, }; + return offset[dir&0x03]; +} + +/// Gets the standard height (z-axis) offset for the specified direction. +/// @param[in] dir The direction. [Limits: 0 <= value < 4] +/// @return The height offset to apply to the current cell position to move +/// in the direction. +inline int rcGetDirOffsetY(int dir) +{ + static const int offset[4] = { 0, 1, 0, -1 }; + return offset[dir&0x03]; +} + +/// Gets the direction for the specified offset. One of x and y should be 0. +/// @param[in] x The x offset. [Limits: -1 <= value <= 1] +/// @param[in] y The y offset. [Limits: -1 <= value <= 1] +/// @return The direction that represents the offset. +inline int rcGetDirForOffset(int x, int y) +{ + static const int dirs[5] = { 3, 0, -1, 2, 1 }; + return dirs[((y+1)<<1)+x]; +} + +/// @} +/// @name Layer, Contour, Polymesh, and Detail Mesh Functions +/// @see rcHeightfieldLayer, rcContourSet, rcPolyMesh, rcPolyMeshDetail +/// @{ + +/// Builds a layer set from the specified compact heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] chf A fully built compact heightfield. +/// @param[in] borderSize The size of the non-navigable border around the heightfield. [Limit: >=0] +/// [Units: vx] +/// @param[in] walkableHeight Minimum floor to 'ceiling' height that will still allow the floor area +/// to be considered walkable. [Limit: >= 3] [Units: vx] +/// @param[out] lset The resulting layer set. (Must be pre-allocated.) +/// @returns True if the operation completed successfully. +bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, + const int borderSize, const int walkableHeight, + rcHeightfieldLayerSet& lset); + +/// Builds a contour set from the region outlines in the provided compact heightfield. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] chf A fully built compact heightfield. +/// @param[in] maxError The maximum distance a simplfied contour's border edges should deviate +/// the original raw contour. [Limit: >=0] [Units: wu] +/// @param[in] maxEdgeLen The maximum allowed length for contour edges along the border of the mesh. +/// [Limit: >=0] [Units: vx] +/// @param[out] cset The resulting contour set. (Must be pre-allocated.) +/// @param[in] buildFlags The build flags. (See: #rcBuildContoursFlags) +/// @returns True if the operation completed successfully. +bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, + const float maxError, const int maxEdgeLen, + rcContourSet& cset, const int buildFlags = RC_CONTOUR_TESS_WALL_EDGES); + +/// Builds a polygon mesh from the provided contours. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] cset A fully built contour set. +/// @param[in] nvp The maximum number of vertices allowed for polygons generated during the +/// contour to polygon conversion process. [Limit: >= 3] +/// @param[out] mesh The resulting polygon mesh. (Must be re-allocated.) +/// @returns True if the operation completed successfully. +bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, const int nvp, rcPolyMesh& mesh); + +/// Merges multiple polygon meshes into a single mesh. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] meshes An array of polygon meshes to merge. [Size: @p nmeshes] +/// @param[in] nmeshes The number of polygon meshes in the meshes array. +/// @param[in] mesh The resulting polygon mesh. (Must be pre-allocated.) +/// @returns True if the operation completed successfully. +bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, rcPolyMesh& mesh); + +/// Builds a detail mesh from the provided polygon mesh. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] mesh A fully built polygon mesh. +/// @param[in] chf The compact heightfield used to build the polygon mesh. +/// @param[in] sampleDist Sets the distance to use when samping the heightfield. [Limit: >=0] [Units: wu] +/// @param[in] sampleMaxError The maximum distance the detail mesh surface should deviate from +/// heightfield data. [Limit: >=0] [Units: wu] +/// @param[out] dmesh The resulting detail mesh. (Must be pre-allocated.) +/// @returns True if the operation completed successfully. +bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompactHeightfield& chf, + const float sampleDist, const float sampleMaxError, + rcPolyMeshDetail& dmesh); + +/// Copies the poly mesh data from src to dst. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] src The source mesh to copy from. +/// @param[out] dst The resulting detail mesh. (Must be pre-allocated, must be empty mesh.) +/// @returns True if the operation completed successfully. +bool rcCopyPolyMesh(rcContext* ctx, const rcPolyMesh& src, rcPolyMesh& dst); + +/// Merges multiple detail meshes into a single detail mesh. +/// @ingroup recast +/// @param[in,out] ctx The build context to use during the operation. +/// @param[in] meshes An array of detail meshes to merge. [Size: @p nmeshes] +/// @param[in] nmeshes The number of detail meshes in the meshes array. +/// @param[out] mesh The resulting detail mesh. (Must be pre-allocated.) +/// @returns True if the operation completed successfully. +bool rcMergePolyMeshDetails(rcContext* ctx, rcPolyMeshDetail** meshes, const int nmeshes, rcPolyMeshDetail& mesh); + +/// @} + +#endif // RECAST_H + +/////////////////////////////////////////////////////////////////////////// + +// Due to the large amount of detail documentation for this file, +// the content normally located at the end of the header file has been separated +// out to a file in /Docs/Extern. diff --git a/lib/Recast/RecastAlloc.cpp b/lib/Recast/RecastAlloc.cpp new file mode 100644 index 0000000..453b5fa --- /dev/null +++ b/lib/Recast/RecastAlloc.cpp @@ -0,0 +1,86 @@ +// +// Copyright (c) 2009-2010 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 +#include +#include "RecastAlloc.h" +#include "RecastAssert.h" + +static void *rcAllocDefault(size_t size, rcAllocHint) +{ + return malloc(size); +} + +static void rcFreeDefault(void *ptr) +{ + free(ptr); +} + +static rcAllocFunc* sRecastAllocFunc = rcAllocDefault; +static rcFreeFunc* sRecastFreeFunc = rcFreeDefault; + +/// @see rcAlloc, rcFree +void rcAllocSetCustom(rcAllocFunc *allocFunc, rcFreeFunc *freeFunc) +{ + sRecastAllocFunc = allocFunc ? allocFunc : rcAllocDefault; + sRecastFreeFunc = freeFunc ? freeFunc : rcFreeDefault; +} + +/// @see rcAllocSetCustom +void* rcAlloc(size_t size, rcAllocHint hint) +{ + return sRecastAllocFunc(size, hint); +} + +/// @par +/// +/// @warning This function leaves the value of @p ptr unchanged. So it still +/// points to the same (now invalid) location, and not to null. +/// +/// @see rcAllocSetCustom +void rcFree(void* ptr) +{ + if (ptr) + sRecastFreeFunc(ptr); +} + +/// @class rcIntArray +/// +/// While it is possible to pre-allocate a specific array size during +/// construction or by using the #resize method, certain methods will +/// automatically resize the array as needed. +/// +/// @warning The array memory is not initialized to zero when the size is +/// manually set during construction or when using #resize. + +/// @par +/// +/// Using this method ensures the array is at least large enough to hold +/// the specified number of elements. This can improve performance by +/// avoiding auto-resizing during use. +void rcIntArray::doResize(int n) +{ + if (!m_cap) m_cap = n; + while (m_cap < n) m_cap *= 2; + int* newData = (int*)rcAlloc(m_cap*sizeof(int), RC_ALLOC_TEMP); + rcAssert(newData); + if (m_size && newData) memcpy(newData, m_data, m_size*sizeof(int)); + rcFree(m_data); + m_data = newData; +} + diff --git a/lib/Recast/RecastAlloc.h b/lib/Recast/RecastAlloc.h new file mode 100644 index 0000000..3cdd450 --- /dev/null +++ b/lib/Recast/RecastAlloc.h @@ -0,0 +1,146 @@ +// +// Copyright (c) 2009-2010 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. +// + +#ifndef RECASTALLOC_H +#define RECASTALLOC_H + +#include + +/// Provides hint values to the memory allocator on how long the +/// memory is expected to be used. +enum rcAllocHint +{ + RC_ALLOC_PERM, ///< Memory will persist after a function call. + RC_ALLOC_TEMP ///< Memory used temporarily within a function. +}; + +/// A memory allocation function. +// @param[in] size The size, in bytes of memory, to allocate. +// @param[in] rcAllocHint A hint to the allocator on how long the memory is expected to be in use. +// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed. +/// @see rcAllocSetCustom +typedef void* (rcAllocFunc)(size_t size, rcAllocHint hint); + +/// A memory deallocation function. +/// @param[in] ptr A pointer to a memory block previously allocated using #rcAllocFunc. +/// @see rcAllocSetCustom +typedef void (rcFreeFunc)(void* ptr); + +/// Sets the base custom allocation functions to be used by Recast. +/// @param[in] allocFunc The memory allocation function to be used by #rcAlloc +/// @param[in] freeFunc The memory de-allocation function to be used by #rcFree +void rcAllocSetCustom(rcAllocFunc *allocFunc, rcFreeFunc *freeFunc); + +/// Allocates a memory block. +/// @param[in] size The size, in bytes of memory, to allocate. +/// @param[in] hint A hint to the allocator on how long the memory is expected to be in use. +/// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed. +/// @see rcFree +void* rcAlloc(size_t size, rcAllocHint hint); + +/// Deallocates a memory block. +/// @param[in] ptr A pointer to a memory block previously allocated using #rcAlloc. +/// @see rcAlloc +void rcFree(void* ptr); + + +/// A simple dynamic array of integers. +class rcIntArray +{ + int* m_data; + int m_size, m_cap; + + void doResize(int n); + + // Explicitly disabled copy constructor and copy assignment operator. + rcIntArray(const rcIntArray&); + rcIntArray& operator=(const rcIntArray&); + +public: + /// Constructs an instance with an initial array size of zero. + rcIntArray() : m_data(0), m_size(0), m_cap(0) {} + + /// Constructs an instance initialized to the specified size. + /// @param[in] n The initial size of the integer array. + rcIntArray(int n) : m_data(0), m_size(0), m_cap(0) { resize(n); } + ~rcIntArray() { rcFree(m_data); } + + /// Specifies the new size of the integer array. + /// @param[in] n The new size of the integer array. + void resize(int n) + { + if (n > m_cap) + doResize(n); + + m_size = n; + } + + /// Push the specified integer onto the end of the array and increases the size by one. + /// @param[in] item The new value. + void push(int item) { resize(m_size+1); m_data[m_size-1] = item; } + + /// Returns the value at the end of the array and reduces the size by one. + /// @return The value at the end of the array. + int pop() + { + if (m_size > 0) + m_size--; + + return m_data[m_size]; + } + + /// The value at the specified array index. + /// @warning Does not provide overflow protection. + /// @param[in] i The index of the value. + const int& operator[](int i) const { return m_data[i]; } + + /// The value at the specified array index. + /// @warning Does not provide overflow protection. + /// @param[in] i The index of the value. + int& operator[](int i) { return m_data[i]; } + + /// The current size of the integer array. + int size() const { return m_size; } +}; + +/// A simple helper class used to delete an array when it goes out of scope. +/// @note This class is rarely if ever used by the end user. +template class rcScopedDelete +{ + T* ptr; +public: + + /// Constructs an instance with a null pointer. + inline rcScopedDelete() : ptr(0) {} + + /// Constructs an instance with the specified pointer. + /// @param[in] p An pointer to an allocated array. + inline rcScopedDelete(T* p) : ptr(p) {} + inline ~rcScopedDelete() { rcFree(ptr); } + + /// The root array pointer. + /// @return The root array pointer. + inline operator T*() { return ptr; } + +private: + // Explicitly disabled copy constructor and copy assignment operator. + rcScopedDelete(const rcScopedDelete&); + rcScopedDelete& operator=(const rcScopedDelete&); +}; + +#endif diff --git a/lib/Recast/RecastArea.cpp b/lib/Recast/RecastArea.cpp new file mode 100644 index 0000000..97139cf --- /dev/null +++ b/lib/Recast/RecastArea.cpp @@ -0,0 +1,591 @@ +// +// Copyright (c) 2009-2010 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 +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastAssert.h" + +/// @par +/// +/// Basically, any spans that are closer to a boundary or obstruction than the specified radius +/// are marked as unwalkable. +/// +/// This method is usually called immediately after the heightfield has been built. +/// +/// @see rcCompactHeightfield, rcBuildCompactHeightfield, rcConfig::walkableRadius +bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf) +{ + rcAssert(ctx); + + const int w = chf.width; + const int h = chf.height; + + rcScopedTimer timer(ctx, RC_TIMER_ERODE_AREA); + + unsigned char* dist = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP); + if (!dist) + { + ctx->log(RC_LOG_ERROR, "erodeWalkableArea: Out of memory 'dist' (%d).", chf.spanCount); + return false; + } + + // Init distance. + memset(dist, 0xff, sizeof(unsigned char)*chf.spanCount); + + // 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) + { + if (chf.areas[i] == RC_NULL_AREA) + { + dist[i] = 0; + } + else + { + const rcCompactSpan& s = chf.spans[i]; + int nc = 0; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const int nx = x + rcGetDirOffsetX(dir); + const int ny = y + rcGetDirOffsetY(dir); + const int nidx = (int)chf.cells[nx+ny*w].index + rcGetCon(s, dir); + if (chf.areas[nidx] != RC_NULL_AREA) + { + nc++; + } + } + } + // At least one missing neighbour. + if (nc != 4) + dist[i] = 0; + } + } + } + } + + unsigned char nd; + + // 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) != RC_NOT_CONNECTED) + { + // (-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]; + nd = (unsigned char)rcMin((int)dist[ai]+2, 255); + if (nd < dist[i]) + dist[i] = nd; + + // (-1,-1) + if (rcGetCon(as, 3) != RC_NOT_CONNECTED) + { + 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); + nd = (unsigned char)rcMin((int)dist[aai]+3, 255); + if (nd < dist[i]) + dist[i] = nd; + } + } + if (rcGetCon(s, 3) != RC_NOT_CONNECTED) + { + // (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]; + nd = (unsigned char)rcMin((int)dist[ai]+2, 255); + if (nd < dist[i]) + dist[i] = nd; + + // (1,-1) + if (rcGetCon(as, 2) != RC_NOT_CONNECTED) + { + 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); + nd = (unsigned char)rcMin((int)dist[aai]+3, 255); + if (nd < dist[i]) + dist[i] = nd; + } + } + } + } + } + + // 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) != RC_NOT_CONNECTED) + { + // (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]; + nd = (unsigned char)rcMin((int)dist[ai]+2, 255); + if (nd < dist[i]) + dist[i] = nd; + + // (1,1) + if (rcGetCon(as, 1) != RC_NOT_CONNECTED) + { + 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); + nd = (unsigned char)rcMin((int)dist[aai]+3, 255); + if (nd < dist[i]) + dist[i] = nd; + } + } + if (rcGetCon(s, 1) != RC_NOT_CONNECTED) + { + // (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]; + nd = (unsigned char)rcMin((int)dist[ai]+2, 255); + if (nd < dist[i]) + dist[i] = nd; + + // (-1,1) + if (rcGetCon(as, 0) != RC_NOT_CONNECTED) + { + 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); + nd = (unsigned char)rcMin((int)dist[aai]+3, 255); + if (nd < dist[i]) + dist[i] = nd; + } + } + } + } + } + + const unsigned char thr = (unsigned char)(radius*2); + for (int i = 0; i < chf.spanCount; ++i) + if (dist[i] < thr) + chf.areas[i] = RC_NULL_AREA; + + rcFree(dist); + + return true; +} + +static void insertSort(unsigned char* a, const int n) +{ + int i, j; + for (i = 1; i < n; i++) + { + const unsigned char value = a[i]; + for (j = i - 1; j >= 0 && a[j] > value; j--) + a[j+1] = a[j]; + a[j+1] = value; + } +} + +/// @par +/// +/// This filter is usually applied after applying area id's using functions +/// such as #rcMarkBoxArea, #rcMarkConvexPolyArea, and #rcMarkCylinderArea. +/// +/// @see rcCompactHeightfield +bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf) +{ + rcAssert(ctx); + + const int w = chf.width; + const int h = chf.height; + + rcScopedTimer timer(ctx, RC_TIMER_MEDIAN_AREA); + + unsigned char* areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP); + if (!areas) + { + ctx->log(RC_LOG_ERROR, "medianFilterWalkableArea: Out of memory 'areas' (%d).", chf.spanCount); + return false; + } + + // Init distance. + memset(areas, 0xff, sizeof(unsigned char)*chf.spanCount); + + 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 (chf.areas[i] == RC_NULL_AREA) + { + areas[i] = chf.areas[i]; + continue; + } + + unsigned char nei[9]; + for (int j = 0; j < 9; ++j) + nei[j] = chf.areas[i]; + + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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 (chf.areas[ai] != RC_NULL_AREA) + nei[dir*2+0] = chf.areas[ai]; + + const rcCompactSpan& as = chf.spans[ai]; + const int dir2 = (dir+1) & 0x3; + if (rcGetCon(as, dir2) != RC_NOT_CONNECTED) + { + 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); + if (chf.areas[ai2] != RC_NULL_AREA) + nei[dir*2+1] = chf.areas[ai2]; + } + } + } + insertSort(nei, 9); + areas[i] = nei[4]; + } + } + } + + memcpy(chf.areas, areas, sizeof(unsigned char)*chf.spanCount); + + rcFree(areas); + + return true; +} + +/// @par +/// +/// The value of spacial parameters are in world units. +/// +/// @see rcCompactHeightfield, rcMedianFilterWalkableArea +void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId, + rcCompactHeightfield& chf) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_MARK_BOX_AREA); + + int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs); + int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch); + int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs); + int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs); + int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch); + int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs); + + if (maxx < 0) return; + if (minx >= chf.width) return; + if (maxz < 0) return; + if (minz >= chf.height) return; + + if (minx < 0) minx = 0; + if (maxx >= chf.width) maxx = chf.width-1; + if (minz < 0) minz = 0; + if (maxz >= chf.height) maxz = chf.height-1; + + for (int z = minz; z <= maxz; ++z) + { + for (int x = minx; x <= maxx; ++x) + { + const rcCompactCell& c = chf.cells[x+z*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + rcCompactSpan& s = chf.spans[i]; + if ((int)s.y >= miny && (int)s.y <= maxy) + { + if (chf.areas[i] != RC_NULL_AREA) + chf.areas[i] = areaId; + } + } + } + } +} + + +static int pointInPoly(int nvert, const float* verts, const float* p) +{ + 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; + } + return c; +} + +/// @par +/// +/// The value of spacial parameters are in world units. +/// +/// The y-values of the polygon vertices are ignored. So the polygon is effectively +/// projected onto the xz-plane at @p hmin, then extruded to @p hmax. +/// +/// @see rcCompactHeightfield, rcMedianFilterWalkableArea +void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts, + const float hmin, const float hmax, unsigned char areaId, + rcCompactHeightfield& chf) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_MARK_CONVEXPOLY_AREA); + + float bmin[3], bmax[3]; + rcVcopy(bmin, verts); + rcVcopy(bmax, verts); + for (int i = 1; i < nverts; ++i) + { + rcVmin(bmin, &verts[i*3]); + rcVmax(bmax, &verts[i*3]); + } + bmin[1] = hmin; + bmax[1] = hmax; + + int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs); + int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch); + int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs); + int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs); + int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch); + int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs); + + if (maxx < 0) return; + if (minx >= chf.width) return; + if (maxz < 0) return; + if (minz >= chf.height) return; + + if (minx < 0) minx = 0; + if (maxx >= chf.width) maxx = chf.width-1; + if (minz < 0) minz = 0; + if (maxz >= chf.height) maxz = chf.height-1; + + + // TODO: Optimize. + for (int z = minz; z <= maxz; ++z) + { + for (int x = minx; x <= maxx; ++x) + { + const rcCompactCell& c = chf.cells[x+z*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + rcCompactSpan& s = chf.spans[i]; + if (chf.areas[i] == RC_NULL_AREA) + continue; + if ((int)s.y >= miny && (int)s.y <= maxy) + { + float p[3]; + p[0] = chf.bmin[0] + (x+0.5f)*chf.cs; + p[1] = 0; + p[2] = chf.bmin[2] + (z+0.5f)*chf.cs; + + if (pointInPoly(nverts, verts, p)) + { + chf.areas[i] = areaId; + } + } + } + } + } +} + +int rcOffsetPoly(const float* verts, const int nverts, const float offset, + float* outVerts, const int maxOutVerts) +{ + const float MITER_LIMIT = 1.20f; + + int n = 0; + + for (int i = 0; i < nverts; i++) + { + const int a = (i+nverts-1) % nverts; + const int b = i; + const int c = (i+1) % nverts; + const float* va = &verts[a*3]; + const float* vb = &verts[b*3]; + const float* vc = &verts[c*3]; + float dx0 = vb[0] - va[0]; + float dy0 = vb[2] - va[2]; + float d0 = dx0*dx0 + dy0*dy0; + if (d0 > 1e-6f) + { + d0 = 1.0f/rcSqrt(d0); + dx0 *= d0; + dy0 *= d0; + } + float dx1 = vc[0] - vb[0]; + float dy1 = vc[2] - vb[2]; + float d1 = dx1*dx1 + dy1*dy1; + if (d1 > 1e-6f) + { + d1 = 1.0f/rcSqrt(d1); + dx1 *= d1; + dy1 *= d1; + } + const float dlx0 = -dy0; + const float dly0 = dx0; + const float dlx1 = -dy1; + const float dly1 = dx1; + float cross = dx1*dy0 - dx0*dy1; + float dmx = (dlx0 + dlx1) * 0.5f; + float dmy = (dly0 + dly1) * 0.5f; + float dmr2 = dmx*dmx + dmy*dmy; + bool bevel = dmr2 * MITER_LIMIT*MITER_LIMIT < 1.0f; + if (dmr2 > 1e-6f) + { + const float scale = 1.0f / dmr2; + dmx *= scale; + dmy *= scale; + } + + if (bevel && cross < 0.0f) + { + if (n+2 >= maxOutVerts) + return 0; + float d = (1.0f - (dx0*dx1 + dy0*dy1))*0.5f; + outVerts[n*3+0] = vb[0] + (-dlx0+dx0*d)*offset; + outVerts[n*3+1] = vb[1]; + outVerts[n*3+2] = vb[2] + (-dly0+dy0*d)*offset; + n++; + outVerts[n*3+0] = vb[0] + (-dlx1-dx1*d)*offset; + outVerts[n*3+1] = vb[1]; + outVerts[n*3+2] = vb[2] + (-dly1-dy1*d)*offset; + n++; + } + else + { + if (n+1 >= maxOutVerts) + return 0; + outVerts[n*3+0] = vb[0] - dmx*offset; + outVerts[n*3+1] = vb[1]; + outVerts[n*3+2] = vb[2] - dmy*offset; + n++; + } + } + + return n; +} + + +/// @par +/// +/// The value of spacial parameters are in world units. +/// +/// @see rcCompactHeightfield, rcMedianFilterWalkableArea +void rcMarkCylinderArea(rcContext* ctx, const float* pos, + const float r, const float h, unsigned char areaId, + rcCompactHeightfield& chf) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_MARK_CYLINDER_AREA); + + float bmin[3], bmax[3]; + bmin[0] = pos[0] - r; + bmin[1] = pos[1]; + bmin[2] = pos[2] - r; + bmax[0] = pos[0] + r; + bmax[1] = pos[1] + h; + bmax[2] = pos[2] + r; + const float r2 = r*r; + + int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs); + int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch); + int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs); + int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs); + int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch); + int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs); + + if (maxx < 0) return; + if (minx >= chf.width) return; + if (maxz < 0) return; + if (minz >= chf.height) return; + + if (minx < 0) minx = 0; + if (maxx >= chf.width) maxx = chf.width-1; + if (minz < 0) minz = 0; + if (maxz >= chf.height) maxz = chf.height-1; + + + for (int z = minz; z <= maxz; ++z) + { + for (int x = minx; x <= maxx; ++x) + { + const rcCompactCell& c = chf.cells[x+z*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + rcCompactSpan& s = chf.spans[i]; + + if (chf.areas[i] == RC_NULL_AREA) + continue; + + if ((int)s.y >= miny && (int)s.y <= maxy) + { + const float sx = chf.bmin[0] + (x+0.5f)*chf.cs; + const float sz = chf.bmin[2] + (z+0.5f)*chf.cs; + const float dx = sx - pos[0]; + const float dz = sz - pos[2]; + + if (dx*dx + dz*dz < r2) + { + chf.areas[i] = areaId; + } + } + } + } + } +} diff --git a/lib/Recast/RecastAssert.cpp b/lib/Recast/RecastAssert.cpp new file mode 100644 index 0000000..6297d42 --- /dev/null +++ b/lib/Recast/RecastAssert.cpp @@ -0,0 +1,35 @@ +// +// Copyright (c) 2009-2010 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 "RecastAssert.h" + +#ifndef NDEBUG + +static rcAssertFailFunc* sRecastAssertFailFunc = 0; + +void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc) +{ + sRecastAssertFailFunc = assertFailFunc; +} + +rcAssertFailFunc* rcAssertFailGetCustom() +{ + return sRecastAssertFailFunc; +} + +#endif diff --git a/lib/Recast/RecastAssert.h b/lib/Recast/RecastAssert.h new file mode 100644 index 0000000..e7cc10e --- /dev/null +++ b/lib/Recast/RecastAssert.h @@ -0,0 +1,56 @@ +// +// Copyright (c) 2009-2010 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. +// + +#ifndef RECASTASSERT_H +#define RECASTASSERT_H + +// Note: This header file's only purpose is to include define assert. +// Feel free to change the file and include your own implementation instead. + +#ifdef NDEBUG + +// From http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/ +# define rcAssert(x) do { (void)sizeof(x); } while((void)(__LINE__==-1),false) + +#else + +/// An assertion failure function. +// @param[in] expression asserted expression. +// @param[in] file Filename of the failed assertion. +// @param[in] line Line number of the failed assertion. +/// @see rcAssertFailSetCustom +typedef void (rcAssertFailFunc)(const char* expression, const char* file, int line); + +/// Sets the base custom assertion failure function to be used by Recast. +/// @param[in] assertFailFunc The function to be used in case of failure of #dtAssert +void rcAssertFailSetCustom(rcAssertFailFunc *assertFailFunc); + +/// Gets the base custom assertion failure function to be used by Recast. +rcAssertFailFunc* rcAssertFailGetCustom(); + +# include +# define rcAssert(expression) \ + { \ + rcAssertFailFunc* failFunc = rcAssertFailGetCustom(); \ + if(failFunc == NULL) { assert(expression); } \ + else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \ + } + +#endif + +#endif // RECASTASSERT_H diff --git a/lib/Recast/RecastContour.cpp b/lib/Recast/RecastContour.cpp new file mode 100644 index 0000000..277ab01 --- /dev/null +++ b/lib/Recast/RecastContour.cpp @@ -0,0 +1,1105 @@ +// +// Copyright (c) 2009-2010 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 +#include +#include +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastAssert.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 int regs[4] = {0,0,0,0}; + + // Combine region and area codes in order to prevent + // border vertices which are in between two areas to be removed. + regs[0] = chf.spans[i].reg | (chf.areas[i] << 16); + + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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] = chf.spans[ai].reg | (chf.areas[ai] << 16); + if (rcGetCon(as, dirp) != RC_NOT_CONNECTED) + { + 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] = chf.spans[ai2].reg | (chf.areas[ai2] << 16); + } + } + if (rcGetCon(s, dirp) != RC_NOT_CONNECTED) + { + 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] = chf.spans[ai].reg | (chf.areas[ai] << 16); + if (rcGetCon(as, dir) != RC_NOT_CONNECTED) + { + 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] = chf.spans[ai2].reg | (chf.areas[ai2] << 16); + } + } + + // 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 intsSameArea = (regs[c]>>16) == (regs[d]>>16); + const bool noZeros = regs[a] != 0 && regs[b] != 0 && regs[c] != 0 && regs[d] != 0; + if (twoSameExts && twoInts && intsSameArea && 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; + + const unsigned char area = chf.areas[i]; + + int iter = 0; + while (++iter < 40000) + { + if (flags[i] & (1 << dir)) + { + // Choose the edge corner + bool isBorderVertex = false; + bool isAreaBorder = 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) != RC_NOT_CONNECTED) + { + 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 = (int)chf.spans[ai].reg; + if (area != chf.areas[ai]) + isAreaBorder = true; + } + if (isBorderVertex) + r |= RC_BORDER_VERTEX; + if (isAreaBorder) + r |= RC_AREA_BORDER; + 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) != RC_NOT_CONNECTED) + { + 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(const int x, const int z, + const int px, const int pz, + const int qx, const int qz) +{ + 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, + const float maxError, const int maxEdgeLen, const int buildFlags) +{ + // Add initial points. + bool hasConnections = false; + for (int i = 0; i < points.size(); i += 4) + { + if ((points[i+3] & RC_CONTOUR_REG_MASK) != 0) + { + hasConnections = true; + break; + } + } + + if (hasConnections) + { + // 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; + const bool differentRegs = (points[i*4+3] & RC_CONTOUR_REG_MASK) != (points[ii*4+3] & RC_CONTOUR_REG_MASK); + const bool areaBorders = (points[i*4+3] & RC_AREA_BORDER) != (points[ii*4+3] & RC_AREA_BORDER); + if (differentRegs || areaBorders) + { + simplified.push(points[i*4+0]); + simplified.push(points[i*4+1]); + simplified.push(points[i*4+2]); + simplified.push(i); + } + } + } + + if (simplified.size() == 0) + { + // 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); + } + + // 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 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. + float maxd = 0; + int maxi = -1; + int ci, cinc, endi; + + // Traverse the segment in lexilogical order so that the + // max deviation is calculated similarly when traversing + // opposite segments. + if (bx > ax || (bx == ax && bz > az)) + { + cinc = 1; + ci = (ai+cinc) % pn; + endi = bi; + } + else + { + cinc = pn-1; + ci = (bi+cinc) % pn; + endi = ai; + rcSwap(ax, bx); + rcSwap(az, bz); + } + + // Tessellate only outer edges or edges between areas. + if ((points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0 || + (points[ci*4+3] & RC_AREA_BORDER)) + { + while (ci != endi) + { + float d = distancePtSeg(points[ci*4+0], points[ci*4+2], ax, az, bx, bz); + if (d > maxd) + { + maxd = d; + maxi = ci; + } + ci = (ci+cinc) % 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); + const 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 && (buildFlags & (RC_CONTOUR_TESS_WALL_EDGES|RC_CONTOUR_TESS_AREA_EDGES)) != 0) + { + for (int i = 0; i < simplified.size()/4; ) + { + const int ii = (i+1) % (simplified.size()/4); + + const int ax = simplified[i*4+0]; + const int az = simplified[i*4+2]; + const int ai = simplified[i*4+3]; + + const int bx = simplified[ii*4+0]; + const int bz = simplified[ii*4+2]; + const int bi = simplified[ii*4+3]; + + // Find maximum deviation from the segment. + int maxi = -1; + int ci = (ai+1) % pn; + + // Tessellate only outer edges or edges between areas. + bool tess = false; + // Wall edges. + if ((buildFlags & RC_CONTOUR_TESS_WALL_EDGES) && (points[ci*4+3] & RC_CONTOUR_REG_MASK) == 0) + tess = true; + // Edges between areas. + if ((buildFlags & RC_CONTOUR_TESS_AREA_EDGES) && (points[ci*4+3] & RC_AREA_BORDER)) + tess = true; + + if (tess) + { + int dx = bx - ax; + int dz = bz - az; + if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen) + { + // Round based on the segments in lexilogical order so that the + // max tesselation is consistent regardles in which direction + // segments are traversed. + const int n = bi < ai ? (bi+pn - ai) : (bi - ai); + if (n > 1) + { + if (bx > ax || (bx == ax && bz > az)) + maxi = (ai + n/2) % pn; + else + maxi = (ai + (n+1)/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); + const 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] & (RC_CONTOUR_REG_MASK|RC_AREA_BORDER)) | (points[bi*4+3] & RC_BORDER_VERTEX); + } + +} + +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; +} + +// TODO: these are the same as in RecastMesh.cpp, consider using the same. +// Last time I checked the if version got compiled using cmov, which was a lot faster than module (with idiv). +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. +static 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]; +} + +static bool intersectSegCountour(const int* d0, const int* d1, int i, int n, const int* verts) +{ + // 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. + if (i == k || i == k1) + continue; + const int* p0 = &verts[k * 4]; + const int* p1 = &verts[k1 * 4]; + if (vequal(d0, p0) || vequal(d1, p0) || vequal(d0, p1) || vequal(d1, p1)) + continue; + + if (intersect(d0, d1, p0, p1)) + return true; + } + return false; +} + +static bool inCone(int i, int n, const int* verts, const int* pj) +{ + const int* pi = &verts[i * 4]; + const int* pi1 = &verts[next(i, n) * 4]; + const int* pin1 = &verts[prev(i, n) * 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)); +} + + +static void removeDegenerateSegments(rcIntArray& simplified) +{ + // Remove adjacent vertices which are equal on xz-plane, + // or else the triangulator will get confused. + int npts = simplified.size()/4; + for (int i = 0; i < npts; ++i) + { + int ni = next(i, npts); + + if (vequal(&simplified[i*4], &simplified[ni*4])) + { + // 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.resize(simplified.size()-4); + npts--; + } + } +} + + +static bool mergeContours(rcContour& ca, rcContour& cb, int ia, int ib) +{ + const int maxVerts = ca.nverts + cb.nverts + 2; + int* verts = (int*)rcAlloc(sizeof(int)*maxVerts*4, RC_ALLOC_PERM); + 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++; + } + + rcFree(ca.verts); + ca.verts = verts; + ca.nverts = nv; + + rcFree(cb.verts); + cb.verts = 0; + cb.nverts = 0; + + return true; +} + +struct rcContourHole +{ + rcContour* contour; + int minx, minz, leftmost; +}; + +struct rcContourRegion +{ + rcContour* outline; + rcContourHole* holes; + int nholes; +}; + +struct rcPotentialDiagonal +{ + int vert; + int dist; +}; + +// Finds the lowest leftmost vertex of a contour. +static void findLeftMostVertex(rcContour* contour, int* minx, int* minz, int* leftmost) +{ + *minx = contour->verts[0]; + *minz = contour->verts[2]; + *leftmost = 0; + for (int i = 1; i < contour->nverts; i++) + { + const int x = contour->verts[i*4+0]; + const int z = contour->verts[i*4+2]; + if (x < *minx || (x == *minx && z < *minz)) + { + *minx = x; + *minz = z; + *leftmost = i; + } + } +} + +static int compareHoles(const void* va, const void* vb) +{ + const rcContourHole* a = (const rcContourHole*)va; + const rcContourHole* b = (const rcContourHole*)vb; + if (a->minx == b->minx) + { + if (a->minz < b->minz) + return -1; + if (a->minz > b->minz) + return 1; + } + else + { + if (a->minx < b->minx) + return -1; + if (a->minx > b->minx) + return 1; + } + return 0; +} + + +static int compareDiagDist(const void* va, const void* vb) +{ + const rcPotentialDiagonal* a = (const rcPotentialDiagonal*)va; + const rcPotentialDiagonal* b = (const rcPotentialDiagonal*)vb; + if (a->dist < b->dist) + return -1; + if (a->dist > b->dist) + return 1; + return 0; +} + + +static void mergeRegionHoles(rcContext* ctx, rcContourRegion& region) +{ + // Sort holes from left to right. + for (int i = 0; i < region.nholes; i++) + findLeftMostVertex(region.holes[i].contour, ®ion.holes[i].minx, ®ion.holes[i].minz, ®ion.holes[i].leftmost); + + qsort(region.holes, region.nholes, sizeof(rcContourHole), compareHoles); + + int maxVerts = region.outline->nverts; + for (int i = 0; i < region.nholes; i++) + maxVerts += region.holes[i].contour->nverts; + + rcScopedDelete diags((rcPotentialDiagonal*)rcAlloc(sizeof(rcPotentialDiagonal)*maxVerts, RC_ALLOC_TEMP)); + if (!diags) + { + ctx->log(RC_LOG_WARNING, "mergeRegionHoles: Failed to allocated diags %d.", maxVerts); + return; + } + + rcContour* outline = region.outline; + + // Merge holes into the outline one by one. + for (int i = 0; i < region.nholes; i++) + { + rcContour* hole = region.holes[i].contour; + + int index = -1; + int bestVertex = region.holes[i].leftmost; + for (int iter = 0; iter < hole->nverts; iter++) + { + // Find potential diagonals. + // The 'best' vertex must be in the cone described by 3 cosequtive vertices of the outline. + // ..o j-1 + // | + // | * best + // | + // j o-----o j+1 + // : + int ndiags = 0; + const int* corner = &hole->verts[bestVertex*4]; + for (int j = 0; j < outline->nverts; j++) + { + if (inCone(j, outline->nverts, outline->verts, corner)) + { + int dx = outline->verts[j*4+0] - corner[0]; + int dz = outline->verts[j*4+2] - corner[2]; + diags[ndiags].vert = j; + diags[ndiags].dist = dx*dx + dz*dz; + ndiags++; + } + } + // Sort potential diagonals by distance, we want to make the connection as short as possible. + qsort(diags, ndiags, sizeof(rcPotentialDiagonal), compareDiagDist); + + // Find a diagonal that is not intersecting the outline not the remaining holes. + index = -1; + for (int j = 0; j < ndiags; j++) + { + const int* pt = &outline->verts[diags[j].vert*4]; + bool intersect = intersectSegCountour(pt, corner, diags[i].vert, outline->nverts, outline->verts); + for (int k = i; k < region.nholes && !intersect; k++) + intersect |= intersectSegCountour(pt, corner, -1, region.holes[k].contour->nverts, region.holes[k].contour->verts); + if (!intersect) + { + index = diags[j].vert; + break; + } + } + // If found non-intersecting diagonal, stop looking. + if (index != -1) + break; + // All the potential diagonals for the current vertex were intersecting, try next vertex. + bestVertex = (bestVertex + 1) % hole->nverts; + } + + if (index == -1) + { + ctx->log(RC_LOG_WARNING, "mergeHoles: Failed to find merge points for %p and %p.", region.outline, hole); + continue; + } + if (!mergeContours(*region.outline, *hole, index, bestVertex)) + { + ctx->log(RC_LOG_WARNING, "mergeHoles: Failed to merge contours %p and %p.", region.outline, hole); + continue; + } + } +} + + +/// @par +/// +/// The raw contours will match the region outlines exactly. The @p maxError and @p maxEdgeLen +/// parameters control how closely the simplified contours will match the raw contours. +/// +/// Simplified contours are generated such that the vertices for portals between areas match up. +/// (They are considered mandatory vertices.) +/// +/// Setting @p maxEdgeLength to zero will disabled the edge length feature. +/// +/// See the #rcConfig documentation for more information on the configuration parameters. +/// +/// @see rcAllocContourSet, rcCompactHeightfield, rcContourSet, rcConfig +bool rcBuildContours(rcContext* ctx, rcCompactHeightfield& chf, + const float maxError, const int maxEdgeLen, + rcContourSet& cset, const int buildFlags) +{ + rcAssert(ctx); + + const int w = chf.width; + const int h = chf.height; + const int borderSize = chf.borderSize; + + rcScopedTimer timer(ctx, RC_TIMER_BUILD_CONTOURS); + + rcVcopy(cset.bmin, chf.bmin); + rcVcopy(cset.bmax, chf.bmax); + if (borderSize > 0) + { + // If the heightfield was build with bordersize, remove the offset. + const float pad = borderSize*chf.cs; + cset.bmin[0] += pad; + cset.bmin[2] += pad; + cset.bmax[0] -= pad; + cset.bmax[2] -= pad; + } + cset.cs = chf.cs; + cset.ch = chf.ch; + cset.width = chf.width - chf.borderSize*2; + cset.height = chf.height - chf.borderSize*2; + cset.borderSize = chf.borderSize; + cset.maxError = maxError; + + int maxContours = rcMax((int)chf.maxRegions, 8); + cset.conts = (rcContour*)rcAlloc(sizeof(rcContour)*maxContours, RC_ALLOC_PERM); + if (!cset.conts) + return false; + cset.nconts = 0; + + rcScopedDelete flags((unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP)); + if (!flags) + { + ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'flags' (%d).", chf.spanCount); + return false; + } + + ctx->startTimer(RC_TIMER_BUILD_CONTOURS_TRACE); + + // 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 (!chf.spans[i].reg || (chf.spans[i].reg & RC_BORDER_REG)) + { + flags[i] = 0; + continue; + } + for (int dir = 0; dir < 4; ++dir) + { + unsigned short r = 0; + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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); + r = chf.spans[ai].reg; + } + if (r == chf.spans[i].reg) + res |= (1 << dir); + } + flags[i] = res ^ 0xf; // Inverse, mark non connected edges. + } + } + } + + ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE); + + 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; + } + const unsigned short reg = chf.spans[i].reg; + if (!reg || (reg & RC_BORDER_REG)) + continue; + const unsigned char area = chf.areas[i]; + + verts.resize(0); + simplified.resize(0); + + ctx->startTimer(RC_TIMER_BUILD_CONTOURS_TRACE); + walkContour(x, y, i, chf, flags, verts); + ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_TRACE); + + ctx->startTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY); + simplifyContour(verts, simplified, maxError, maxEdgeLen, buildFlags); + removeDegenerateSegments(simplified); + ctx->stopTimer(RC_TIMER_BUILD_CONTOURS_SIMPLIFY); + + + // Store region->contour remap info. + // Create contour. + if (simplified.size()/4 >= 3) + { + if (cset.nconts >= maxContours) + { + // Allocate more contours. + // This happens when a region has holes. + const int oldMax = maxContours; + maxContours *= 2; + rcContour* newConts = (rcContour*)rcAlloc(sizeof(rcContour)*maxContours, RC_ALLOC_PERM); + for (int j = 0; j < cset.nconts; ++j) + { + newConts[j] = cset.conts[j]; + // Reset source pointers to prevent data deletion. + cset.conts[j].verts = 0; + cset.conts[j].rverts = 0; + } + rcFree(cset.conts); + cset.conts = newConts; + + ctx->log(RC_LOG_WARNING, "rcBuildContours: Expanding max contours from %d to %d.", oldMax, maxContours); + } + + rcContour* cont = &cset.conts[cset.nconts++]; + + cont->nverts = simplified.size()/4; + cont->verts = (int*)rcAlloc(sizeof(int)*cont->nverts*4, RC_ALLOC_PERM); + if (!cont->verts) + { + ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'verts' (%d).", cont->nverts); + return false; + } + memcpy(cont->verts, &simplified[0], sizeof(int)*cont->nverts*4); + if (borderSize > 0) + { + // If the heightfield was build with bordersize, remove the offset. + for (int j = 0; j < cont->nverts; ++j) + { + int* v = &cont->verts[j*4]; + v[0] -= borderSize; + v[2] -= borderSize; + } + } + + cont->nrverts = verts.size()/4; + cont->rverts = (int*)rcAlloc(sizeof(int)*cont->nrverts*4, RC_ALLOC_PERM); + if (!cont->rverts) + { + ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'rverts' (%d).", cont->nrverts); + return false; + } + memcpy(cont->rverts, &verts[0], sizeof(int)*cont->nrverts*4); + if (borderSize > 0) + { + // If the heightfield was build with bordersize, remove the offset. + for (int j = 0; j < cont->nrverts; ++j) + { + int* v = &cont->rverts[j*4]; + v[0] -= borderSize; + v[2] -= borderSize; + } + } + + cont->reg = reg; + cont->area = area; + } + } + } + } + + // Merge holes if needed. + if (cset.nconts > 0) + { + // Calculate winding of all polygons. + rcScopedDelete winding((char*)rcAlloc(sizeof(char)*cset.nconts, RC_ALLOC_TEMP)); + if (!winding) + { + ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'hole' (%d).", cset.nconts); + return false; + } + int nholes = 0; + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + // If the contour is wound backwards, it is a hole. + winding[i] = calcAreaOfPolygon2D(cont.verts, cont.nverts) < 0 ? -1 : 1; + if (winding[i] < 0) + nholes++; + } + + if (nholes > 0) + { + // Collect outline contour and holes contours per region. + // We assume that there is one outline and multiple holes. + const int nregions = chf.maxRegions+1; + rcScopedDelete regions((rcContourRegion*)rcAlloc(sizeof(rcContourRegion)*nregions, RC_ALLOC_TEMP)); + if (!regions) + { + ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'regions' (%d).", nregions); + return false; + } + memset(regions, 0, sizeof(rcContourRegion)*nregions); + + rcScopedDelete holes((rcContourHole*)rcAlloc(sizeof(rcContourHole)*cset.nconts, RC_ALLOC_TEMP)); + if (!holes) + { + ctx->log(RC_LOG_ERROR, "rcBuildContours: Out of memory 'holes' (%d).", cset.nconts); + return false; + } + memset(holes, 0, sizeof(rcContourHole)*cset.nconts); + + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + // Positively would contours are outlines, negative holes. + if (winding[i] > 0) + { + if (regions[cont.reg].outline) + ctx->log(RC_LOG_ERROR, "rcBuildContours: Multiple outlines for region %d.", cont.reg); + regions[cont.reg].outline = &cont; + } + else + { + regions[cont.reg].nholes++; + } + } + int index = 0; + for (int i = 0; i < nregions; i++) + { + if (regions[i].nholes > 0) + { + regions[i].holes = &holes[index]; + index += regions[i].nholes; + regions[i].nholes = 0; + } + } + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + rcContourRegion& reg = regions[cont.reg]; + if (winding[i] < 0) + reg.holes[reg.nholes++].contour = &cont; + } + + // Finally merge each regions holes into the outline. + for (int i = 0; i < nregions; i++) + { + rcContourRegion& reg = regions[i]; + if (!reg.nholes) continue; + + if (reg.outline) + { + mergeRegionHoles(ctx, reg); + } + else + { + // The region does not have an outline. + // This can happen if the contour becaomes selfoverlapping because of + // too aggressive simplification settings. + ctx->log(RC_LOG_ERROR, "rcBuildContours: Bad outline for region %d, contour simplification is likely too aggressive.", i); + } + } + } + + } + + return true; +} diff --git a/lib/Recast/RecastFilter.cpp b/lib/Recast/RecastFilter.cpp new file mode 100644 index 0000000..9d3e63c --- /dev/null +++ b/lib/Recast/RecastFilter.cpp @@ -0,0 +1,202 @@ +// +// Copyright (c) 2009-2010 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 +#include +#include "Recast.h" +#include "RecastAssert.h" + +/// @par +/// +/// Allows the formation of walkable regions that will flow over low lying +/// objects such as curbs, and up structures such as stairways. +/// +/// Two neighboring spans are walkable if: rcAbs(currentSpan.smax - neighborSpan.smax) < waklableClimb +/// +/// @warning Will override the effect of #rcFilterLedgeSpans. So if both filters are used, call +/// #rcFilterLedgeSpans after calling this filter. +/// +/// @see rcHeightfield, rcConfig +void rcFilterLowHangingWalkableObstacles(rcContext* ctx, const int walkableClimb, rcHeightfield& solid) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_FILTER_LOW_OBSTACLES); + + const int w = solid.width; + const int h = solid.height; + + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + rcSpan* ps = 0; + bool previousWalkable = false; + unsigned char previousArea = RC_NULL_AREA; + + for (rcSpan* s = solid.spans[x + y*w]; s; ps = s, s = s->next) + { + const bool walkable = s->area != RC_NULL_AREA; + // If current span is not walkable, but there is walkable + // span just below it, mark the span above it walkable too. + if (!walkable && previousWalkable) + { + if (rcAbs((int)s->smax - (int)ps->smax) <= walkableClimb) + s->area = previousArea; + } + // Copy walkable flag so that it cannot propagate + // past multiple non-walkable objects. + previousWalkable = walkable; + previousArea = s->area; + } + } + } +} + +/// @par +/// +/// A ledge is a span with one or more neighbors whose maximum is further away than @p walkableClimb +/// from the current span's maximum. +/// This method removes the impact of the overestimation of conservative voxelization +/// so the resulting mesh will not have regions hanging in the air over ledges. +/// +/// A span is a ledge if: rcAbs(currentSpan.smax - neighborSpan.smax) > walkableClimb +/// +/// @see rcHeightfield, rcConfig +void rcFilterLedgeSpans(rcContext* ctx, const int walkableHeight, const int walkableClimb, + rcHeightfield& solid) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_FILTER_BORDER); + + 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->area == RC_NULL_AREA) + 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; + + // Min and max height of accessible neighbours. + int asmin = s->smax; + int asmax = s->smax; + + 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); + + // Find min/max accessible neighbour height. + if (rcAbs(nbot - bot) <= walkableClimb) + { + if (nbot < asmin) asmin = nbot; + if (nbot > asmax) asmax = nbot; + } + + } + } + } + + // The current span is close to a ledge if the drop to any + // neighbour span is less than the walkableClimb. + if (minh < -walkableClimb) + { + s->area = RC_NULL_AREA; + } + // If the difference between all neighbours is too large, + // we are at steep slope, mark the span as ledge. + else if ((asmax - asmin) > walkableClimb) + { + s->area = RC_NULL_AREA; + } + } + } + } +} + +/// @par +/// +/// For this filter, the clearance above the span is the distance from the span's +/// maximum to the next higher span's minimum. (Same grid column.) +/// +/// @see rcHeightfield, rcConfig +void rcFilterWalkableLowHeightSpans(rcContext* ctx, int walkableHeight, rcHeightfield& solid) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_FILTER_WALKABLE); + + 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->area = RC_NULL_AREA; + } + } + } +} diff --git a/lib/Recast/RecastLayers.cpp b/lib/Recast/RecastLayers.cpp new file mode 100644 index 0000000..acc97e4 --- /dev/null +++ b/lib/Recast/RecastLayers.cpp @@ -0,0 +1,644 @@ +// +// Copyright (c) 2009-2010 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 +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastAssert.h" + + +// Must be 255 or smaller (not 256) because layer IDs are stored as +// a byte where 255 is a special value. +static const int RC_MAX_LAYERS = 63; +static const int RC_MAX_NEIS = 16; + +struct rcLayerRegion +{ + unsigned char layers[RC_MAX_LAYERS]; + unsigned char neis[RC_MAX_NEIS]; + unsigned short ymin, ymax; + unsigned char layerId; // Layer ID + unsigned char nlayers; // Layer count + unsigned char nneis; // Neighbour count + unsigned char base; // Flag indicating if the region is the base of merged regions. +}; + + +static bool contains(const unsigned char* a, const unsigned char an, const unsigned char v) +{ + const int n = (int)an; + for (int i = 0; i < n; ++i) + { + if (a[i] == v) + return true; + } + return false; +} + +static bool addUnique(unsigned char* a, unsigned char& an, int anMax, unsigned char v) +{ + if (contains(a, an, v)) + return true; + + if ((int)an >= anMax) + return false; + + a[an] = v; + an++; + return true; +} + + +inline bool overlapRange(const unsigned short amin, const unsigned short amax, + const unsigned short bmin, const unsigned short bmax) +{ + return (amin > bmax || amax < bmin) ? false : true; +} + + + +struct rcLayerSweepSpan +{ + unsigned short ns; // number samples + unsigned char id; // region id + unsigned char nei; // neighbour id +}; + +/// @par +/// +/// See the #rcConfig documentation for more information on the configuration parameters. +/// +/// @see rcAllocHeightfieldLayerSet, rcCompactHeightfield, rcHeightfieldLayerSet, rcConfig +bool rcBuildHeightfieldLayers(rcContext* ctx, rcCompactHeightfield& chf, + const int borderSize, const int walkableHeight, + rcHeightfieldLayerSet& lset) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_BUILD_LAYERS); + + const int w = chf.width; + const int h = chf.height; + + rcScopedDelete srcReg((unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP)); + if (!srcReg) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'srcReg' (%d).", chf.spanCount); + return false; + } + memset(srcReg,0xff,sizeof(unsigned char)*chf.spanCount); + + const int nsweeps = chf.width; + rcScopedDelete sweeps((rcLayerSweepSpan*)rcAlloc(sizeof(rcLayerSweepSpan)*nsweeps, RC_ALLOC_TEMP)); + if (!sweeps) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'sweeps' (%d).", nsweeps); + return false; + } + + + // Partition walkable area into monotone regions. + int prevCount[256]; + unsigned char regId = 0; + + for (int y = borderSize; y < h-borderSize; ++y) + { + memset(prevCount,0,sizeof(int)*regId); + unsigned char sweepId = 0; + + for (int x = borderSize; x < w-borderSize; ++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 (chf.areas[i] == RC_NULL_AREA) continue; + + unsigned char sid = 0xff; + + // -x + if (rcGetCon(s, 0) != RC_NOT_CONNECTED) + { + 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); + if (chf.areas[ai] != RC_NULL_AREA && srcReg[ai] != 0xff) + sid = srcReg[ai]; + } + + if (sid == 0xff) + { + sid = sweepId++; + sweeps[sid].nei = 0xff; + sweeps[sid].ns = 0; + } + + // -y + if (rcGetCon(s,3) != RC_NOT_CONNECTED) + { + 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 unsigned char nr = srcReg[ai]; + if (nr != 0xff) + { + // Set neighbour when first valid neighbour is encoutered. + if (sweeps[sid].ns == 0) + sweeps[sid].nei = nr; + + if (sweeps[sid].nei == nr) + { + // Update existing neighbour + sweeps[sid].ns++; + prevCount[nr]++; + } + else + { + // This is hit if there is nore than one neighbour. + // Invalidate the neighbour. + sweeps[sid].nei = 0xff; + } + } + } + + srcReg[i] = sid; + } + } + + // Create unique ID. + for (int i = 0; i < sweepId; ++i) + { + // If the neighbour is set and there is only one continuous connection to it, + // the sweep will be merged with the previous one, else new region is created. + if (sweeps[i].nei != 0xff && prevCount[sweeps[i].nei] == (int)sweeps[i].ns) + { + sweeps[i].id = sweeps[i].nei; + } + else + { + if (regId == 255) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Region ID overflow."); + return false; + } + sweeps[i].id = regId++; + } + } + + // Remap local sweep ids to region ids. + for (int x = borderSize; x < w-borderSize; ++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 (srcReg[i] != 0xff) + srcReg[i] = sweeps[srcReg[i]].id; + } + } + } + + // Allocate and init layer regions. + const int nregs = (int)regId; + rcScopedDelete regs((rcLayerRegion*)rcAlloc(sizeof(rcLayerRegion)*nregs, RC_ALLOC_TEMP)); + if (!regs) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'regs' (%d).", nregs); + return false; + } + memset(regs, 0, sizeof(rcLayerRegion)*nregs); + for (int i = 0; i < nregs; ++i) + { + regs[i].layerId = 0xff; + regs[i].ymin = 0xffff; + regs[i].ymax = 0; + } + + // Find region neighbours and overlapping regions. + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + + unsigned char lregs[RC_MAX_LAYERS]; + int nlregs = 0; + + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + const unsigned char ri = srcReg[i]; + if (ri == 0xff) continue; + + regs[ri].ymin = rcMin(regs[ri].ymin, s.y); + regs[ri].ymax = rcMax(regs[ri].ymax, s.y); + + // Collect all region layers. + if (nlregs < RC_MAX_LAYERS) + lregs[nlregs++] = ri; + + // Update neighbours + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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 unsigned char rai = srcReg[ai]; + if (rai != 0xff && rai != ri) + { + // Don't check return value -- if we cannot add the neighbor + // it will just cause a few more regions to be created, which + // is fine. + addUnique(regs[ri].neis, regs[ri].nneis, RC_MAX_NEIS, rai); + } + } + } + + } + + // Update overlapping regions. + for (int i = 0; i < nlregs-1; ++i) + { + for (int j = i+1; j < nlregs; ++j) + { + if (lregs[i] != lregs[j]) + { + rcLayerRegion& ri = regs[lregs[i]]; + rcLayerRegion& rj = regs[lregs[j]]; + + if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, lregs[j]) || + !addUnique(rj.layers, rj.nlayers, RC_MAX_LAYERS, lregs[i])) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS."); + return false; + } + } + } + } + + } + } + + // Create 2D layers from regions. + unsigned char layerId = 0; + + static const int MAX_STACK = 64; + unsigned char stack[MAX_STACK]; + int nstack = 0; + + for (int i = 0; i < nregs; ++i) + { + rcLayerRegion& root = regs[i]; + // Skip already visited. + if (root.layerId != 0xff) + continue; + + // Start search. + root.layerId = layerId; + root.base = 1; + + nstack = 0; + stack[nstack++] = (unsigned char)i; + + while (nstack) + { + // Pop front + rcLayerRegion& reg = regs[stack[0]]; + nstack--; + for (int j = 0; j < nstack; ++j) + stack[j] = stack[j+1]; + + const int nneis = (int)reg.nneis; + for (int j = 0; j < nneis; ++j) + { + const unsigned char nei = reg.neis[j]; + rcLayerRegion& regn = regs[nei]; + // Skip already visited. + if (regn.layerId != 0xff) + continue; + // Skip if the neighbour is overlapping root region. + if (contains(root.layers, root.nlayers, nei)) + continue; + // Skip if the height range would become too large. + const int ymin = rcMin(root.ymin, regn.ymin); + const int ymax = rcMax(root.ymax, regn.ymax); + if ((ymax - ymin) >= 255) + continue; + + if (nstack < MAX_STACK) + { + // Deepen + stack[nstack++] = (unsigned char)nei; + + // Mark layer id + regn.layerId = layerId; + // Merge current layers to root. + for (int k = 0; k < regn.nlayers; ++k) + { + if (!addUnique(root.layers, root.nlayers, RC_MAX_LAYERS, regn.layers[k])) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS."); + return false; + } + } + root.ymin = rcMin(root.ymin, regn.ymin); + root.ymax = rcMax(root.ymax, regn.ymax); + } + } + } + + layerId++; + } + + // Merge non-overlapping regions that are close in height. + const unsigned short mergeHeight = (unsigned short)walkableHeight * 4; + + for (int i = 0; i < nregs; ++i) + { + rcLayerRegion& ri = regs[i]; + if (!ri.base) continue; + + unsigned char newId = ri.layerId; + + for (;;) + { + unsigned char oldId = 0xff; + + for (int j = 0; j < nregs; ++j) + { + if (i == j) continue; + rcLayerRegion& rj = regs[j]; + if (!rj.base) continue; + + // Skip if the regions are not close to each other. + if (!overlapRange(ri.ymin,ri.ymax+mergeHeight, rj.ymin,rj.ymax+mergeHeight)) + continue; + // Skip if the height range would become too large. + const int ymin = rcMin(ri.ymin, rj.ymin); + const int ymax = rcMax(ri.ymax, rj.ymax); + if ((ymax - ymin) >= 255) + continue; + + // Make sure that there is no overlap when merging 'ri' and 'rj'. + bool overlap = false; + // Iterate over all regions which have the same layerId as 'rj' + for (int k = 0; k < nregs; ++k) + { + if (regs[k].layerId != rj.layerId) + continue; + // Check if region 'k' is overlapping region 'ri' + // Index to 'regs' is the same as region id. + if (contains(ri.layers,ri.nlayers, (unsigned char)k)) + { + overlap = true; + break; + } + } + // Cannot merge of regions overlap. + if (overlap) + continue; + + // Can merge i and j. + oldId = rj.layerId; + break; + } + + // Could not find anything to merge with, stop. + if (oldId == 0xff) + break; + + // Merge + for (int j = 0; j < nregs; ++j) + { + rcLayerRegion& rj = regs[j]; + if (rj.layerId == oldId) + { + rj.base = 0; + // Remap layerIds. + rj.layerId = newId; + // Add overlaid layers from 'rj' to 'ri'. + for (int k = 0; k < rj.nlayers; ++k) + { + if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, rj.layers[k])) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS."); + return false; + } + } + + // Update height bounds. + ri.ymin = rcMin(ri.ymin, rj.ymin); + ri.ymax = rcMax(ri.ymax, rj.ymax); + } + } + } + } + + // Compact layerIds + unsigned char remap[256]; + memset(remap, 0, 256); + + // Find number of unique layers. + layerId = 0; + for (int i = 0; i < nregs; ++i) + remap[regs[i].layerId] = 1; + for (int i = 0; i < 256; ++i) + { + if (remap[i]) + remap[i] = layerId++; + else + remap[i] = 0xff; + } + // Remap ids. + for (int i = 0; i < nregs; ++i) + regs[i].layerId = remap[regs[i].layerId]; + + // No layers, return empty. + if (layerId == 0) + return true; + + // Create layers. + rcAssert(lset.layers == 0); + + const int lw = w - borderSize*2; + const int lh = h - borderSize*2; + + // Build contracted bbox for layers. + float bmin[3], bmax[3]; + rcVcopy(bmin, chf.bmin); + rcVcopy(bmax, chf.bmax); + bmin[0] += borderSize*chf.cs; + bmin[2] += borderSize*chf.cs; + bmax[0] -= borderSize*chf.cs; + bmax[2] -= borderSize*chf.cs; + + lset.nlayers = (int)layerId; + + lset.layers = (rcHeightfieldLayer*)rcAlloc(sizeof(rcHeightfieldLayer)*lset.nlayers, RC_ALLOC_PERM); + if (!lset.layers) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'layers' (%d).", lset.nlayers); + return false; + } + memset(lset.layers, 0, sizeof(rcHeightfieldLayer)*lset.nlayers); + + + // Store layers. + for (int i = 0; i < lset.nlayers; ++i) + { + unsigned char curId = (unsigned char)i; + + rcHeightfieldLayer* layer = &lset.layers[i]; + + const int gridSize = sizeof(unsigned char)*lw*lh; + + layer->heights = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM); + if (!layer->heights) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'heights' (%d).", gridSize); + return false; + } + memset(layer->heights, 0xff, gridSize); + + layer->areas = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM); + if (!layer->areas) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'areas' (%d).", gridSize); + return false; + } + memset(layer->areas, 0, gridSize); + + layer->cons = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM); + if (!layer->cons) + { + ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'cons' (%d).", gridSize); + return false; + } + memset(layer->cons, 0, gridSize); + + // Find layer height bounds. + int hmin = 0, hmax = 0; + for (int j = 0; j < nregs; ++j) + { + if (regs[j].base && regs[j].layerId == curId) + { + hmin = (int)regs[j].ymin; + hmax = (int)regs[j].ymax; + } + } + + layer->width = lw; + layer->height = lh; + layer->cs = chf.cs; + layer->ch = chf.ch; + + // Adjust the bbox to fit the heightfield. + rcVcopy(layer->bmin, bmin); + rcVcopy(layer->bmax, bmax); + layer->bmin[1] = bmin[1] + hmin*chf.ch; + layer->bmax[1] = bmin[1] + hmax*chf.ch; + layer->hmin = hmin; + layer->hmax = hmax; + + // Update usable data region. + layer->minx = layer->width; + layer->maxx = 0; + layer->miny = layer->height; + layer->maxy = 0; + + // Copy height and area from compact heightfield. + for (int y = 0; y < lh; ++y) + { + for (int x = 0; x < lw; ++x) + { + const int cx = borderSize+x; + const int cy = borderSize+y; + const rcCompactCell& c = chf.cells[cx+cy*w]; + for (int j = (int)c.index, nj = (int)(c.index+c.count); j < nj; ++j) + { + const rcCompactSpan& s = chf.spans[j]; + // Skip unassigned regions. + if (srcReg[j] == 0xff) + continue; + // Skip of does nto belong to current layer. + unsigned char lid = regs[srcReg[j]].layerId; + if (lid != curId) + continue; + + // Update data bounds. + layer->minx = rcMin(layer->minx, x); + layer->maxx = rcMax(layer->maxx, x); + layer->miny = rcMin(layer->miny, y); + layer->maxy = rcMax(layer->maxy, y); + + // Store height and area type. + const int idx = x+y*lw; + layer->heights[idx] = (unsigned char)(s.y - hmin); + layer->areas[idx] = chf.areas[j]; + + // Check connection. + unsigned char portal = 0; + unsigned char con = 0; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir); + unsigned char alid = srcReg[ai] != 0xff ? regs[srcReg[ai]].layerId : 0xff; + // Portal mask + if (chf.areas[ai] != RC_NULL_AREA && lid != alid) + { + portal |= (unsigned char)(1< hmin) + layer->heights[idx] = rcMax(layer->heights[idx], (unsigned char)(as.y - hmin)); + } + // Valid connection mask + if (chf.areas[ai] != RC_NULL_AREA && lid == alid) + { + const int nx = ax - borderSize; + const int ny = ay - borderSize; + if (nx >= 0 && ny >= 0 && nx < lw && ny < lh) + con |= (unsigned char)(1<cons[idx] = (portal << 4) | con; + } + } + } + + if (layer->minx > layer->maxx) + layer->minx = layer->maxx = 0; + if (layer->miny > layer->maxy) + layer->miny = layer->maxy = 0; + } + + return true; +} diff --git a/lib/Recast/RecastMesh.cpp b/lib/Recast/RecastMesh.cpp new file mode 100644 index 0000000..e99eaeb --- /dev/null +++ b/lib/Recast/RecastMesh.cpp @@ -0,0 +1,1552 @@ +// +// Copyright (c) 2009-2010 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 +#include +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastAssert.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 = (unsigned short*)rcAlloc(sizeof(unsigned short)*(nverts + maxEdgeCount), RC_ALLOC_TEMP); + if (!firstEdge) + return false; + unsigned short* nextEdge = firstEdge + nverts; + int edgeCount = 0; + + rcEdge* edges = (rcEdge*)rcAlloc(sizeof(rcEdge)*maxEdgeCount, RC_ALLOC_TEMP); + if (!edges) + { + rcFree(firstEdge); + return false; + } + + for (int i = 0; i < nverts; i++) + firstEdge[i] = RC_MESH_NULL_IDX; + + for (int i = 0; i < npolys; ++i) + { + unsigned short* t = &polys[i*vertsPerPoly*2]; + for (int j = 0; j < vertsPerPoly; ++j) + { + if (t[j] == RC_MESH_NULL_IDX) break; + unsigned short v0 = t[j]; + unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? 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] = (unsigned short)edgeCount; + edgeCount++; + } + } + } + + for (int i = 0; i < npolys; ++i) + { + unsigned short* t = &polys[i*vertsPerPoly*2]; + for (int j = 0; j < vertsPerPoly; ++j) + { + if (t[j] == RC_MESH_NULL_IDX) break; + unsigned short v0 = t[j]; + unsigned short v1 = (j+1 >= vertsPerPoly || t[j+1] == RC_MESH_NULL_IDX) ? t[0] : t[j+1]; + if (v0 > v1) + { + for (unsigned short e = firstEdge[v1]; e != RC_MESH_NULL_IDX; 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]; + } + } + + rcFree(firstEdge); + rcFree(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 unsigned short 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 (unsigned short)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 (unsigned short)i; +} + +// Last time I checked the if version got compiled using cmov, which was a lot faster than module (with idiv). +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. +static 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); +} + + +static bool diagonalieLoose(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 (intersectProp(d0, d1, p0, p1)) + return false; + } + } + return true; +} + +static bool inConeLoose(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 leftOn(pi, pj, pin1) && leftOn(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)); +} + +static bool diagonalLoose(int i, int j, int n, const int* verts, int* indices) +{ + return inConeLoose(i, j, n, verts, indices) && diagonalieLoose(i, j, n, verts, indices); +} + + +static 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) + { + // We might get here because the contour has overlapping segments, like this: + // + // A o-o=====o---o B + // / |C D| \. + // o o o o + // : : : : + // We'll try to recover by loosing up the inCone test a bit so that a diagonal + // like A-B or C-D can be found and we can continue. + minLen = -1; + mini = -1; + for (int i = 0; i < n; i++) + { + int i1 = next(i, n); + int i2 = next(i1, n); + if (diagonalLoose(i, i2, n, verts, indices)) + { + const int* p0 = &verts[(indices[i] & 0x0fffffff) * 4]; + const int* p2 = &verts[(indices[next(i2, 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) + { + // The contour is messed up. This sometimes happens + // if the contour simplification is too aggressive. + 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] == RC_MESH_NULL_IDX) + 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 mergePolyVerts(unsigned short* pa, unsigned short* pb, 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 canRemoveVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short rem) +{ + const int nvp = mesh.nvp; + + // Count number of polygons to remove. + int numRemovedVerts = 0; + int numTouchedVerts = 0; + int numRemainingEdges = 0; + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*nvp*2]; + const int nv = countPolyVerts(p, nvp); + int numRemoved = 0; + int numVerts = 0; + for (int j = 0; j < nv; ++j) + { + if (p[j] == rem) + { + numTouchedVerts++; + numRemoved++; + } + numVerts++; + } + if (numRemoved) + { + numRemovedVerts += numRemoved; + numRemainingEdges += numVerts-(numRemoved+1); + } + } + + // There would be too few edges remaining to create a polygon. + // This can happen for example when a tip of a triangle is marked + // as deletion, but there are no other polys that share the vertex. + // In this case, the vertex should not be removed. + if (numRemainingEdges <= 2) + return false; + + // Find edges which share the removed vertex. + const int maxEdges = numTouchedVerts*2; + int nedges = 0; + rcScopedDelete edges((int*)rcAlloc(sizeof(int)*maxEdges*3, RC_ALLOC_TEMP)); + if (!edges) + { + ctx->log(RC_LOG_WARNING, "canRemoveVertex: Out of memory 'edges' (%d).", maxEdges*3); + return false; + } + + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*nvp*2]; + const int nv = countPolyVerts(p, nvp); + + // Collect edges which touches the removed vertex. + for (int j = 0, k = nv-1; j < nv; k = j++) + { + if (p[j] == rem || p[k] == rem) + { + // Arrange edge so that a=rem. + int a = p[j], b = p[k]; + if (b == rem) + rcSwap(a,b); + + // Check if the edge exists + bool exists = false; + for (int m = 0; m < nedges; ++m) + { + int* e = &edges[m*3]; + if (e[1] == b) + { + // Exists, increment vertex share count. + e[2]++; + exists = true; + } + } + // Add new edge. + if (!exists) + { + int* e = &edges[nedges*3]; + e[0] = a; + e[1] = b; + e[2] = 1; + nedges++; + } + } + } + } + + // There should be no more than 2 open edges. + // This catches the case that two non-adjacent polygons + // share the removed vertex. In that case, do not remove the vertex. + int numOpenEdges = 0; + for (int i = 0; i < nedges; ++i) + { + if (edges[i*3+2] < 2) + numOpenEdges++; + } + if (numOpenEdges > 2) + return false; + + return true; +} + +static bool removeVertex(rcContext* ctx, rcPolyMesh& mesh, const unsigned short rem, const int maxTris) +{ + const int nvp = mesh.nvp; + + // Count number of polygons to remove. + int numRemovedVerts = 0; + 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) + numRemovedVerts++; + } + } + + int nedges = 0; + rcScopedDelete edges((int*)rcAlloc(sizeof(int)*numRemovedVerts*nvp*4, RC_ALLOC_TEMP)); + if (!edges) + { + ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'edges' (%d).", numRemovedVerts*nvp*4); + return false; + } + + int nhole = 0; + rcScopedDelete hole((int*)rcAlloc(sizeof(int)*numRemovedVerts*nvp, RC_ALLOC_TEMP)); + if (!hole) + { + ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hole' (%d).", numRemovedVerts*nvp); + return false; + } + + int nhreg = 0; + rcScopedDelete hreg((int*)rcAlloc(sizeof(int)*numRemovedVerts*nvp, RC_ALLOC_TEMP)); + if (!hreg) + { + ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'hreg' (%d).", numRemovedVerts*nvp); + return false; + } + + int nharea = 0; + rcScopedDelete harea((int*)rcAlloc(sizeof(int)*numRemovedVerts*nvp, RC_ALLOC_TEMP)); + if (!harea) + { + ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'harea' (%d).", numRemovedVerts*nvp); + return false; + } + + 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*4]; + e[0] = p[k]; + e[1] = p[j]; + e[2] = mesh.regs[i]; + e[3] = mesh.areas[i]; + nedges++; + } + } + // Remove the polygon. + unsigned short* p2 = &mesh.polys[(mesh.npolys-1)*nvp*2]; + if (p != p2) + memcpy(p,p2,sizeof(unsigned short)*nvp); + memset(p+nvp,0xff,sizeof(unsigned short)*nvp); + mesh.regs[i] = mesh.regs[mesh.npolys-1]; + mesh.areas[i] = mesh.areas[mesh.npolys-1]; + mesh.npolys--; + --i; + } + } + + // Remove vertex. + for (int i = (int)rem; i < mesh.nverts - 1; ++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*4+0] > rem) edges[i*4+0]--; + if (edges[i*4+1] > rem) edges[i*4+1]--; + } + + if (nedges == 0) + return true; + + // Start with one vertex, keep appending connected + // segments to the start and end of the hole. + pushBack(edges[0], hole, nhole); + pushBack(edges[2], hreg, nhreg); + pushBack(edges[3], harea, nharea); + + while (nedges) + { + bool match = false; + + for (int i = 0; i < nedges; ++i) + { + const int ea = edges[i*4+0]; + const int eb = edges[i*4+1]; + const int r = edges[i*4+2]; + const int a = edges[i*4+3]; + bool add = false; + if (hole[0] == eb) + { + // The segment matches the beginning of the hole boundary. + pushFront(ea, hole, nhole); + pushFront(r, hreg, nhreg); + pushFront(a, harea, nharea); + add = true; + } + else if (hole[nhole-1] == ea) + { + // The segment matches the end of the hole boundary. + pushBack(eb, hole, nhole); + pushBack(r, hreg, nhreg); + pushBack(a, harea, nharea); + add = true; + } + if (add) + { + // The edge segment was added, remove it. + edges[i*4+0] = edges[(nedges-1)*4+0]; + edges[i*4+1] = edges[(nedges-1)*4+1]; + edges[i*4+2] = edges[(nedges-1)*4+2]; + edges[i*4+3] = edges[(nedges-1)*4+3]; + --nedges; + match = true; + --i; + } + } + + if (!match) + break; + } + + rcScopedDelete tris((int*)rcAlloc(sizeof(int)*nhole*3, RC_ALLOC_TEMP)); + if (!tris) + { + ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tris' (%d).", nhole*3); + return false; + } + + rcScopedDelete tverts((int*)rcAlloc(sizeof(int)*nhole*4, RC_ALLOC_TEMP)); + if (!tverts) + { + ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'tverts' (%d).", nhole*4); + return false; + } + + rcScopedDelete thole((int*)rcAlloc(sizeof(int)*nhole, RC_ALLOC_TEMP)); + if (!thole) + { + ctx->log(RC_LOG_WARNING, "removeVertex: Out of memory 'thole' (%d).", nhole); + return false; + } + + // 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. + int ntris = triangulate(nhole, &tverts[0], &thole[0], tris); + if (ntris < 0) + { + ntris = -ntris; + ctx->log(RC_LOG_WARNING, "removeVertex: triangulate() returned bad results."); + } + + // Merge the hole triangles back to polygons. + rcScopedDelete polys((unsigned short*)rcAlloc(sizeof(unsigned short)*(ntris+1)*nvp, RC_ALLOC_TEMP)); + if (!polys) + { + ctx->log(RC_LOG_ERROR, "removeVertex: Out of memory 'polys' (%d).", (ntris+1)*nvp); + return false; + } + rcScopedDelete pregs((unsigned short*)rcAlloc(sizeof(unsigned short)*ntris, RC_ALLOC_TEMP)); + if (!pregs) + { + ctx->log(RC_LOG_ERROR, "removeVertex: Out of memory 'pregs' (%d).", ntris); + return false; + } + rcScopedDelete pareas((unsigned char*)rcAlloc(sizeof(unsigned char)*ntris, RC_ALLOC_TEMP)); + if (!pareas) + { + ctx->log(RC_LOG_ERROR, "removeVertex: Out of memory 'pareas' (%d).", ntris); + return false; + } + + unsigned short* tmpPoly = &polys[ntris*nvp]; + + // Build initial polygons. + int npolys = 0; + 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]]; + + // If this polygon covers multiple region types then + // mark it as such + if (hreg[t[0]] != hreg[t[1]] || hreg[t[1]] != hreg[t[2]]) + pregs[npolys] = RC_MULTIPLE_REGS; + else + pregs[npolys] = (unsigned short)hreg[t[0]]; + + pareas[npolys] = (unsigned char)harea[t[0]]; + npolys++; + } + } + if (!npolys) + return true; + + // Merge polygons. + if (nvp > 3) + { + for (;;) + { + // Find best polygons to merge. + int bestMergeVal = 0; + int bestPa = 0, bestPb = 0, bestEa = 0, bestEb = 0; + + 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]; + mergePolyVerts(pa, pb, bestEa, bestEb, tmpPoly, nvp); + if (pregs[bestPa] != pregs[bestPb]) + pregs[bestPa] = RC_MULTIPLE_REGS; + + unsigned short* last = &polys[(npolys-1)*nvp]; + if (pb != last) + memcpy(pb, last, sizeof(unsigned short)*nvp); + pregs[bestPb] = pregs[npolys-1]; + pareas[bestPb] = pareas[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.areas[mesh.npolys] = pareas[i]; + mesh.npolys++; + if (mesh.npolys > maxTris) + { + ctx->log(RC_LOG_ERROR, "removeVertex: Too many polygons %d (max:%d).", mesh.npolys, maxTris); + return false; + } + } + + return true; +} + +/// @par +/// +/// @note If the mesh data is to be used to construct a Detour navigation mesh, then the upper +/// limit must be retricted to <= #DT_VERTS_PER_POLYGON. +/// +/// @see rcAllocPolyMesh, rcContourSet, rcPolyMesh, rcConfig +bool rcBuildPolyMesh(rcContext* ctx, rcContourSet& cset, const int nvp, rcPolyMesh& mesh) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_BUILD_POLYMESH); + + rcVcopy(mesh.bmin, cset.bmin); + rcVcopy(mesh.bmax, cset.bmax); + mesh.cs = cset.cs; + mesh.ch = cset.ch; + mesh.borderSize = cset.borderSize; + mesh.maxEdgeError = cset.maxError; + + int maxVertices = 0; + int maxTris = 0; + int maxVertsPerCont = 0; + for (int i = 0; i < cset.nconts; ++i) + { + // Skip null contours. + if (cset.conts[i].nverts < 3) continue; + maxVertices += cset.conts[i].nverts; + maxTris += cset.conts[i].nverts - 2; + maxVertsPerCont = rcMax(maxVertsPerCont, cset.conts[i].nverts); + } + + if (maxVertices >= 0xfffe) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Too many vertices %d.", maxVertices); + return false; + } + + rcScopedDelete vflags((unsigned char*)rcAlloc(sizeof(unsigned char)*maxVertices, RC_ALLOC_TEMP)); + if (!vflags) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'vflags' (%d).", maxVertices); + return false; + } + memset(vflags, 0, maxVertices); + + mesh.verts = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxVertices*3, RC_ALLOC_PERM); + if (!mesh.verts) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.verts' (%d).", maxVertices); + return false; + } + mesh.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxTris*nvp*2, RC_ALLOC_PERM); + if (!mesh.polys) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.polys' (%d).", maxTris*nvp*2); + return false; + } + mesh.regs = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxTris, RC_ALLOC_PERM); + if (!mesh.regs) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.regs' (%d).", maxTris); + return false; + } + mesh.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*maxTris, RC_ALLOC_PERM); + if (!mesh.areas) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.areas' (%d).", maxTris); + return false; + } + + mesh.nverts = 0; + mesh.npolys = 0; + mesh.nvp = nvp; + mesh.maxpolys = maxTris; + + 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); + memset(mesh.areas, 0, sizeof(unsigned char)*maxTris); + + rcScopedDelete nextVert((int*)rcAlloc(sizeof(int)*maxVertices, RC_ALLOC_TEMP)); + if (!nextVert) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'nextVert' (%d).", maxVertices); + return false; + } + memset(nextVert, 0, sizeof(int)*maxVertices); + + rcScopedDelete firstVert((int*)rcAlloc(sizeof(int)*VERTEX_BUCKET_COUNT, RC_ALLOC_TEMP)); + if (!firstVert) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT); + return false; + } + for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i) + firstVert[i] = -1; + + rcScopedDelete indices((int*)rcAlloc(sizeof(int)*maxVertsPerCont, RC_ALLOC_TEMP)); + if (!indices) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'indices' (%d).", maxVertsPerCont); + return false; + } + rcScopedDelete tris((int*)rcAlloc(sizeof(int)*maxVertsPerCont*3, RC_ALLOC_TEMP)); + if (!tris) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'tris' (%d).", maxVertsPerCont*3); + return false; + } + rcScopedDelete polys((unsigned short*)rcAlloc(sizeof(unsigned short)*(maxVertsPerCont+1)*nvp, RC_ALLOC_TEMP)); + if (!polys) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'polys' (%d).", maxVertsPerCont*nvp); + return false; + } + unsigned short* tmpPoly = &polys[maxVertsPerCont*nvp]; + + for (int i = 0; i < cset.nconts; ++i) + { + rcContour& cont = cset.conts[i]; + + // Skip null 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. +/* printf("\tconst float bmin[3] = {%ff,%ff,%ff};\n", cset.bmin[0], cset.bmin[1], cset.bmin[2]); + printf("\tconst float cs = %ff;\n", cset.cs); + printf("\tconst float ch = %ff;\n", cset.ch); + printf("\tconst int verts[] = {\n"); + 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]); + } + printf("\t};\n\tconst int nverts = sizeof(verts)/(sizeof(int)*4);\n");*/ + ctx->log(RC_LOG_WARNING, "rcBuildPolyMesh: Bad triangulation Contour %d.", i); + 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) + { + for(;;) + { + // Find best polygons to merge. + int bestMergeVal = 0; + int bestPa = 0, bestPb = 0, bestEa = 0, bestEb = 0; + + 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]; + mergePolyVerts(pa, pb, bestEa, bestEb, tmpPoly, nvp); + unsigned short* lastPoly = &polys[(npolys-1)*nvp]; + if (pb != lastPoly) + memcpy(pb, lastPoly, 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.areas[mesh.npolys] = cont.area; + mesh.npolys++; + if (mesh.npolys > maxTris) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Too many polygons %d (max:%d).", mesh.npolys, maxTris); + return false; + } + } + } + + + // Remove edge vertices. + for (int i = 0; i < mesh.nverts; ++i) + { + if (vflags[i]) + { + if (!canRemoveVertex(ctx, mesh, (unsigned short)i)) + continue; + if (!removeVertex(ctx, mesh, (unsigned short)i, maxTris)) + { + // Failed to remove vertex + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Failed to remove edge vertex %d.", i); + return false; + } + // Remove vertex + // Note: mesh.nverts is already decremented inside removeVertex()! + // Fixup vertex flags + for (int j = i; j < mesh.nverts; ++j) + vflags[j] = vflags[j+1]; + --i; + } + } + + // Calculate adjacency. + if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, nvp)) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Adjacency failed."); + return false; + } + + // Find portal edges + if (mesh.borderSize > 0) + { + const int w = cset.width; + const int h = cset.height; + for (int i = 0; i < mesh.npolys; ++i) + { + unsigned short* p = &mesh.polys[i*2*nvp]; + for (int j = 0; j < nvp; ++j) + { + if (p[j] == RC_MESH_NULL_IDX) break; + // Skip connected edges. + if (p[nvp+j] != RC_MESH_NULL_IDX) + continue; + int nj = j+1; + if (nj >= nvp || p[nj] == RC_MESH_NULL_IDX) nj = 0; + const unsigned short* va = &mesh.verts[p[j]*3]; + const unsigned short* vb = &mesh.verts[p[nj]*3]; + + if ((int)va[0] == 0 && (int)vb[0] == 0) + p[nvp+j] = 0x8000 | 0; + else if ((int)va[2] == h && (int)vb[2] == h) + p[nvp+j] = 0x8000 | 1; + else if ((int)va[0] == w && (int)vb[0] == w) + p[nvp+j] = 0x8000 | 2; + else if ((int)va[2] == 0 && (int)vb[2] == 0) + p[nvp+j] = 0x8000 | 3; + } + } + } + + // Just allocate the mesh flags array. The user is resposible to fill it. + mesh.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*mesh.npolys, RC_ALLOC_PERM); + if (!mesh.flags) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: Out of memory 'mesh.flags' (%d).", mesh.npolys); + return false; + } + memset(mesh.flags, 0, sizeof(unsigned short) * mesh.npolys); + + if (mesh.nverts > 0xffff) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: The resulting mesh has too many vertices %d (max %d). Data can be corrupted.", mesh.nverts, 0xffff); + } + if (mesh.npolys > 0xffff) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMesh: The resulting mesh has too many polygons %d (max %d). Data can be corrupted.", mesh.npolys, 0xffff); + } + + return true; +} + +/// @see rcAllocPolyMesh, rcPolyMesh +bool rcMergePolyMeshes(rcContext* ctx, rcPolyMesh** meshes, const int nmeshes, rcPolyMesh& mesh) +{ + rcAssert(ctx); + + if (!nmeshes || !meshes) + return true; + + rcScopedTimer timer(ctx, RC_TIMER_MERGE_POLYMESH); + + mesh.nvp = meshes[0]->nvp; + mesh.cs = meshes[0]->cs; + mesh.ch = meshes[0]->ch; + rcVcopy(mesh.bmin, meshes[0]->bmin); + rcVcopy(mesh.bmax, meshes[0]->bmax); + + int maxVerts = 0; + int maxPolys = 0; + int maxVertsPerMesh = 0; + for (int i = 0; i < nmeshes; ++i) + { + rcVmin(mesh.bmin, meshes[i]->bmin); + rcVmax(mesh.bmax, meshes[i]->bmax); + maxVertsPerMesh = rcMax(maxVertsPerMesh, meshes[i]->nverts); + maxVerts += meshes[i]->nverts; + maxPolys += meshes[i]->npolys; + } + + mesh.nverts = 0; + mesh.verts = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxVerts*3, RC_ALLOC_PERM); + if (!mesh.verts) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.verts' (%d).", maxVerts*3); + return false; + } + + mesh.npolys = 0; + mesh.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxPolys*2*mesh.nvp, RC_ALLOC_PERM); + if (!mesh.polys) + { + ctx->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 = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxPolys, RC_ALLOC_PERM); + if (!mesh.regs) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.regs' (%d).", maxPolys); + return false; + } + memset(mesh.regs, 0, sizeof(unsigned short)*maxPolys); + + mesh.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*maxPolys, RC_ALLOC_PERM); + if (!mesh.areas) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.areas' (%d).", maxPolys); + return false; + } + memset(mesh.areas, 0, sizeof(unsigned char)*maxPolys); + + mesh.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxPolys, RC_ALLOC_PERM); + if (!mesh.flags) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'mesh.flags' (%d).", maxPolys); + return false; + } + memset(mesh.flags, 0, sizeof(unsigned short)*maxPolys); + + rcScopedDelete nextVert((int*)rcAlloc(sizeof(int)*maxVerts, RC_ALLOC_TEMP)); + if (!nextVert) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'nextVert' (%d).", maxVerts); + return false; + } + memset(nextVert, 0, sizeof(int)*maxVerts); + + rcScopedDelete firstVert((int*)rcAlloc(sizeof(int)*VERTEX_BUCKET_COUNT, RC_ALLOC_TEMP)); + if (!firstVert) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'firstVert' (%d).", VERTEX_BUCKET_COUNT); + return false; + } + for (int i = 0; i < VERTEX_BUCKET_COUNT; ++i) + firstVert[i] = -1; + + rcScopedDelete vremap((unsigned short*)rcAlloc(sizeof(unsigned short)*maxVertsPerMesh, RC_ALLOC_PERM)); + if (!vremap) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Out of memory 'vremap' (%d).", maxVertsPerMesh); + return false; + } + memset(vremap, 0, sizeof(unsigned short)*maxVertsPerMesh); + + 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); + + bool isMinX = (ox == 0); + bool isMinZ = (oz == 0); + bool isMaxX = ((unsigned short)floorf((mesh.bmax[0] - pmesh->bmax[0]) / mesh.cs + 0.5f)) == 0; + bool isMaxZ = ((unsigned short)floorf((mesh.bmax[2] - pmesh->bmax[2]) / mesh.cs + 0.5f)) == 0; + bool isOnBorder = (isMinX || isMinZ || isMaxX || isMaxZ); + + 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.areas[mesh.npolys] = pmesh->areas[j]; + mesh.flags[mesh.npolys] = pmesh->flags[j]; + mesh.npolys++; + for (int k = 0; k < mesh.nvp; ++k) + { + if (src[k] == RC_MESH_NULL_IDX) break; + tgt[k] = vremap[src[k]]; + } + + if (isOnBorder) + { + for (int k = mesh.nvp; k < mesh.nvp * 2; ++k) + { + if (src[k] & 0x8000 && src[k] != 0xffff) + { + unsigned short dir = src[k] & 0xf; + switch (dir) + { + case 0: // Portal x- + if (isMinX) + tgt[k] = src[k]; + break; + case 1: // Portal z+ + if (isMaxZ) + tgt[k] = src[k]; + break; + case 2: // Portal x+ + if (isMaxX) + tgt[k] = src[k]; + break; + case 3: // Portal z- + if (isMinZ) + tgt[k] = src[k]; + break; + } + } + } + } + } + } + + // Calculate adjacency. + if (!buildMeshAdjacency(mesh.polys, mesh.npolys, mesh.nverts, mesh.nvp)) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: Adjacency failed."); + return false; + } + + if (mesh.nverts > 0xffff) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: The resulting mesh has too many vertices %d (max %d). Data can be corrupted.", mesh.nverts, 0xffff); + } + if (mesh.npolys > 0xffff) + { + ctx->log(RC_LOG_ERROR, "rcMergePolyMeshes: The resulting mesh has too many polygons %d (max %d). Data can be corrupted.", mesh.npolys, 0xffff); + } + + return true; +} + +bool rcCopyPolyMesh(rcContext* ctx, const rcPolyMesh& src, rcPolyMesh& dst) +{ + rcAssert(ctx); + + // Destination must be empty. + rcAssert(dst.verts == 0); + rcAssert(dst.polys == 0); + rcAssert(dst.regs == 0); + rcAssert(dst.areas == 0); + rcAssert(dst.flags == 0); + + dst.nverts = src.nverts; + dst.npolys = src.npolys; + dst.maxpolys = src.npolys; + dst.nvp = src.nvp; + rcVcopy(dst.bmin, src.bmin); + rcVcopy(dst.bmax, src.bmax); + dst.cs = src.cs; + dst.ch = src.ch; + dst.borderSize = src.borderSize; + dst.maxEdgeError = src.maxEdgeError; + + dst.verts = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.nverts*3, RC_ALLOC_PERM); + if (!dst.verts) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.verts' (%d).", src.nverts*3); + return false; + } + memcpy(dst.verts, src.verts, sizeof(unsigned short)*src.nverts*3); + + dst.polys = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys*2*src.nvp, RC_ALLOC_PERM); + if (!dst.polys) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.polys' (%d).", src.npolys*2*src.nvp); + return false; + } + memcpy(dst.polys, src.polys, sizeof(unsigned short)*src.npolys*2*src.nvp); + + dst.regs = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys, RC_ALLOC_PERM); + if (!dst.regs) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.regs' (%d).", src.npolys); + return false; + } + memcpy(dst.regs, src.regs, sizeof(unsigned short)*src.npolys); + + dst.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*src.npolys, RC_ALLOC_PERM); + if (!dst.areas) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.areas' (%d).", src.npolys); + return false; + } + memcpy(dst.areas, src.areas, sizeof(unsigned char)*src.npolys); + + dst.flags = (unsigned short*)rcAlloc(sizeof(unsigned short)*src.npolys, RC_ALLOC_PERM); + if (!dst.flags) + { + ctx->log(RC_LOG_ERROR, "rcCopyPolyMesh: Out of memory 'dst.flags' (%d).", src.npolys); + return false; + } + memcpy(dst.flags, src.flags, sizeof(unsigned short)*src.npolys); + + return true; +} diff --git a/lib/Recast/RecastMeshDetail.cpp b/lib/Recast/RecastMeshDetail.cpp new file mode 100644 index 0000000..f953132 --- /dev/null +++ b/lib/Recast/RecastMeshDetail.cpp @@ -0,0 +1,1462 @@ +// +// Copyright (c) 2009-2010 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 +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastAssert.h" + + +static const unsigned RC_UNSET_HEIGHT = 0xffff; + +struct rcHeightPatch +{ + inline rcHeightPatch() : data(0), xmin(0), ymin(0), width(0), height(0) {} + inline ~rcHeightPatch() { rcFree(data); } + unsigned short* data; + int xmin, ymin, width, height; +}; + + +inline float vdot2(const float* a, const float* b) +{ + return a[0]*b[0] + a[2]*b[2]; +} + +inline float vdistSq2(const float* p, const float* q) +{ + const float dx = q[0] - p[0]; + const float dy = q[2] - p[2]; + return dx*dx + dy*dy; +} + +inline float vdist2(const float* p, const float* q) +{ + return sqrtf(vdistSq2(p,q)); +} + +inline float vcross2(const float* p1, const float* p2, const float* p3) +{ + const float u1 = p2[0] - p1[0]; + const float v1 = p2[2] - p1[2]; + const float u2 = p3[0] - p1[0]; + const float v2 = p3[2] - p1[2]; + return u1 * v2 - v1 * u2; +} + +static bool circumCircle(const float* p1, const float* p2, const float* p3, + float* c, float& r) +{ + static const float EPS = 1e-6f; + // Calculate the circle relative to p1, to avoid some precision issues. + const float v1[3] = {0,0,0}; + float v2[3], v3[3]; + rcVsub(v2, p2,p1); + rcVsub(v3, p3,p1); + + const float cp = vcross2(v1, v2, v3); + if (fabsf(cp) > EPS) + { + const float v1Sq = vdot2(v1,v1); + const float v2Sq = vdot2(v2,v2); + const float v3Sq = vdot2(v3,v3); + c[0] = (v1Sq*(v2[2]-v3[2]) + v2Sq*(v3[2]-v1[2]) + v3Sq*(v1[2]-v2[2])) / (2*cp); + c[1] = 0; + c[2] = (v1Sq*(v3[0]-v2[0]) + v2Sq*(v1[0]-v3[0]) + v3Sq*(v2[0]-v1[0])) / (2*cp); + r = vdist2(c, v1); + rcVadd(c, c, p1); + return true; + } + + rcVcopy(c, p1); + r = 0; + return false; +} + +static float distPtTri(const float* p, const float* a, const float* b, const float* c) +{ + float v0[3], v1[3], v2[3]; + rcVsub(v0, c,a); + rcVsub(v1, b,a); + rcVsub(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 + const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + const 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) + { + const 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, const int /*nverts*/, const int* tris, const 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 fx, const float fy, const float fz, + const float /*cs*/, const float ics, const float ch, + const int radius, const rcHeightPatch& hp) +{ + int ix = (int)floorf(fx*ics + 0.01f); + int iz = (int)floorf(fz*ics + 0.01f); + ix = rcClamp(ix-hp.xmin, 0, hp.width - 1); + iz = rcClamp(iz-hp.ymin, 0, hp.height - 1); + unsigned short h = hp.data[ix+iz*hp.width]; + if (h == RC_UNSET_HEIGHT) + { + // Special case when data might be bad. + // Walk adjacent cells in a spiral up to 'radius', and look + // for a pixel which has a valid height. + int x = 1, z = 0, dx = 1, dz = 0; + int maxSize = radius * 2 + 1; + int maxIter = maxSize * maxSize - 1; + + int nextRingIterStart = 8; + int nextRingIters = 16; + + float dmin = FLT_MAX; + for (int i = 0; i < maxIter; i++) + { + const int nx = ix + x; + const int nz = iz + z; + + if (nx >= 0 && nz >= 0 && nx < hp.width && nz < hp.height) + { + const unsigned short nh = hp.data[nx + nz*hp.width]; + if (nh != RC_UNSET_HEIGHT) + { + const float d = fabsf(nh*ch - fy); + if (d < dmin) + { + h = nh; + dmin = d; + } + } + } + + // We are searching in a grid which looks approximately like this: + // __________ + // |2 ______ 2| + // | |1 __ 1| | + // | | |__| | | + // | |______| | + // |__________| + // We want to find the best height as close to the center cell as possible. This means that + // if we find a height in one of the neighbor cells to the center, we don't want to + // expand further out than the 8 neighbors - we want to limit our search to the closest + // of these "rings", but the best height in the ring. + // For example, the center is just 1 cell. We checked that at the entrance to the function. + // The next "ring" contains 8 cells (marked 1 above). Those are all the neighbors to the center cell. + // The next one again contains 16 cells (marked 2). In general each ring has 8 additional cells, which + // can be thought of as adding 2 cells around the "center" of each side when we expand the ring. + // Here we detect if we are about to enter the next ring, and if we are and we have found + // a height, we abort the search. + if (i + 1 == nextRingIterStart) + { + if (h != RC_UNSET_HEIGHT) + break; + + nextRingIterStart += nextRingIters; + nextRingIters += 8; + } + + if ((x == z) || ((x < 0) && (x == -z)) || ((x > 0) && (x == 1 - z))) + { + int tmp = dx; + dx = -dz; + dz = tmp; + } + x += dx; + z += dz; + } + } + return h; +} + + +enum EdgeValues +{ + EV_UNDEF = -1, + EV_HULL = -2, +}; + +static int findEdge(const int* edges, int nedges, int s, int t) +{ + for (int i = 0; i < nedges; i++) + { + const int* e = &edges[i*4]; + if ((e[0] == s && e[1] == t) || (e[0] == t && e[1] == s)) + return i; + } + return EV_UNDEF; +} + +static int addEdge(rcContext* ctx, int* edges, int& nedges, const int maxEdges, int s, int t, int l, int r) +{ + if (nedges >= maxEdges) + { + ctx->log(RC_LOG_ERROR, "addEdge: Too many edges (%d/%d).", nedges, maxEdges); + return EV_UNDEF; + } + + // Add edge if not already in the triangulation. + int e = findEdge(edges, nedges, s, t); + if (e == EV_UNDEF) + { + int* edge = &edges[nedges*4]; + edge[0] = s; + edge[1] = t; + edge[2] = l; + edge[3] = r; + return nedges++; + } + else + { + return EV_UNDEF; + } +} + +static void updateLeftFace(int* e, int s, int t, int f) +{ + if (e[0] == s && e[1] == t && e[2] == EV_UNDEF) + e[2] = f; + else if (e[1] == s && e[0] == t && e[3] == EV_UNDEF) + e[3] = f; +} + +static int overlapSegSeg2d(const float* a, const float* b, const float* c, const float* d) +{ + const float a1 = vcross2(a, b, d); + const float a2 = vcross2(a, b, c); + if (a1*a2 < 0.0f) + { + float a3 = vcross2(c, d, a); + float a4 = a3 + a2 - a1; + if (a3 * a4 < 0.0f) + return 1; + } + return 0; +} + +static bool overlapEdges(const float* pts, const int* edges, int nedges, int s1, int t1) +{ + for (int i = 0; i < nedges; ++i) + { + const int s0 = edges[i*4+0]; + const int t0 = edges[i*4+1]; + // Same or connected edges do not overlap. + if (s0 == s1 || s0 == t1 || t0 == s1 || t0 == t1) + continue; + if (overlapSegSeg2d(&pts[s0*3],&pts[t0*3], &pts[s1*3],&pts[t1*3])) + return true; + } + return false; +} + +static void completeFacet(rcContext* ctx, const float* pts, int npts, int* edges, int& nedges, const int maxEdges, int& nfaces, int e) +{ + static const float EPS = 1e-5f; + + int* edge = &edges[e*4]; + + // Cache s and t. + int s,t; + if (edge[2] == EV_UNDEF) + { + s = edge[0]; + t = edge[1]; + } + else if (edge[3] == EV_UNDEF) + { + s = edge[1]; + t = edge[0]; + } + else + { + // Edge already completed. + return; + } + + // Find best point on left of edge. + int pt = npts; + float c[3] = {0,0,0}; + float r = -1; + for (int u = 0; u < npts; ++u) + { + if (u == s || u == t) continue; + if (vcross2(&pts[s*3], &pts[t*3], &pts[u*3]) > EPS) + { + if (r < 0) + { + // The circle is not updated yet, do it now. + pt = u; + circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r); + continue; + } + const float d = vdist2(c, &pts[u*3]); + const float tol = 0.001f; + if (d > r*(1+tol)) + { + // Outside current circumcircle, skip. + continue; + } + else if (d < r*(1-tol)) + { + // Inside safe circumcircle, update circle. + pt = u; + circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r); + } + else + { + // Inside epsilon circum circle, do extra tests to make sure the edge is valid. + // s-u and t-u cannot overlap with s-pt nor t-pt if they exists. + if (overlapEdges(pts, edges, nedges, s,u)) + continue; + if (overlapEdges(pts, edges, nedges, t,u)) + continue; + // Edge is valid. + pt = u; + circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r); + } + } + } + + // Add new triangle or update edge info if s-t is on hull. + if (pt < npts) + { + // Update face information of edge being completed. + updateLeftFace(&edges[e*4], s, t, nfaces); + + // Add new edge or update face info of old edge. + e = findEdge(edges, nedges, pt, s); + if (e == EV_UNDEF) + addEdge(ctx, edges, nedges, maxEdges, pt, s, nfaces, EV_UNDEF); + else + updateLeftFace(&edges[e*4], pt, s, nfaces); + + // Add new edge or update face info of old edge. + e = findEdge(edges, nedges, t, pt); + if (e == EV_UNDEF) + addEdge(ctx, edges, nedges, maxEdges, t, pt, nfaces, EV_UNDEF); + else + updateLeftFace(&edges[e*4], t, pt, nfaces); + + nfaces++; + } + else + { + updateLeftFace(&edges[e*4], s, t, EV_HULL); + } +} + +static void delaunayHull(rcContext* ctx, const int npts, const float* pts, + const int nhull, const int* hull, + rcIntArray& tris, rcIntArray& edges) +{ + int nfaces = 0; + int nedges = 0; + const int maxEdges = npts*10; + edges.resize(maxEdges*4); + + for (int i = 0, j = nhull-1; i < nhull; j=i++) + addEdge(ctx, &edges[0], nedges, maxEdges, hull[j],hull[i], EV_HULL, EV_UNDEF); + + int currentEdge = 0; + while (currentEdge < nedges) + { + if (edges[currentEdge*4+2] == EV_UNDEF) + completeFacet(ctx, pts, npts, &edges[0], nedges, maxEdges, nfaces, currentEdge); + if (edges[currentEdge*4+3] == EV_UNDEF) + completeFacet(ctx, pts, npts, &edges[0], nedges, maxEdges, nfaces, currentEdge); + currentEdge++; + } + + // Create tris + tris.resize(nfaces*4); + for (int i = 0; i < nfaces*4; ++i) + tris[i] = -1; + + for (int i = 0; i < nedges; ++i) + { + const int* e = &edges[i*4]; + if (e[3] >= 0) + { + // Left face + int* t = &tris[e[3]*4]; + if (t[0] == -1) + { + t[0] = e[0]; + t[1] = e[1]; + } + else if (t[0] == e[1]) + t[2] = e[0]; + else if (t[1] == e[0]) + t[2] = e[1]; + } + if (e[2] >= 0) + { + // Right + int* t = &tris[e[2]*4]; + if (t[0] == -1) + { + t[0] = e[1]; + t[1] = e[0]; + } + else if (t[0] == e[0]) + t[2] = e[1]; + else if (t[1] == e[1]) + t[2] = e[0]; + } + } + + for (int i = 0; i < tris.size()/4; ++i) + { + int* t = &tris[i*4]; + if (t[0] == -1 || t[1] == -1 || t[2] == -1) + { + ctx->log(RC_LOG_WARNING, "delaunayHull: Removing dangling face %d [%d,%d,%d].", i, t[0],t[1],t[2]); + 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; + } + } +} + +// Calculate minimum extend of the polygon. +static float polyMinExtent(const float* verts, const int nverts) +{ + float minDist = FLT_MAX; + for (int i = 0; i < nverts; i++) + { + const int ni = (i+1) % nverts; + const float* p1 = &verts[i*3]; + const float* p2 = &verts[ni*3]; + float maxEdgeDist = 0; + for (int j = 0; j < nverts; j++) + { + if (j == i || j == ni) continue; + float d = distancePtSeg2d(&verts[j*3], p1,p2); + maxEdgeDist = rcMax(maxEdgeDist, d); + } + minDist = rcMin(minDist, maxEdgeDist); + } + return rcSqrt(minDist); +} + +// Last time I checked the if version got compiled using cmov, which was a lot faster than module (with idiv). +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; } + +static void triangulateHull(const int /*nverts*/, const float* verts, const int nhull, const int* hull, rcIntArray& tris) +{ + int start = 0, left = 1, right = nhull-1; + + // Start from an ear with shortest perimeter. + // This tends to favor well formed triangles as starting point. + float dmin = 0; + for (int i = 0; i < nhull; i++) + { + int pi = prev(i, nhull); + int ni = next(i, nhull); + const float* pv = &verts[hull[pi]*3]; + const float* cv = &verts[hull[i]*3]; + const float* nv = &verts[hull[ni]*3]; + const float d = vdist2(pv,cv) + vdist2(cv,nv) + vdist2(nv,pv); + if (d < dmin) + { + start = i; + left = ni; + right = pi; + dmin = d; + } + } + + // Add first triangle + tris.push(hull[start]); + tris.push(hull[left]); + tris.push(hull[right]); + tris.push(0); + + // Triangulate the polygon by moving left or right, + // depending on which triangle has shorter perimeter. + // This heuristic was chose emprically, since it seems + // handle tesselated straight edges well. + while (next(left, nhull) != right) + { + // Check to see if se should advance left or right. + int nleft = next(left, nhull); + int nright = prev(right, nhull); + + const float* cvleft = &verts[hull[left]*3]; + const float* nvleft = &verts[hull[nleft]*3]; + const float* cvright = &verts[hull[right]*3]; + const float* nvright = &verts[hull[nright]*3]; + const float dleft = vdist2(cvleft, nvleft) + vdist2(nvleft, cvright); + const float dright = vdist2(cvright, nvright) + vdist2(cvleft, nvright); + + if (dleft < dright) + { + tris.push(hull[left]); + tris.push(hull[nleft]); + tris.push(hull[right]); + tris.push(0); + left = nleft; + } + else + { + tris.push(hull[left]); + tris.push(hull[nright]); + tris.push(hull[right]); + tris.push(0); + right = nright; + } + } +} + + +inline float getJitterX(const int i) +{ + return (((i * 0x8da6b343) & 0xffff) / 65535.0f * 2.0f) - 1.0f; +} + +inline float getJitterY(const int i) +{ + return (((i * 0xd8163841) & 0xffff) / 65535.0f * 2.0f) - 1.0f; +} + +static bool buildPolyDetail(rcContext* ctx, const float* in, const int nin, + const float sampleDist, const float sampleMaxError, + const int heightSearchRadius, const rcCompactHeightfield& chf, + const rcHeightPatch& hp, float* verts, int& nverts, + rcIntArray& tris, rcIntArray& edges, rcIntArray& samples) +{ + static const int MAX_VERTS = 127; + static const int MAX_TRIS = 255; // Max tris for delaunay is 2n-2-k (n=num verts, k=num hull verts). + static const int MAX_VERTS_PER_EDGE = 32; + float edge[(MAX_VERTS_PER_EDGE+1)*3]; + int hull[MAX_VERTS]; + int nhull = 0; + + nverts = nin; + + for (int i = 0; i < nin; ++i) + rcVcopy(&verts[i*3], &in[i*3]); + + edges.resize(0); + tris.resize(0); + + const float cs = chf.cs; + const float ics = 1.0f/cs; + + // Calculate minimum extents of the polygon based on input data. + float minExtent = polyMinExtent(verts, nverts); + + // Tessellate 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]; + bool swapped = false; + // 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); + swapped = true; + } + } + else + { + if (vj[0] > vi[0]) + { + rcSwap(vj,vi); + swapped = true; + } + } + // 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_VERTS_PER_EDGE) nn = MAX_VERTS_PER_EDGE-1; + 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] = getHeight(pos[0],pos[1],pos[2], cs, ics, chf.ch, heightSearchRadius, hp)*chf.ch; + } + // Simplify samples. + int idx[MAX_VERTS_PER_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 dev = distancePtSeg(&edge[m*3],va,vb); + if (dev > maxd) + { + maxd = dev; + 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; + } + } + + hull[nhull++] = j; + // Add new vertices. + if (swapped) + { + for (int k = nidx-2; k > 0; --k) + { + rcVcopy(&verts[nverts*3], &edge[idx[k]*3]); + hull[nhull++] = nverts; + nverts++; + } + } + else + { + for (int k = 1; k < nidx-1; ++k) + { + rcVcopy(&verts[nverts*3], &edge[idx[k]*3]); + hull[nhull++] = nverts; + nverts++; + } + } + } + } + + // If the polygon minimum extent is small (sliver or small triangle), do not try to add internal points. + if (minExtent < sampleDist*2) + { + triangulateHull(nverts, verts, nhull, hull, tris); + return true; + } + + // Tessellate the base mesh. + // We're using the triangulateHull instead of delaunayHull as it tends to + // create a bit better triangulation for long thin triangles when there + // are no internal points. + triangulateHull(nverts, verts, nhull, hull, tris); + + if (tris.size() == 0) + { + // Could not triangulate the poly, make sure there is some valid data there. + ctx->log(RC_LOG_WARNING, "buildPolyDetail: Could not triangulate polygon (%d verts).", nverts); + return true; + } + + if (sampleDist > 0) + { + // Create sample locations in a grid. + float bmin[3], bmax[3]; + rcVcopy(bmin, in); + rcVcopy(bmax, in); + for (int i = 1; i < nin; ++i) + { + rcVmin(bmin, &in[i*3]); + rcVmax(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[1] = (bmax[1]+bmin[1])*0.5f; + 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[0], pt[1], pt[2], cs, ics, chf.ch, heightSearchRadius, hp)); + samples.push(z); + samples.push(0); // Not added + } + } + + // 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()/4; + for (int iter = 0; iter < nsamples; ++iter) + { + if (nverts >= MAX_VERTS) + break; + + // Find sample with most error. + float bestpt[3] = {0,0,0}; + float bestd = 0; + int besti = -1; + for (int i = 0; i < nsamples; ++i) + { + const int* s = &samples[i*4]; + if (s[3]) continue; // skip added. + float pt[3]; + // The sample location is jittered to get rid of some bad triangulations + // which are cause by symmetrical data from the grid structure. + pt[0] = s[0]*sampleDist + getJitterX(i)*cs*0.1f; + pt[1] = s[1]*chf.ch; + pt[2] = s[2]*sampleDist + getJitterY(i)*cs*0.1f; + 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; + besti = i; + rcVcopy(bestpt,pt); + } + } + // If the max error is within accepted threshold, stop tesselating. + if (bestd <= sampleMaxError || besti == -1) + break; + // Mark sample as added. + samples[besti*4+3] = 1; + // Add the new sample point. + rcVcopy(&verts[nverts*3],bestpt); + nverts++; + + // Create new triangulation. + // TODO: Incremental add instead of full rebuild. + edges.resize(0); + tris.resize(0); + delaunayHull(ctx, nverts, verts, nhull, hull, tris, edges); + } + } + + const int ntris = tris.size()/4; + if (ntris > MAX_TRIS) + { + tris.resize(MAX_TRIS*4); + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Shrinking triangle count from %d to max %d.", ntris, MAX_TRIS); + } + + return true; +} + +static void seedArrayWithPolyCenter(rcContext* ctx, const rcCompactHeightfield& chf, + const unsigned short* poly, const int npoly, + const unsigned short* verts, const int bs, + rcHeightPatch& hp, rcIntArray& array) +{ + // Note: Reads to the compact heightfield are offset by border size (bs) + // since border size offset is already removed from the polymesh vertices. + + static const int offset[9*2] = + { + 0,0, -1,-1, 0,-1, 1,-1, 1,0, 1,1, 0,1, -1,1, -1,0, + }; + + // Find cell closest to a poly vertex + int startCellX = 0, startCellY = 0, startSpanIndex = -1; + int dmin = RC_UNSET_HEIGHT; + for (int j = 0; j < npoly && dmin > 0; ++j) + { + for (int k = 0; k < 9 && dmin > 0; ++k) + { + const int ax = (int)verts[poly[j]*3+0] + offset[k*2+0]; + const int ay = (int)verts[poly[j]*3+1]; + const int az = (int)verts[poly[j]*3+2] + offset[k*2+1]; + if (ax < hp.xmin || ax >= hp.xmin+hp.width || + az < hp.ymin || az >= hp.ymin+hp.height) + continue; + + const rcCompactCell& c = chf.cells[(ax+bs)+(az+bs)*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni && dmin > 0; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + int d = rcAbs(ay - (int)s.y); + if (d < dmin) + { + startCellX = ax; + startCellY = az; + startSpanIndex = i; + dmin = d; + } + } + } + } + + rcAssert(startSpanIndex != -1); + // Find center of the polygon + int pcx = 0, pcy = 0; + for (int j = 0; j < npoly; ++j) + { + pcx += (int)verts[poly[j]*3+0]; + pcy += (int)verts[poly[j]*3+2]; + } + pcx /= npoly; + pcy /= npoly; + + // Use seeds array as a stack for DFS + array.resize(0); + array.push(startCellX); + array.push(startCellY); + array.push(startSpanIndex); + + int dirs[] = { 0, 1, 2, 3 }; + memset(hp.data, 0, sizeof(unsigned short)*hp.width*hp.height); + // DFS to move to the center. Note that we need a DFS here and can not just move + // directly towards the center without recording intermediate nodes, even though the polygons + // are convex. In very rare we can get stuck due to contour simplification if we do not + // record nodes. + int cx = -1, cy = -1, ci = -1; + while (true) + { + if (array.size() < 3) + { + ctx->log(RC_LOG_WARNING, "Walk towards polygon center failed to reach center"); + break; + } + + ci = array.pop(); + cy = array.pop(); + cx = array.pop(); + + if (cx == pcx && cy == pcy) + break; + + // If we are already at the correct X-position, prefer direction + // directly towards the center in the Y-axis; otherwise prefer + // direction in the X-axis + int directDir; + if (cx == pcx) + directDir = rcGetDirForOffset(0, pcy > cy ? 1 : -1); + else + directDir = rcGetDirForOffset(pcx > cx ? 1 : -1, 0); + + // Push the direct dir last so we start with this on next iteration + rcSwap(dirs[directDir], dirs[3]); + + const rcCompactSpan& cs = chf.spans[ci]; + for (int i = 0; i < 4; i++) + { + int dir = dirs[i]; + if (rcGetCon(cs, dir) == RC_NOT_CONNECTED) + continue; + + int newX = cx + rcGetDirOffsetX(dir); + int newY = cy + rcGetDirOffsetY(dir); + + int hpx = newX - hp.xmin; + int hpy = newY - hp.ymin; + if (hpx < 0 || hpx >= hp.width || hpy < 0 || hpy >= hp.height) + continue; + + if (hp.data[hpx+hpy*hp.width] != 0) + continue; + + hp.data[hpx+hpy*hp.width] = 1; + array.push(newX); + array.push(newY); + array.push((int)chf.cells[(newX+bs)+(newY+bs)*chf.width].index + rcGetCon(cs, dir)); + } + + rcSwap(dirs[directDir], dirs[3]); + } + + array.resize(0); + // getHeightData seeds are given in coordinates with borders + array.push(cx+bs); + array.push(cy+bs); + array.push(ci); + + memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height); + const rcCompactSpan& cs = chf.spans[ci]; + hp.data[cx-hp.xmin+(cy-hp.ymin)*hp.width] = cs.y; +} + + +static void push3(rcIntArray& queue, int v1, int v2, int v3) +{ + queue.resize(queue.size() + 3); + queue[queue.size() - 3] = v1; + queue[queue.size() - 2] = v2; + queue[queue.size() - 1] = v3; +} + +static void getHeightData(rcContext* ctx, const rcCompactHeightfield& chf, + const unsigned short* poly, const int npoly, + const unsigned short* verts, const int bs, + rcHeightPatch& hp, rcIntArray& queue, + int region) +{ + // Note: Reads to the compact heightfield are offset by border size (bs) + // since border size offset is already removed from the polymesh vertices. + + queue.resize(0); + // Set all heights to RC_UNSET_HEIGHT. + memset(hp.data, 0xff, sizeof(unsigned short)*hp.width*hp.height); + + bool empty = true; + + // We cannot sample from this poly if it was created from polys + // of different regions. If it was then it could potentially be overlapping + // with polys of that region and the heights sampled here could be wrong. + if (region != RC_MULTIPLE_REGS) + { + // Copy the height from the same region, and mark region borders + // as seed points to fill the rest. + for (int hy = 0; hy < hp.height; hy++) + { + int y = hp.ymin + hy + bs; + for (int hx = 0; hx < hp.width; hx++) + { + int x = hp.xmin + hx + bs; + const rcCompactCell& c = chf.cells[x + y*chf.width]; + for (int i = (int)c.index, ni = (int)(c.index + c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + if (s.reg == region) + { + // Store height + hp.data[hx + hy*hp.width] = s.y; + empty = false; + + // If any of the neighbours is not in same region, + // add the current location as flood fill start + bool border = false; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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]; + if (as.reg != region) + { + border = true; + break; + } + } + } + if (border) + push3(queue, x, y, i); + break; + } + } + } + } + } + + // if the polygon does not contain any points from the current region (rare, but happens) + // or if it could potentially be overlapping polygons of the same region, + // then use the center as the seed point. + if (empty) + seedArrayWithPolyCenter(ctx, chf, poly, npoly, verts, bs, hp, queue); + + static const int RETRACT_SIZE = 256; + int head = 0; + + // We assume the seed is centered in the polygon, so a BFS to collect + // height data will ensure we do not move onto overlapping polygons and + // sample wrong heights. + while (head*3 < queue.size()) + { + int cx = queue[head*3+0]; + int cy = queue[head*3+1]; + int ci = queue[head*3+2]; + head++; + if (head >= RETRACT_SIZE) + { + head = 0; + if (queue.size() > RETRACT_SIZE*3) + memmove(&queue[0], &queue[RETRACT_SIZE*3], sizeof(int)*(queue.size()-RETRACT_SIZE*3)); + queue.resize(queue.size()-RETRACT_SIZE*3); + } + + const rcCompactSpan& cs = chf.spans[ci]; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(cs, dir) == RC_NOT_CONNECTED) continue; + + const int ax = cx + rcGetDirOffsetX(dir); + const int ay = cy + rcGetDirOffsetY(dir); + const int hx = ax - hp.xmin - bs; + const int hy = ay - hp.ymin - bs; + + if ((unsigned int)hx >= (unsigned int)hp.width || (unsigned int)hy >= (unsigned int)hp.height) + continue; + + if (hp.data[hx + hy*hp.width] != RC_UNSET_HEIGHT) + continue; + + const int ai = (int)chf.cells[ax + ay*chf.width].index + rcGetCon(cs, dir); + const rcCompactSpan& as = chf.spans[ai]; + + hp.data[hx + hy*hp.width] = as.y; + + push3(queue, ax, ay, 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; +} + +/// @par +/// +/// See the #rcConfig documentation for more information on the configuration parameters. +/// +/// @see rcAllocPolyMeshDetail, rcPolyMesh, rcCompactHeightfield, rcPolyMeshDetail, rcConfig +bool rcBuildPolyMeshDetail(rcContext* ctx, const rcPolyMesh& mesh, const rcCompactHeightfield& chf, + const float sampleDist, const float sampleMaxError, + rcPolyMeshDetail& dmesh) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_BUILD_POLYMESHDETAIL); + + if (mesh.nverts == 0 || mesh.npolys == 0) + return true; + + const int nvp = mesh.nvp; + const float cs = mesh.cs; + const float ch = mesh.ch; + const float* orig = mesh.bmin; + const int borderSize = mesh.borderSize; + const int heightSearchRadius = rcMax(1, (int)ceilf(mesh.maxEdgeError)); + + rcIntArray edges(64); + rcIntArray tris(512); + rcIntArray arr(512); + rcIntArray samples(512); + float verts[256*3]; + rcHeightPatch hp; + int nPolyVerts = 0; + int maxhw = 0, maxhh = 0; + + rcScopedDelete bounds((int*)rcAlloc(sizeof(int)*mesh.npolys*4, RC_ALLOC_TEMP)); + if (!bounds) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'bounds' (%d).", mesh.npolys*4); + return false; + } + rcScopedDelete poly((float*)rcAlloc(sizeof(float)*nvp*3, RC_ALLOC_TEMP)); + if (!poly) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'poly' (%d).", nvp*3); + return false; + } + + // 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] == RC_MESH_NULL_IDX) 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 = (unsigned short*)rcAlloc(sizeof(unsigned short)*maxhw*maxhh, RC_ALLOC_TEMP); + if (!hp.data) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'hp.data' (%d).", maxhw*maxhh); + return false; + } + + dmesh.nmeshes = mesh.npolys; + dmesh.nverts = 0; + dmesh.ntris = 0; + dmesh.meshes = (unsigned int*)rcAlloc(sizeof(unsigned int)*dmesh.nmeshes*4, RC_ALLOC_PERM); + if (!dmesh.meshes) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.meshes' (%d).", dmesh.nmeshes*4); + return false; + } + + int vcap = nPolyVerts+nPolyVerts/2; + int tcap = vcap*2; + + dmesh.nverts = 0; + dmesh.verts = (float*)rcAlloc(sizeof(float)*vcap*3, RC_ALLOC_PERM); + if (!dmesh.verts) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.verts' (%d).", vcap*3); + return false; + } + dmesh.ntris = 0; + dmesh.tris = (unsigned char*)rcAlloc(sizeof(unsigned char)*tcap*4, RC_ALLOC_PERM); + if (!dmesh.tris) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", tcap*4); + return false; + } + + for (int i = 0; i < mesh.npolys; ++i) + { + const unsigned short* p = &mesh.polys[i*nvp*2]; + + // Store polygon vertices for processing. + int npoly = 0; + for (int j = 0; j < nvp; ++j) + { + if(p[j] == RC_MESH_NULL_IDX) break; + const unsigned short* v = &mesh.verts[p[j]*3]; + poly[j*3+0] = v[0]*cs; + poly[j*3+1] = v[1]*ch; + poly[j*3+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(ctx, chf, p, npoly, mesh.verts, borderSize, hp, arr, mesh.regs[i]); + + // Build detail mesh. + int nverts = 0; + if (!buildPolyDetail(ctx, poly, npoly, + sampleDist, sampleMaxError, + heightSearchRadius, chf, hp, + verts, nverts, tris, + edges, samples)) + { + return false; + } + + // Move detail verts to world space. + for (int j = 0; j < nverts; ++j) + { + verts[j*3+0] += orig[0]; + verts[j*3+1] += orig[1] + chf.ch; // Is this offset necessary? + verts[j*3+2] += orig[2]; + } + // Offset poly too, will be used to flag checking. + for (int j = 0; j < npoly; ++j) + { + poly[j*3+0] += orig[0]; + poly[j*3+1] += orig[1]; + poly[j*3+2] += orig[2]; + } + + // Store detail submesh. + const int ntris = tris.size()/4; + + dmesh.meshes[i*4+0] = (unsigned int)dmesh.nverts; + dmesh.meshes[i*4+1] = (unsigned int)nverts; + dmesh.meshes[i*4+2] = (unsigned int)dmesh.ntris; + dmesh.meshes[i*4+3] = (unsigned int)ntris; + + // Store vertices, allocate more memory if necessary. + if (dmesh.nverts+nverts > vcap) + { + while (dmesh.nverts+nverts > vcap) + vcap += 256; + + float* newv = (float*)rcAlloc(sizeof(float)*vcap*3, RC_ALLOC_PERM); + if (!newv) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newv' (%d).", vcap*3); + return false; + } + if (dmesh.nverts) + memcpy(newv, dmesh.verts, sizeof(float)*3*dmesh.nverts); + rcFree(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 = (unsigned char*)rcAlloc(sizeof(unsigned char)*tcap*4, RC_ALLOC_PERM); + if (!newt) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'newt' (%d).", tcap*4); + return false; + } + if (dmesh.ntris) + memcpy(newt, dmesh.tris, sizeof(unsigned char)*4*dmesh.ntris); + rcFree(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++; + } + } + + return true; +} + +/// @see rcAllocPolyMeshDetail, rcPolyMeshDetail +bool rcMergePolyMeshDetails(rcContext* ctx, rcPolyMeshDetail** meshes, const int nmeshes, rcPolyMeshDetail& mesh) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_MERGE_POLYMESHDETAIL); + + 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 = (unsigned int*)rcAlloc(sizeof(unsigned int)*maxMeshes*4, RC_ALLOC_PERM); + if (!mesh.meshes) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'pmdtl.meshes' (%d).", maxMeshes*4); + return false; + } + + mesh.ntris = 0; + mesh.tris = (unsigned char*)rcAlloc(sizeof(unsigned char)*maxTris*4, RC_ALLOC_PERM); + if (!mesh.tris) + { + ctx->log(RC_LOG_ERROR, "rcBuildPolyMeshDetail: Out of memory 'dmesh.tris' (%d).", maxTris*4); + return false; + } + + mesh.nverts = 0; + mesh.verts = (float*)rcAlloc(sizeof(float)*maxVerts*3, RC_ALLOC_PERM); + if (!mesh.verts) + { + ctx->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 int* dst = &mesh.meshes[mesh.nmeshes*4]; + unsigned int* src = &dm->meshes[j*4]; + dst[0] = (unsigned int)mesh.nverts+src[0]; + dst[1] = src[1]; + dst[2] = (unsigned int)mesh.ntris+src[2]; + dst[3] = src[3]; + mesh.nmeshes++; + } + + for (int k = 0; k < dm->nverts; ++k) + { + rcVcopy(&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++; + } + } + + return true; +} diff --git a/lib/Recast/RecastRasterization.cpp b/lib/Recast/RecastRasterization.cpp new file mode 100644 index 0000000..a4cef74 --- /dev/null +++ b/lib/Recast/RecastRasterization.cpp @@ -0,0 +1,454 @@ +// +// Copyright (c) 2009-2010 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 +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastAssert.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. + rcSpanPool* pool = (rcSpanPool*)rcAlloc(sizeof(rcSpanPool), RC_ALLOC_PERM); + if (!pool) return 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 bool addSpan(rcHeightfield& hf, const int x, const int y, + const unsigned short smin, const unsigned short smax, + const unsigned char area, const int flagMergeThr) +{ + + int idx = x + y*hf.width; + + rcSpan* s = allocSpan(hf); + if (!s) + return false; + s->smin = smin; + s->smax = smax; + s->area = area; + s->next = 0; + + // Empty cell, add the first span. + if (!hf.spans[idx]) + { + hf.spans[idx] = s; + return true; + } + 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 (rcAbs((int)s->smax - (int)cur->smax) <= flagMergeThr) + s->area = rcMax(s->area, cur->area); + + // 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; + } + + return true; +} + +/// @par +/// +/// The span addition can be set to favor flags. If the span is merged to +/// another span and the new @p smax is within @p flagMergeThr units +/// from the existing span, the span flags are merged. +/// +/// @see rcHeightfield, rcSpan. +bool rcAddSpan(rcContext* ctx, rcHeightfield& hf, const int x, const int y, + const unsigned short smin, const unsigned short smax, + const unsigned char area, const int flagMergeThr) +{ + rcAssert(ctx); + + if (!addSpan(hf, x, y, smin, smax, area, flagMergeThr)) + { + ctx->log(RC_LOG_ERROR, "rcAddSpan: Out of memory."); + return false; + } + + return true; +} + +// divides a convex polygons into two convex polygons on both sides of a line +static void dividePoly(const float* in, int nin, + float* out1, int* nout1, + float* out2, int* nout2, + float x, int axis) +{ + float d[12]; + for (int i = 0; i < nin; ++i) + d[i] = x - in[i*3+axis]; + + int m = 0, n = 0; + for (int i = 0, j = nin-1; i < nin; j=i, ++i) + { + bool ina = d[j] >= 0; + bool inb = d[i] >= 0; + if (ina != inb) + { + float s = d[j] / (d[j] - d[i]); + out1[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s; + out1[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s; + out1[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s; + rcVcopy(out2 + n*3, out1 + m*3); + m++; + n++; + // add the i'th point to the right polygon. Do NOT add points that are on the dividing line + // since these were already added above + if (d[i] > 0) + { + rcVcopy(out1 + m*3, in + i*3); + m++; + } + else if (d[i] < 0) + { + rcVcopy(out2 + n*3, in + i*3); + n++; + } + } + else // same side + { + // add the i'th point to the right polygon. Addition is done even for points on the dividing line + if (d[i] >= 0) + { + rcVcopy(out1 + m*3, in + i*3); + m++; + if (d[i] != 0) + continue; + } + rcVcopy(out2 + n*3, in + i*3); + n++; + } + } + + *nout1 = m; + *nout2 = n; +} + + + +static bool rasterizeTri(const float* v0, const float* v1, const float* v2, + const unsigned char area, rcHeightfield& hf, + const float* bmin, const float* bmax, + const float cs, const float ics, const float ich, + const int flagMergeThr) +{ + 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. + rcVcopy(tmin, v0); + rcVcopy(tmax, v0); + rcVmin(tmin, v1); + rcVmin(tmin, v2); + rcVmax(tmax, v1); + rcVmax(tmax, v2); + + // If the triangle does not touch the bbox of the heightfield, skip the triagle. + if (!overlapBounds(bmin, bmax, tmin, tmax)) + return true; + + // Calculate the footprint of the triangle on the grid's y-axis + int y0 = (int)((tmin[2] - bmin[2])*ics); + int y1 = (int)((tmax[2] - bmin[2])*ics); + y0 = rcClamp(y0, 0, h-1); + y1 = rcClamp(y1, 0, h-1); + + // Clip the triangle into all grid cells it touches. + float buf[7*3*4]; + float *in = buf, *inrow = buf+7*3, *p1 = inrow+7*3, *p2 = p1+7*3; + + rcVcopy(&in[0], v0); + rcVcopy(&in[1*3], v1); + rcVcopy(&in[2*3], v2); + int nvrow, nvIn = 3; + + for (int y = y0; y <= y1; ++y) + { + // Clip polygon to row. Store the remaining polygon as well + const float cz = bmin[2] + y*cs; + dividePoly(in, nvIn, inrow, &nvrow, p1, &nvIn, cz+cs, 2); + rcSwap(in, p1); + if (nvrow < 3) continue; + + // find the horizontal bounds in the row + float minX = inrow[0], maxX = inrow[0]; + for (int i=1; i inrow[i*3]) minX = inrow[i*3]; + if (maxX < inrow[i*3]) maxX = inrow[i*3]; + } + int x0 = (int)((minX - bmin[0])*ics); + int x1 = (int)((maxX - bmin[0])*ics); + x0 = rcClamp(x0, 0, w-1); + x1 = rcClamp(x1, 0, w-1); + + int nv, nv2 = nvrow; + + for (int x = x0; x <= x1; ++x) + { + // Clip polygon to column. store the remaining polygon as well + const float cx = bmin[0] + x*cs; + dividePoly(inrow, nv2, p1, &nv, p2, &nv2, cx+cs, 0); + rcSwap(inrow, p2); + if (nv < 3) continue; + + // Calculate min and max of the span. + float smin = p1[1], smax = p1[1]; + for (int i = 1; i < nv; ++i) + { + smin = rcMin(smin, p1[i*3+1]); + smax = rcMax(smax, p1[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 = 0; + if (smax > by) smax = by; + + // Snap the span to the heightfield height grid. + unsigned short ismin = (unsigned short)rcClamp((int)floorf(smin * ich), 0, RC_SPAN_MAX_HEIGHT); + unsigned short ismax = (unsigned short)rcClamp((int)ceilf(smax * ich), (int)ismin+1, RC_SPAN_MAX_HEIGHT); + + if (!addSpan(hf, x, y, ismin, ismax, area, flagMergeThr)) + return false; + } + } + + return true; +} + +/// @par +/// +/// No spans will be added if the triangle does not overlap the heightfield grid. +/// +/// @see rcHeightfield +bool rcRasterizeTriangle(rcContext* ctx, const float* v0, const float* v1, const float* v2, + const unsigned char area, rcHeightfield& solid, + const int flagMergeThr) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES); + + const float ics = 1.0f/solid.cs; + const float ich = 1.0f/solid.ch; + if (!rasterizeTri(v0, v1, v2, area, solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr)) + { + ctx->log(RC_LOG_ERROR, "rcRasterizeTriangle: Out of memory."); + return false; + } + + return true; +} + +/// @par +/// +/// Spans will only be added for triangles that overlap the heightfield grid. +/// +/// @see rcHeightfield +bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/, + const int* tris, const unsigned char* areas, const int nt, + rcHeightfield& solid, const int flagMergeThr) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES); + + 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. + if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr)) + { + ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory."); + return false; + } + } + + return true; +} + +/// @par +/// +/// Spans will only be added for triangles that overlap the heightfield grid. +/// +/// @see rcHeightfield +bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/, + const unsigned short* tris, const unsigned char* areas, const int nt, + rcHeightfield& solid, const int flagMergeThr) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES); + + 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. + if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr)) + { + ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory."); + return false; + } + } + + return true; +} + +/// @par +/// +/// Spans will only be added for triangles that overlap the heightfield grid. +/// +/// @see rcHeightfield +bool rcRasterizeTriangles(rcContext* ctx, const float* verts, const unsigned char* areas, const int nt, + rcHeightfield& solid, const int flagMergeThr) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_RASTERIZE_TRIANGLES); + + 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[(i*3+0)*3]; + const float* v1 = &verts[(i*3+1)*3]; + const float* v2 = &verts[(i*3+2)*3]; + // Rasterize. + if (!rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr)) + { + ctx->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory."); + return false; + } + } + + return true; +} diff --git a/lib/Recast/RecastRegion.cpp b/lib/Recast/RecastRegion.cpp new file mode 100644 index 0000000..38a2bd6 --- /dev/null +++ b/lib/Recast/RecastRegion.cpp @@ -0,0 +1,1824 @@ +// +// Copyright (c) 2009-2010 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 +#define _USE_MATH_DEFINES +#include +#include +#include +#include +#include "Recast.h" +#include "RecastAlloc.h" +#include "RecastAssert.h" +#include + + +static void calculateDistanceField(rcCompactHeightfield& chf, unsigned short* src, 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]; + const unsigned char area = chf.areas[i]; + + int nc = 0; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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 (area == chf.areas[ai]) + 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) != RC_NOT_CONNECTED) + { + // (-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) != RC_NOT_CONNECTED) + { + 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) != RC_NOT_CONNECTED) + { + // (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) != RC_NOT_CONNECTED) + { + 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) != RC_NOT_CONNECTED) + { + // (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) != RC_NOT_CONNECTED) + { + 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) != RC_NOT_CONNECTED) + { + // (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) != RC_NOT_CONNECTED) + { + 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); + +} + +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]; + const unsigned short cd = src[i]; + if (cd <= thr) + { + dst[i] = cd; + continue; + } + + int d = (int)cd; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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) != RC_NOT_CONNECTED) + { + 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 r, + rcCompactHeightfield& chf, + unsigned short* srcReg, unsigned short* srcDist, + rcIntArray& stack) +{ + const int w = chf.width; + + const unsigned char area = chf.areas[i]; + + // Flood fill mark region. + stack.resize(0); + stack.push((int)x); + stack.push((int)y); + stack.push((int)i); + srcReg[i] = r; + srcDist[i] = 0; + + unsigned short lev = level >= 2 ? level-2 : 0; + 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) != RC_NOT_CONNECTED) + { + 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.areas[ai] != area) + continue; + unsigned short nr = srcReg[ai]; + if (nr & RC_BORDER_REG) // Do not take borders into account. + continue; + if (nr != 0 && nr != r) + { + ar = nr; + break; + } + + const rcCompactSpan& as = chf.spans[ai]; + + const int dir2 = (dir+1) & 0x3; + if (rcGetCon(as, dir2) != RC_NOT_CONNECTED) + { + 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); + if (chf.areas[ai2] != area) + continue; + unsigned short nr2 = srcReg[ai2]; + if (nr2 != 0 && nr2 != r) + { + ar = nr2; + break; + } + } + } + } + if (ar != 0) + { + srcReg[ci] = 0; + continue; + } + + count++; + + // Expand neighbours. + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(cs, dir) != RC_NOT_CONNECTED) + { + 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.areas[ai] != area) + continue; + if (chf.dist[ai] >= lev && srcReg[ai] == 0) + { + srcReg[ai] = r; + srcDist[ai] = 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* srcReg, unsigned short* srcDist, + unsigned short* dstReg, unsigned short* dstDist, + rcIntArray& stack, + bool fillStack) +{ + const int w = chf.width; + const int h = chf.height; + + if (fillStack) + { + // 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.dist[i] >= level && srcReg[i] == 0 && chf.areas[i] != RC_NULL_AREA) + { + stack.push(x); + stack.push(y); + stack.push(i); + } + } + } + } + } + else // use cells in the input stack + { + // mark all cells which already have a region + for (int j=0; j 0) + { + int failed = 0; + + memcpy(dstReg, srcReg, sizeof(unsigned short)*chf.spanCount); + memcpy(dstDist, srcDist, sizeof(unsigned short)*chf.spanCount); + + 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 = srcReg[i]; + unsigned short d2 = 0xffff; + const unsigned char area = chf.areas[i]; + const rcCompactSpan& s = chf.spans[i]; + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) == RC_NOT_CONNECTED) 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 (chf.areas[ai] != area) continue; + if (srcReg[ai] > 0 && (srcReg[ai] & RC_BORDER_REG) == 0) + { + if ((int)srcDist[ai]+2 < (int)d2) + { + r = srcReg[ai]; + d2 = srcDist[ai]+2; + } + } + } + if (r) + { + stack[j+2] = -1; // mark as used + dstReg[i] = r; + dstDist[i] = d2; + } + else + { + failed++; + } + } + + // rcSwap source and dest. + rcSwap(srcReg, dstReg); + rcSwap(srcDist, dstDist); + + if (failed*3 == stack.size()) + break; + + if (level > 0) + { + ++iter; + if (iter >= maxIter) + break; + } + } + + return srcReg; +} + + + +static void sortCellsByLevel(unsigned short startLevel, + rcCompactHeightfield& chf, + unsigned short* srcReg, + unsigned int nbStacks, rcIntArray* stacks, + unsigned short loglevelsPerStack) // the levels per stack (2 in our case) as a bit shift +{ + const int w = chf.width; + const int h = chf.height; + startLevel = startLevel >> loglevelsPerStack; + + for (unsigned int j=0; j> loglevelsPerStack; + int sId = startLevel - level; + if (sId >= (int)nbStacks) + continue; + if (sId < 0) + sId = 0; + + stacks[sId].push(x); + stacks[sId].push(y); + stacks[sId].push(i); + } + } + } +} + + +static void appendStacks(rcIntArray& srcStack, rcIntArray& dstStack, + unsigned short* srcReg) +{ + for (int j=0; j 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(const rcRegion& rega, const rcRegion& regb) +{ + if (rega.areaType != regb.areaType) + return false; + int n = 0; + for (int i = 0; i < rega.connections.size(); ++i) + { + if (rega.connections[i] == regb.id) + n++; + } + if (n > 1) + return false; + for (int i = 0; i < rega.floors.size(); ++i) + { + if (rega.floors[i] == regb.id) + return false; + } + return true; +} + +static void addUniqueFloorRegion(rcRegion& reg, int 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.spanCount += regb.spanCount; + regb.spanCount = 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* srcReg, + int x, int y, int i, int dir) +{ + const rcCompactSpan& s = chf.spans[i]; + unsigned short r = 0; + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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 = srcReg[ai]; + } + if (r == srcReg[i]) + return false; + return true; +} + +static void walkContour(int x, int y, int i, int dir, + rcCompactHeightfield& chf, + unsigned short* srcReg, + rcIntArray& cont) +{ + int startDir = dir; + int starti = i; + + const rcCompactSpan& ss = chf.spans[i]; + unsigned short curReg = 0; + if (rcGetCon(ss, dir) != RC_NOT_CONNECTED) + { + 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 = srcReg[ai]; + } + cont.push(curReg); + + int iter = 0; + while (++iter < 40000) + { + const rcCompactSpan& s = chf.spans[i]; + + if (isSolidEdge(chf, srcReg, x, y, i, dir)) + { + // Choose the edge corner + unsigned short r = 0; + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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 = srcReg[ai]; + } + 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) != RC_NOT_CONNECTED) + { + 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 j = 0; j < cont.size(); ) + { + int nj = (j+1) % cont.size(); + if (cont[j] == cont[nj]) + { + for (int k = j; k < cont.size()-1; ++k) + cont[k] = cont[k+1]; + cont.pop(); + } + else + ++j; + } + } +} + + +static bool mergeAndFilterRegions(rcContext* ctx, int minRegionArea, int mergeRegionSize, + unsigned short& maxRegionId, + rcCompactHeightfield& chf, + unsigned short* srcReg, rcIntArray& overlaps) +{ + const int w = chf.width; + const int h = chf.height; + + const int nreg = maxRegionId+1; + rcRegion* regions = (rcRegion*)rcAlloc(sizeof(rcRegion)*nreg, RC_ALLOC_TEMP); + if (!regions) + { + ctx->log(RC_LOG_ERROR, "mergeAndFilterRegions: Out of memory 'regions' (%d).", nreg); + return false; + } + + // Construct regions + for (int i = 0; i < nreg; ++i) + new(®ions[i]) rcRegion((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 = srcReg[i]; + if (r == 0 || r >= nreg) + continue; + + rcRegion& reg = regions[r]; + reg.spanCount++; + + // Update floors. + for (int j = (int)c.index; j < ni; ++j) + { + if (i == j) continue; + unsigned short floorId = srcReg[j]; + if (floorId == 0 || floorId >= nreg) + continue; + if (floorId == r) + reg.overlap = true; + addUniqueFloorRegion(reg, floorId); + } + + // Have found contour + if (reg.connections.size() > 0) + continue; + + reg.areaType = chf.areas[i]; + + // Check if this cell is next to a border. + int ndir = -1; + for (int dir = 0; dir < 4; ++dir) + { + if (isSolidEdge(chf, srcReg, 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, srcReg, reg.connections); + } + } + } + } + + // Remove too small regions. + rcIntArray stack(32); + rcIntArray trace(32); + for (int i = 0; i < nreg; ++i) + { + rcRegion& reg = regions[i]; + if (reg.id == 0 || (reg.id & RC_BORDER_REG)) + continue; + if (reg.spanCount == 0) + continue; + if (reg.visited) + continue; + + // Count the total size of all the connected regions. + // Also keep track of the regions connects to a tile border. + bool connectsToBorder = false; + int spanCount = 0; + stack.resize(0); + trace.resize(0); + + reg.visited = true; + stack.push(i); + + while (stack.size()) + { + // Pop + int ri = stack.pop(); + + rcRegion& creg = regions[ri]; + + spanCount += creg.spanCount; + trace.push(ri); + + for (int j = 0; j < creg.connections.size(); ++j) + { + if (creg.connections[j] & RC_BORDER_REG) + { + connectsToBorder = true; + continue; + } + rcRegion& neireg = regions[creg.connections[j]]; + if (neireg.visited) + continue; + if (neireg.id == 0 || (neireg.id & RC_BORDER_REG)) + continue; + // Visit + stack.push(neireg.id); + neireg.visited = true; + } + } + + // If the accumulated regions size is too small, remove it. + // Do not remove areas which connect to tile borders + // as their size cannot be estimated correctly and removing them + // can potentially remove necessary areas. + if (spanCount < minRegionArea && !connectsToBorder) + { + // Kill all visited regions. + for (int j = 0; j < trace.size(); ++j) + { + regions[trace[j]].spanCount = 0; + regions[trace[j]].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.overlap) + continue; + if (reg.spanCount == 0) + continue; + + // Check to see if the region should be merged. + if (reg.spanCount > 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) || mreg.overlap) continue; + if (mreg.spanCount < smallest && + canMergeWithRegion(reg, mreg) && + canMergeWithRegion(mreg, reg)) + { + smallest = mreg.spanCount; + 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 ((srcReg[i] & RC_BORDER_REG) == 0) + srcReg[i] = regions[srcReg[i]].id; + } + + // Return regions that we found to be overlapping. + for (int i = 0; i < nreg; ++i) + if (regions[i].overlap) + overlaps.push(regions[i].id); + + for (int i = 0; i < nreg; ++i) + regions[i].~rcRegion(); + rcFree(regions); + + + return true; +} + + +static void addUniqueConnection(rcRegion& reg, int n) +{ + for (int i = 0; i < reg.connections.size(); ++i) + if (reg.connections[i] == n) + return; + reg.connections.push(n); +} + +static bool mergeAndFilterLayerRegions(rcContext* ctx, int minRegionArea, + unsigned short& maxRegionId, + rcCompactHeightfield& chf, + unsigned short* srcReg, rcIntArray& /*overlaps*/) +{ + const int w = chf.width; + const int h = chf.height; + + const int nreg = maxRegionId+1; + rcRegion* regions = (rcRegion*)rcAlloc(sizeof(rcRegion)*nreg, RC_ALLOC_TEMP); + if (!regions) + { + ctx->log(RC_LOG_ERROR, "mergeAndFilterLayerRegions: Out of memory 'regions' (%d).", nreg); + return false; + } + + // Construct regions + for (int i = 0; i < nreg; ++i) + new(®ions[i]) rcRegion((unsigned short)i); + + // Find region neighbours and overlapping regions. + rcIntArray lregs(32); + for (int y = 0; y < h; ++y) + { + for (int x = 0; x < w; ++x) + { + const rcCompactCell& c = chf.cells[x+y*w]; + + lregs.resize(0); + + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i) + { + const rcCompactSpan& s = chf.spans[i]; + const unsigned short ri = srcReg[i]; + if (ri == 0 || ri >= nreg) continue; + rcRegion& reg = regions[ri]; + + reg.spanCount++; + + reg.ymin = rcMin(reg.ymin, s.y); + reg.ymax = rcMax(reg.ymax, s.y); + + // Collect all region layers. + lregs.push(ri); + + // Update neighbours + for (int dir = 0; dir < 4; ++dir) + { + if (rcGetCon(s, dir) != RC_NOT_CONNECTED) + { + 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 unsigned short rai = srcReg[ai]; + if (rai > 0 && rai < nreg && rai != ri) + addUniqueConnection(reg, rai); + if (rai & RC_BORDER_REG) + reg.connectsToBorder = true; + } + } + + } + + // Update overlapping regions. + for (int i = 0; i < lregs.size()-1; ++i) + { + for (int j = i+1; j < lregs.size(); ++j) + { + if (lregs[i] != lregs[j]) + { + rcRegion& ri = regions[lregs[i]]; + rcRegion& rj = regions[lregs[j]]; + addUniqueFloorRegion(ri, lregs[j]); + addUniqueFloorRegion(rj, lregs[i]); + } + } + } + + } + } + + // Create 2D layers from regions. + unsigned short layerId = 1; + + for (int i = 0; i < nreg; ++i) + regions[i].id = 0; + + // Merge montone regions to create non-overlapping areas. + rcIntArray stack(32); + for (int i = 1; i < nreg; ++i) + { + rcRegion& root = regions[i]; + // Skip already visited. + if (root.id != 0) + continue; + + // Start search. + root.id = layerId; + + stack.resize(0); + stack.push(i); + + while (stack.size() > 0) + { + // Pop front + rcRegion& reg = regions[stack[0]]; + for (int j = 0; j < stack.size()-1; ++j) + stack[j] = stack[j+1]; + stack.resize(stack.size()-1); + + const int ncons = (int)reg.connections.size(); + for (int j = 0; j < ncons; ++j) + { + const int nei = reg.connections[j]; + rcRegion& regn = regions[nei]; + // Skip already visited. + if (regn.id != 0) + continue; + // Skip if the neighbour is overlapping root region. + bool overlap = false; + for (int k = 0; k < root.floors.size(); k++) + { + if (root.floors[k] == nei) + { + overlap = true; + break; + } + } + if (overlap) + continue; + + // Deepen + stack.push(nei); + + // Mark layer id + regn.id = layerId; + // Merge current layers to root. + for (int k = 0; k < regn.floors.size(); ++k) + addUniqueFloorRegion(root, regn.floors[k]); + root.ymin = rcMin(root.ymin, regn.ymin); + root.ymax = rcMax(root.ymax, regn.ymax); + root.spanCount += regn.spanCount; + regn.spanCount = 0; + root.connectsToBorder = root.connectsToBorder || regn.connectsToBorder; + } + } + + layerId++; + } + + // Remove small regions + for (int i = 0; i < nreg; ++i) + { + if (regions[i].spanCount > 0 && regions[i].spanCount < minRegionArea && !regions[i].connectsToBorder) + { + unsigned short reg = regions[i].id; + for (int j = 0; j < nreg; ++j) + if (regions[j].id == reg) + regions[j].id = 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 ((srcReg[i] & RC_BORDER_REG) == 0) + srcReg[i] = regions[srcReg[i]].id; + } + + for (int i = 0; i < nreg; ++i) + regions[i].~rcRegion(); + rcFree(regions); + + return true; +} + + + +/// @par +/// +/// This is usually the second to the last step in creating a fully built +/// compact heightfield. This step is required before regions are built +/// using #rcBuildRegions or #rcBuildRegionsMonotone. +/// +/// After this step, the distance data is available via the rcCompactHeightfield::maxDistance +/// and rcCompactHeightfield::dist fields. +/// +/// @see rcCompactHeightfield, rcBuildRegions, rcBuildRegionsMonotone +bool rcBuildDistanceField(rcContext* ctx, rcCompactHeightfield& chf) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_BUILD_DISTANCEFIELD); + + if (chf.dist) + { + rcFree(chf.dist); + chf.dist = 0; + } + + unsigned short* src = (unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_TEMP); + if (!src) + { + ctx->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'src' (%d).", chf.spanCount); + return false; + } + unsigned short* dst = (unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_TEMP); + if (!dst) + { + ctx->log(RC_LOG_ERROR, "rcBuildDistanceField: Out of memory 'dst' (%d).", chf.spanCount); + rcFree(src); + return false; + } + + unsigned short maxDist = 0; + + { + rcScopedTimer timerDist(ctx, RC_TIMER_BUILD_DISTANCEFIELD_DIST); + + calculateDistanceField(chf, src, maxDist); + chf.maxDistance = maxDist; + } + + { + rcScopedTimer timerBlur(ctx, RC_TIMER_BUILD_DISTANCEFIELD_BLUR); + + // Blur + if (boxBlur(chf, 1, src, dst) != src) + rcSwap(src, dst); + + // Store distance. + chf.dist = src; + } + + rcFree(dst); + + return true; +} + +static void paintRectRegion(int minx, int maxx, int miny, int maxy, unsigned short regId, + rcCompactHeightfield& chf, unsigned short* srcReg) +{ + 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.areas[i] != RC_NULL_AREA) + srcReg[i] = regId; + } + } + } +} + + +static const unsigned short RC_NULL_NEI = 0xffff; + +struct rcSweepSpan +{ + unsigned short rid; // row id + unsigned short id; // region id + unsigned short ns; // number samples + unsigned short nei; // neighbour id +}; + +/// @par +/// +/// Non-null regions will consist of connected, non-overlapping walkable spans that form a single contour. +/// Contours will form simple polygons. +/// +/// If multiple regions form an area that is smaller than @p minRegionArea, then all spans will be +/// re-assigned to the zero (null) region. +/// +/// Partitioning can result in smaller than necessary regions. @p mergeRegionArea helps +/// reduce unecessarily small regions. +/// +/// See the #rcConfig documentation for more information on the configuration parameters. +/// +/// The region data will be available via the rcCompactHeightfield::maxRegions +/// and rcCompactSpan::reg fields. +/// +/// @warning The distance field must be created using #rcBuildDistanceField before attempting to build regions. +/// +/// @see rcCompactHeightfield, rcCompactSpan, rcBuildDistanceField, rcBuildRegionsMonotone, rcConfig +bool rcBuildRegionsMonotone(rcContext* ctx, rcCompactHeightfield& chf, + const int borderSize, const int minRegionArea, const int mergeRegionArea) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_BUILD_REGIONS); + + const int w = chf.width; + const int h = chf.height; + unsigned short id = 1; + + rcScopedDelete srcReg((unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_TEMP)); + if (!srcReg) + { + ctx->log(RC_LOG_ERROR, "rcBuildRegionsMonotone: Out of memory 'src' (%d).", chf.spanCount); + return false; + } + memset(srcReg,0,sizeof(unsigned short)*chf.spanCount); + + const int nsweeps = rcMax(chf.width,chf.height); + rcScopedDelete sweeps((rcSweepSpan*)rcAlloc(sizeof(rcSweepSpan)*nsweeps, RC_ALLOC_TEMP)); + if (!sweeps) + { + ctx->log(RC_LOG_ERROR, "rcBuildRegionsMonotone: Out of memory 'sweeps' (%d).", nsweeps); + return false; + } + + + // Mark border regions. + if (borderSize > 0) + { + // Make sure border will not overflow. + const int bw = rcMin(w, borderSize); + const int bh = rcMin(h, borderSize); + // Paint regions + paintRectRegion(0, bw, 0, h, id|RC_BORDER_REG, chf, srcReg); id++; + paintRectRegion(w-bw, w, 0, h, id|RC_BORDER_REG, chf, srcReg); id++; + paintRectRegion(0, w, 0, bh, id|RC_BORDER_REG, chf, srcReg); id++; + paintRectRegion(0, w, h-bh, h, id|RC_BORDER_REG, chf, srcReg); id++; + + chf.borderSize = borderSize; + } + + rcIntArray prev(256); + + // Sweep one line at a time. + for (int y = borderSize; y < h-borderSize; ++y) + { + // Collect spans from this row. + prev.resize(id+1); + memset(&prev[0],0,sizeof(int)*id); + unsigned short rid = 1; + + for (int x = borderSize; x < w-borderSize; ++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 (chf.areas[i] == RC_NULL_AREA) continue; + + // -x + unsigned short previd = 0; + if (rcGetCon(s, 0) != RC_NOT_CONNECTED) + { + 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); + if ((srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai]) + previd = srcReg[ai]; + } + + if (!previd) + { + previd = rid++; + sweeps[previd].rid = previd; + sweeps[previd].ns = 0; + sweeps[previd].nei = 0; + } + + // -y + if (rcGetCon(s,3) != RC_NOT_CONNECTED) + { + 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); + if (srcReg[ai] && (srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai]) + { + unsigned short nr = srcReg[ai]; + if (!sweeps[previd].nei || sweeps[previd].nei == nr) + { + sweeps[previd].nei = nr; + sweeps[previd].ns++; + prev[nr]++; + } + else + { + sweeps[previd].nei = RC_NULL_NEI; + } + } + } + + srcReg[i] = previd; + } + } + + // Create unique ID. + for (int i = 1; i < rid; ++i) + { + if (sweeps[i].nei != RC_NULL_NEI && sweeps[i].nei != 0 && + prev[sweeps[i].nei] == (int)sweeps[i].ns) + { + sweeps[i].id = sweeps[i].nei; + } + else + { + sweeps[i].id = id++; + } + } + + // Remap IDs + for (int x = borderSize; x < w-borderSize; ++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 (srcReg[i] > 0 && srcReg[i] < rid) + srcReg[i] = sweeps[srcReg[i]].id; + } + } + } + + + { + rcScopedTimer timerFilter(ctx, RC_TIMER_BUILD_REGIONS_FILTER); + + // Merge regions and filter out small regions. + rcIntArray overlaps; + chf.maxRegions = id; + if (!mergeAndFilterRegions(ctx, minRegionArea, mergeRegionArea, chf.maxRegions, chf, srcReg, overlaps)) + return false; + + // Monotone partitioning does not generate overlapping regions. + } + + // Store the result out. + for (int i = 0; i < chf.spanCount; ++i) + chf.spans[i].reg = srcReg[i]; + + return true; +} + +/// @par +/// +/// Non-null regions will consist of connected, non-overlapping walkable spans that form a single contour. +/// Contours will form simple polygons. +/// +/// If multiple regions form an area that is smaller than @p minRegionArea, then all spans will be +/// re-assigned to the zero (null) region. +/// +/// Watershed partitioning can result in smaller than necessary regions, especially in diagonal corridors. +/// @p mergeRegionArea helps reduce unecessarily small regions. +/// +/// See the #rcConfig documentation for more information on the configuration parameters. +/// +/// The region data will be available via the rcCompactHeightfield::maxRegions +/// and rcCompactSpan::reg fields. +/// +/// @warning The distance field must be created using #rcBuildDistanceField before attempting to build regions. +/// +/// @see rcCompactHeightfield, rcCompactSpan, rcBuildDistanceField, rcBuildRegionsMonotone, rcConfig +bool rcBuildRegions(rcContext* ctx, rcCompactHeightfield& chf, + const int borderSize, const int minRegionArea, const int mergeRegionArea) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_BUILD_REGIONS); + + const int w = chf.width; + const int h = chf.height; + + rcScopedDelete buf((unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount*4, RC_ALLOC_TEMP)); + if (!buf) + { + ctx->log(RC_LOG_ERROR, "rcBuildRegions: Out of memory 'tmp' (%d).", chf.spanCount*4); + return false; + } + + ctx->startTimer(RC_TIMER_BUILD_REGIONS_WATERSHED); + + const int LOG_NB_STACKS = 3; + const int NB_STACKS = 1 << LOG_NB_STACKS; + rcIntArray lvlStacks[NB_STACKS]; + for (int i=0; i 0) + { + // Make sure border will not overflow. + const int bw = rcMin(w, borderSize); + const int bh = rcMin(h, borderSize); + + // Paint regions + paintRectRegion(0, bw, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + paintRectRegion(w-bw, w, 0, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + paintRectRegion(0, w, 0, bh, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + paintRectRegion(0, w, h-bh, h, regionId|RC_BORDER_REG, chf, srcReg); regionId++; + + chf.borderSize = borderSize; + } + + int sId = -1; + while (level > 0) + { + level = level >= 2 ? level-2 : 0; + sId = (sId+1) & (NB_STACKS-1); + +// ctx->startTimer(RC_TIMER_DIVIDE_TO_LEVELS); + + if (sId == 0) + sortCellsByLevel(level, chf, srcReg, NB_STACKS, lvlStacks, 1); + else + appendStacks(lvlStacks[sId-1], lvlStacks[sId], srcReg); // copy left overs from last level + +// ctx->stopTimer(RC_TIMER_DIVIDE_TO_LEVELS); + + { + rcScopedTimer timerExpand(ctx, RC_TIMER_BUILD_REGIONS_EXPAND); + + // Expand current regions until no empty connected cells found. + if (expandRegions(expandIters, level, chf, srcReg, srcDist, dstReg, dstDist, lvlStacks[sId], false) != srcReg) + { + rcSwap(srcReg, dstReg); + rcSwap(srcDist, dstDist); + } + } + + { + rcScopedTimer timerFloor(ctx, RC_TIMER_BUILD_REGIONS_FLOOD); + + // Mark new regions with IDs. + for (int j = 0; j= 0 && srcReg[i] == 0) + { + if (floodRegion(x, y, i, level, regionId, chf, srcReg, srcDist, stack)) + { + if (regionId == 0xFFFF) + { + ctx->log(RC_LOG_ERROR, "rcBuildRegions: Region ID overflow"); + return false; + } + + regionId++; + } + } + } + } + } + + // Expand current regions until no empty connected cells found. + if (expandRegions(expandIters*8, 0, chf, srcReg, srcDist, dstReg, dstDist, stack, true) != srcReg) + { + rcSwap(srcReg, dstReg); + rcSwap(srcDist, dstDist); + } + + ctx->stopTimer(RC_TIMER_BUILD_REGIONS_WATERSHED); + + { + rcScopedTimer timerFilter(ctx, RC_TIMER_BUILD_REGIONS_FILTER); + + // Merge regions and filter out smalle regions. + rcIntArray overlaps; + chf.maxRegions = regionId; + if (!mergeAndFilterRegions(ctx, minRegionArea, mergeRegionArea, chf.maxRegions, chf, srcReg, overlaps)) + return false; + + // If overlapping regions were found during merging, split those regions. + if (overlaps.size() > 0) + { + ctx->log(RC_LOG_ERROR, "rcBuildRegions: %d overlapping regions.", overlaps.size()); + } + } + + // Write the result out. + for (int i = 0; i < chf.spanCount; ++i) + chf.spans[i].reg = srcReg[i]; + + return true; +} + + +bool rcBuildLayerRegions(rcContext* ctx, rcCompactHeightfield& chf, + const int borderSize, const int minRegionArea) +{ + rcAssert(ctx); + + rcScopedTimer timer(ctx, RC_TIMER_BUILD_REGIONS); + + const int w = chf.width; + const int h = chf.height; + unsigned short id = 1; + + rcScopedDelete srcReg((unsigned short*)rcAlloc(sizeof(unsigned short)*chf.spanCount, RC_ALLOC_TEMP)); + if (!srcReg) + { + ctx->log(RC_LOG_ERROR, "rcBuildLayerRegions: Out of memory 'src' (%d).", chf.spanCount); + return false; + } + memset(srcReg,0,sizeof(unsigned short)*chf.spanCount); + + const int nsweeps = rcMax(chf.width,chf.height); + rcScopedDelete sweeps((rcSweepSpan*)rcAlloc(sizeof(rcSweepSpan)*nsweeps, RC_ALLOC_TEMP)); + if (!sweeps) + { + ctx->log(RC_LOG_ERROR, "rcBuildLayerRegions: Out of memory 'sweeps' (%d).", nsweeps); + return false; + } + + + // Mark border regions. + if (borderSize > 0) + { + // Make sure border will not overflow. + const int bw = rcMin(w, borderSize); + const int bh = rcMin(h, borderSize); + // Paint regions + paintRectRegion(0, bw, 0, h, id|RC_BORDER_REG, chf, srcReg); id++; + paintRectRegion(w-bw, w, 0, h, id|RC_BORDER_REG, chf, srcReg); id++; + paintRectRegion(0, w, 0, bh, id|RC_BORDER_REG, chf, srcReg); id++; + paintRectRegion(0, w, h-bh, h, id|RC_BORDER_REG, chf, srcReg); id++; + + chf.borderSize = borderSize; + } + + rcIntArray prev(256); + + // Sweep one line at a time. + for (int y = borderSize; y < h-borderSize; ++y) + { + // Collect spans from this row. + prev.resize(id+1); + memset(&prev[0],0,sizeof(int)*id); + unsigned short rid = 1; + + for (int x = borderSize; x < w-borderSize; ++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 (chf.areas[i] == RC_NULL_AREA) continue; + + // -x + unsigned short previd = 0; + if (rcGetCon(s, 0) != RC_NOT_CONNECTED) + { + 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); + if ((srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai]) + previd = srcReg[ai]; + } + + if (!previd) + { + previd = rid++; + sweeps[previd].rid = previd; + sweeps[previd].ns = 0; + sweeps[previd].nei = 0; + } + + // -y + if (rcGetCon(s,3) != RC_NOT_CONNECTED) + { + 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); + if (srcReg[ai] && (srcReg[ai] & RC_BORDER_REG) == 0 && chf.areas[i] == chf.areas[ai]) + { + unsigned short nr = srcReg[ai]; + if (!sweeps[previd].nei || sweeps[previd].nei == nr) + { + sweeps[previd].nei = nr; + sweeps[previd].ns++; + prev[nr]++; + } + else + { + sweeps[previd].nei = RC_NULL_NEI; + } + } + } + + srcReg[i] = previd; + } + } + + // Create unique ID. + for (int i = 1; i < rid; ++i) + { + if (sweeps[i].nei != RC_NULL_NEI && sweeps[i].nei != 0 && + prev[sweeps[i].nei] == (int)sweeps[i].ns) + { + sweeps[i].id = sweeps[i].nei; + } + else + { + sweeps[i].id = id++; + } + } + + // Remap IDs + for (int x = borderSize; x < w-borderSize; ++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 (srcReg[i] > 0 && srcReg[i] < rid) + srcReg[i] = sweeps[srcReg[i]].id; + } + } + } + + + { + rcScopedTimer timerFilter(ctx, RC_TIMER_BUILD_REGIONS_FILTER); + + // Merge monotone regions to layers and remove small regions. + rcIntArray overlaps; + chf.maxRegions = id; + if (!mergeAndFilterLayerRegions(ctx, minRegionArea, chf.maxRegions, chf, srcReg, overlaps)) + return false; + } + + + // Store the result out. + for (int i = 0; i < chf.spanCount; ++i) + chf.spans[i].reg = srcReg[i]; + + return true; +} diff --git a/lib/gpc/gpc.cpp.h b/lib/gpc/gpc.cpp.h index f317ab1..1f8371f 100755 --- a/lib/gpc/gpc.cpp.h +++ b/lib/gpc/gpc.cpp.h @@ -3,8 +3,8 @@ Project: Generic Polygon Clipper - A new algorithm for calculating the difference, intersection, - exclusive-or or union of arbitrary polygon sets. + A new algorithm for calculating the difference, intersection, + exclusive-or or union of arbitrary polygon sets. File: gpc.c Author: Alan Murta (email: gpc@cs.man.ac.uk) @@ -12,28 +12,30 @@ Version: 2.32 Date: 17th December 2004 Copyright: (C) Advanced Interfaces Group, - University of Manchester. + University of Manchester. - This software is free for non-commercial use. It may be copied, - modified, and redistributed provided that this copyright notice - is preserved on all copies. The intellectual property rights of - the algorithms used reside with the University of Manchester - Advanced Interfaces Group. + This software is free for non-commercial use. It may be copied, + modified, and redistributed provided that this copyright notice + is preserved on all copies. The intellectual property rights of + the algorithms used reside with the University of Manchester + Advanced Interfaces Group. - You may not use this software, in whole or in part, in support - of any commercial product without the express consent of the - author. + You may not use this software, in whole or in part, in support + of any commercial product without the express consent of the + author. - There is no warranty or other guarantee of fitness of this - software for any purpose. It is provided solely "as is". + There is no warranty or other guarantee of fitness of this + software for any purpose. It is provided solely "as is". =========================================================================== */ +#ifndef GPC_CPP_H +#define GPC_CPP_H /* =========================================================================== - Includes + Includes =========================================================================== */ @@ -45,7 +47,7 @@ Copyright: (C) Advanced Interfaces Group, /* =========================================================================== - Constants + Constants =========================================================================== */ @@ -68,7 +70,7 @@ Copyright: (C) Advanced Interfaces Group, /* =========================================================================== - Macros + Macros =========================================================================== */ @@ -78,40 +80,40 @@ Copyright: (C) Advanced Interfaces Group, #define NEXT_INDEX(i, n) ((i + 1 ) % n) #define OPTIMAL(v, i, n) ((v[PREV_INDEX(i, n)].y != v[i].y) || \ - (v[NEXT_INDEX(i, n)].y != v[i].y)) + (v[NEXT_INDEX(i, n)].y != v[i].y)) #define FWD_MIN(v, i, n) ((v[PREV_INDEX(i, n)].vertex.y >= v[i].vertex.y) \ - && (v[NEXT_INDEX(i, n)].vertex.y > v[i].vertex.y)) + && (v[NEXT_INDEX(i, n)].vertex.y > v[i].vertex.y)) #define NOT_FMAX(v, i, n) (v[NEXT_INDEX(i, n)].vertex.y > v[i].vertex.y) #define REV_MIN(v, i, n) ((v[PREV_INDEX(i, n)].vertex.y > v[i].vertex.y) \ - && (v[NEXT_INDEX(i, n)].vertex.y >= v[i].vertex.y)) + && (v[NEXT_INDEX(i, n)].vertex.y >= v[i].vertex.y)) #define NOT_RMAX(v, i, n) (v[PREV_INDEX(i, n)].vertex.y > v[i].vertex.y) #define VERTEX(e,p,s,x,y) {add_vertex(&((e)->outp[(p)]->v[(s)]), x, y); \ - (e)->outp[(p)]->active++;} + (e)->outp[(p)]->active++;} #define P_EDGE(d,e,p,i,j) {(d)= (e); \ - do {(d)= (d)->prev;} while (!(d)->outp[(p)]); \ - (i)= (d)->bot.x + (d)->dx * ((j)-(d)->bot.y);} + do {(d)= (d)->prev;} while (!(d)->outp[(p)]); \ + (i)= (d)->bot.x + (d)->dx * ((j)-(d)->bot.y);} #define N_EDGE(d,e,p,i,j) {(d)= (e); \ - do {(d)= (d)->next;} while (!(d)->outp[(p)]); \ - (i)= (d)->bot.x + (d)->dx * ((j)-(d)->bot.y);} + do {(d)= (d)->next;} while (!(d)->outp[(p)]); \ + (i)= (d)->bot.x + (d)->dx * ((j)-(d)->bot.y);} #define MALLOC(p, b, s, t) {if ((b) > 0) { \ - p= (t*)malloc(b); if (!(p)) { \ - fprintf(stderr, "gpc malloc failure: %s\n", s); \ - exit(0);}} else p= NULL;} + p= (t*)malloc(b); if (!(p)) { \ + fprintf(stderr, "gpc malloc failure: %s\n", s); \ + exit(0);}} else p= NULL;} #define FREE(p) {if (p) {free(p); (p)= NULL;}} /* =========================================================================== - Private Data Types + Private Data Types =========================================================================== */ @@ -226,7 +228,7 @@ typedef struct bbox_shape /* Contour axis-aligned bounding box */ /* =========================================================================== - Global Data + Global Data =========================================================================== */ @@ -234,7 +236,7 @@ typedef struct bbox_shape /* Contour axis-aligned bounding box */ const h_state next_h_state[3][6]= { /* ABOVE BELOW CROSS */ - /* L R L R L R */ + /* L R L R L R */ /* NH */ {BH, TH, TH, BH, NH, NH}, /* BH */ {NH, NH, NH, NH, TH, TH}, /* TH */ {NH, NH, NH, NH, BH, BH} @@ -243,7 +245,7 @@ const h_state next_h_state[3][6]= /* =========================================================================== - Private Functions + Private Functions =========================================================================== */ @@ -253,9 +255,9 @@ static void reset_it(it_node **it) while (*it) { - itn= (*it)->next; - FREE(*it); - *it= itn; + itn= (*it)->next; + FREE(*it); + *it= itn; } } @@ -266,9 +268,9 @@ static void reset_lmt(lmt_node **lmt) while (*lmt) { - lmtn= (*lmt)->next; - FREE(*lmt); - *lmt= lmtn; + lmtn= (*lmt)->next; + FREE(*lmt); + *lmt= lmtn; } } @@ -279,43 +281,43 @@ static void insert_bound(edge_node **b, edge_node *e) if (!*b) { - /* Link node e to the tail of the list */ - *b= e; + /* Link node e to the tail of the list */ + *b= e; } else { - /* Do primary sort on the x field */ - if (e[0].bot.x < (*b)[0].bot.x) - { - /* Insert a new node mid-list */ - existing_bound= *b; - *b= e; - (*b)->next_bound= existing_bound; - } - else - { - if (e[0].bot.x == (*b)[0].bot.x) - { - /* Do secondary sort on the dx field */ - if (e[0].dx < (*b)[0].dx) - { - /* Insert a new node mid-list */ - existing_bound= *b; - *b= e; - (*b)->next_bound= existing_bound; - } - else - { - /* Head further down the list */ - insert_bound(&((*b)->next_bound), e); - } - } - else - { - /* Head further down the list */ - insert_bound(&((*b)->next_bound), e); - } - } + /* Do primary sort on the x field */ + if (e[0].bot.x < (*b)[0].bot.x) + { + /* Insert a new node mid-list */ + existing_bound= *b; + *b= e; + (*b)->next_bound= existing_bound; + } + else + { + if (e[0].bot.x == (*b)[0].bot.x) + { + /* Do secondary sort on the dx field */ + if (e[0].dx < (*b)[0].dx) + { + /* Insert a new node mid-list */ + existing_bound= *b; + *b= e; + (*b)->next_bound= existing_bound; + } + else + { + /* Head further down the list */ + insert_bound(&((*b)->next_bound), e); + } + } + else + { + /* Head further down the list */ + insert_bound(&((*b)->next_bound), e); + } + } } } @@ -326,31 +328,31 @@ static edge_node **bound_list(lmt_node **lmt, double y) if (!*lmt) { - /* Add node onto the tail end of the LMT */ - MALLOC(*lmt, sizeof(lmt_node), "LMT insertion", lmt_node); - (*lmt)->y= y; - (*lmt)->first_bound= NULL; - (*lmt)->next= NULL; - return &((*lmt)->first_bound); + /* Add node onto the tail end of the LMT */ + MALLOC(*lmt, sizeof(lmt_node), "LMT insertion", lmt_node); + (*lmt)->y= y; + (*lmt)->first_bound= NULL; + (*lmt)->next= NULL; + return &((*lmt)->first_bound); } else - if (y < (*lmt)->y) - { - /* Insert a new LMT node before the current node */ - existing_node= *lmt; - MALLOC(*lmt, sizeof(lmt_node), "LMT insertion", lmt_node); - (*lmt)->y= y; - (*lmt)->first_bound= NULL; - (*lmt)->next= existing_node; - return &((*lmt)->first_bound); - } - else - if (y > (*lmt)->y) - /* Head further up the LMT */ - return bound_list(&((*lmt)->next), y); - else - /* Use this existing LMT node */ - return &((*lmt)->first_bound); + if (y < (*lmt)->y) + { + /* Insert a new LMT node before the current node */ + existing_node= *lmt; + MALLOC(*lmt, sizeof(lmt_node), "LMT insertion", lmt_node); + (*lmt)->y= y; + (*lmt)->first_bound= NULL; + (*lmt)->next= existing_node; + return &((*lmt)->first_bound); + } + else + if (y > (*lmt)->y) + /* Head further up the LMT */ + return bound_list(&((*lmt)->next), y); + else + /* Use this existing LMT node */ + return &((*lmt)->first_bound); } @@ -358,28 +360,28 @@ static void add_to_sbtree(int *entries, sb_tree **sbtree, double y) { if (!*sbtree) { - /* Add a new tree node here */ - MALLOC(*sbtree, sizeof(sb_tree), "scanbeam tree insertion", sb_tree); - (*sbtree)->y= y; - (*sbtree)->less= NULL; - (*sbtree)->more= NULL; - (*entries)++; + /* Add a new tree node here */ + MALLOC(*sbtree, sizeof(sb_tree), "scanbeam tree insertion", sb_tree); + (*sbtree)->y= y; + (*sbtree)->less= NULL; + (*sbtree)->more= NULL; + (*entries)++; } else { - if ((*sbtree)->y > y) - { - /* Head into the 'less' sub-tree */ - add_to_sbtree(entries, &((*sbtree)->less), y); - } - else - { - if ((*sbtree)->y < y) - { - /* Head into the 'more' sub-tree */ - add_to_sbtree(entries, &((*sbtree)->more), y); - } - } + if ((*sbtree)->y > y) + { + /* Head into the 'less' sub-tree */ + add_to_sbtree(entries, &((*sbtree)->less), y); + } + else + { + if ((*sbtree)->y < y) + { + /* Head into the 'more' sub-tree */ + add_to_sbtree(entries, &((*sbtree)->more), y); + } + } } } @@ -387,11 +389,11 @@ static void add_to_sbtree(int *entries, sb_tree **sbtree, double y) static void build_sbt(int *entries, double *sbt, sb_tree *sbtree) { if (sbtree->less) - build_sbt(entries, sbt, sbtree->less); + build_sbt(entries, sbt, sbtree->less); sbt[*entries]= sbtree->y; (*entries)++; if (sbtree->more) - build_sbt(entries, sbt, sbtree->more); + build_sbt(entries, sbt, sbtree->more); } @@ -399,9 +401,9 @@ static void free_sbtree(sb_tree **sbtree) { if (*sbtree) { - free_sbtree(&((*sbtree)->less)); - free_sbtree(&((*sbtree)->more)); - FREE(*sbtree); + free_sbtree(&((*sbtree)->less)); + free_sbtree(&((*sbtree)->more)); + FREE(*sbtree); } } @@ -413,154 +415,154 @@ static int count_optimal_vertices(gpc_vertex_list c) /* Ignore non-contributing contours */ if (c.num_vertices > 0) { - for (i= 0; i < c.num_vertices; i++) - /* Ignore superfluous vertices embedded in horizontal edges */ - if (OPTIMAL(c.vertex, i, c.num_vertices)) - result++; + for (i= 0; i < c.num_vertices; i++) + /* Ignore superfluous vertices embedded in horizontal edges */ + if (OPTIMAL(c.vertex, i, c.num_vertices)) + result++; } return result; } static edge_node *build_lmt(lmt_node **lmt, sb_tree **sbtree, - int *sbt_entries, gpc_polygon *p, int type, - gpc_op op) + int *sbt_entries, gpc_polygon *p, int type, + gpc_op op) { int c, i, min, max, num_edges, v, num_vertices; int total_vertices= 0, e_index=0; edge_node *e, *edge_table; for (c= 0; c < p->num_contours; c++) - total_vertices+= count_optimal_vertices(p->contour[c]); + total_vertices+= count_optimal_vertices(p->contour[c]); /* Create the entire input polygon edge table in one go */ MALLOC(edge_table, total_vertices * sizeof(edge_node), - "edge table creation", edge_node); + "edge table creation", edge_node); for (c= 0; c < p->num_contours; c++) { - if (p->contour[c].num_vertices < 0) - { - /* Ignore the non-contributing contour and repair the vertex count */ - p->contour[c].num_vertices= -p->contour[c].num_vertices; - } - else - { - /* Perform contour optimisation */ - num_vertices= 0; - for (i= 0; i < p->contour[c].num_vertices; i++) - if (OPTIMAL(p->contour[c].vertex, i, p->contour[c].num_vertices)) - { - edge_table[num_vertices].vertex.x= p->contour[c].vertex[i].x; - edge_table[num_vertices].vertex.y= p->contour[c].vertex[i].y; + if (p->contour[c].num_vertices < 0) + { + /* Ignore the non-contributing contour and repair the vertex count */ + p->contour[c].num_vertices= -p->contour[c].num_vertices; + } + else + { + /* Perform contour optimisation */ + num_vertices= 0; + for (i= 0; i < p->contour[c].num_vertices; i++) + if (OPTIMAL(p->contour[c].vertex, i, p->contour[c].num_vertices)) + { + edge_table[num_vertices].vertex.x= p->contour[c].vertex[i].x; + edge_table[num_vertices].vertex.y= p->contour[c].vertex[i].y; - /* Record vertex in the scanbeam table */ - add_to_sbtree(sbt_entries, sbtree, - edge_table[num_vertices].vertex.y); + /* Record vertex in the scanbeam table */ + add_to_sbtree(sbt_entries, sbtree, + edge_table[num_vertices].vertex.y); - num_vertices++; - } + num_vertices++; + } - /* Do the contour forward pass */ - for (min= 0; min < num_vertices; min++) - { - /* If a forward local minimum... */ - if (FWD_MIN(edge_table, min, num_vertices)) - { - /* Search for the next local maximum... */ - num_edges= 1; - max= NEXT_INDEX(min, num_vertices); - while (NOT_FMAX(edge_table, max, num_vertices)) - { - num_edges++; - max= NEXT_INDEX(max, num_vertices); - } + /* Do the contour forward pass */ + for (min= 0; min < num_vertices; min++) + { + /* If a forward local minimum... */ + if (FWD_MIN(edge_table, min, num_vertices)) + { + /* Search for the next local maximum... */ + num_edges= 1; + max= NEXT_INDEX(min, num_vertices); + while (NOT_FMAX(edge_table, max, num_vertices)) + { + num_edges++; + max= NEXT_INDEX(max, num_vertices); + } - /* Build the next edge list */ - e= &edge_table[e_index]; - e_index+= num_edges; - v= min; - e[0].bstate[BELOW]= UNBUNDLED; - e[0].bundle[BELOW][CLIP]= FALSE; - e[0].bundle[BELOW][SUBJ]= FALSE; - for (i= 0; i < num_edges; i++) - { - e[i].xb= edge_table[v].vertex.x; - e[i].bot.x= edge_table[v].vertex.x; - e[i].bot.y= edge_table[v].vertex.y; + /* Build the next edge list */ + e= &edge_table[e_index]; + e_index+= num_edges; + v= min; + e[0].bstate[BELOW]= UNBUNDLED; + e[0].bundle[BELOW][CLIP]= FALSE; + e[0].bundle[BELOW][SUBJ]= FALSE; + for (i= 0; i < num_edges; i++) + { + e[i].xb= edge_table[v].vertex.x; + e[i].bot.x= edge_table[v].vertex.x; + e[i].bot.y= edge_table[v].vertex.y; - v= NEXT_INDEX(v, num_vertices); + v= NEXT_INDEX(v, num_vertices); - e[i].top.x= edge_table[v].vertex.x; - e[i].top.y= edge_table[v].vertex.y; - e[i].dx= (edge_table[v].vertex.x - e[i].bot.x) / - (e[i].top.y - e[i].bot.y); - e[i].type= type; - e[i].outp[ABOVE]= NULL; - e[i].outp[BELOW]= NULL; - e[i].next= NULL; - e[i].prev= NULL; - e[i].succ= ((num_edges > 1) && (i < (num_edges - 1))) ? - &(e[i + 1]) : NULL; - e[i].pred= ((num_edges > 1) && (i > 0)) ? &(e[i - 1]) : NULL; - e[i].next_bound= NULL; + e[i].top.x= edge_table[v].vertex.x; + e[i].top.y= edge_table[v].vertex.y; + e[i].dx= (edge_table[v].vertex.x - e[i].bot.x) / + (e[i].top.y - e[i].bot.y); + e[i].type= type; + e[i].outp[ABOVE]= NULL; + e[i].outp[BELOW]= NULL; + e[i].next= NULL; + e[i].prev= NULL; + e[i].succ= ((num_edges > 1) && (i < (num_edges - 1))) ? + &(e[i + 1]) : NULL; + e[i].pred= ((num_edges > 1) && (i > 0)) ? &(e[i - 1]) : NULL; + e[i].next_bound= NULL; e[i].bside[CLIP]= (op == GPC_DIFF) ? GPC_RIGHT : GPC_LEFT; e[i].bside[SUBJ]= GPC_LEFT; - } - insert_bound(bound_list(lmt, edge_table[min].vertex.y), e); - } - } + } + insert_bound(bound_list(lmt, edge_table[min].vertex.y), e); + } + } - /* Do the contour reverse pass */ - for (min= 0; min < num_vertices; min++) - { - /* If a reverse local minimum... */ - if (REV_MIN(edge_table, min, num_vertices)) - { - /* Search for the previous local maximum... */ - num_edges= 1; - max= PREV_INDEX(min, num_vertices); - while (NOT_RMAX(edge_table, max, num_vertices)) - { - num_edges++; - max= PREV_INDEX(max, num_vertices); - } + /* Do the contour reverse pass */ + for (min= 0; min < num_vertices; min++) + { + /* If a reverse local minimum... */ + if (REV_MIN(edge_table, min, num_vertices)) + { + /* Search for the previous local maximum... */ + num_edges= 1; + max= PREV_INDEX(min, num_vertices); + while (NOT_RMAX(edge_table, max, num_vertices)) + { + num_edges++; + max= PREV_INDEX(max, num_vertices); + } - /* Build the previous edge list */ - e= &edge_table[e_index]; - e_index+= num_edges; - v= min; - e[0].bstate[BELOW]= UNBUNDLED; - e[0].bundle[BELOW][CLIP]= FALSE; - e[0].bundle[BELOW][SUBJ]= FALSE; - for (i= 0; i < num_edges; i++) - { - e[i].xb= edge_table[v].vertex.x; - e[i].bot.x= edge_table[v].vertex.x; - e[i].bot.y= edge_table[v].vertex.y; + /* Build the previous edge list */ + e= &edge_table[e_index]; + e_index+= num_edges; + v= min; + e[0].bstate[BELOW]= UNBUNDLED; + e[0].bundle[BELOW][CLIP]= FALSE; + e[0].bundle[BELOW][SUBJ]= FALSE; + for (i= 0; i < num_edges; i++) + { + e[i].xb= edge_table[v].vertex.x; + e[i].bot.x= edge_table[v].vertex.x; + e[i].bot.y= edge_table[v].vertex.y; - v= PREV_INDEX(v, num_vertices); + v= PREV_INDEX(v, num_vertices); - e[i].top.x= edge_table[v].vertex.x; - e[i].top.y= edge_table[v].vertex.y; - e[i].dx= (edge_table[v].vertex.x - e[i].bot.x) / - (e[i].top.y - e[i].bot.y); - e[i].type= type; - e[i].outp[ABOVE]= NULL; - e[i].outp[BELOW]= NULL; - e[i].next= NULL; - e[i].prev= NULL; - e[i].succ= ((num_edges > 1) && (i < (num_edges - 1))) ? - &(e[i + 1]) : NULL; - e[i].pred= ((num_edges > 1) && (i > 0)) ? &(e[i - 1]) : NULL; - e[i].next_bound= NULL; + e[i].top.x= edge_table[v].vertex.x; + e[i].top.y= edge_table[v].vertex.y; + e[i].dx= (edge_table[v].vertex.x - e[i].bot.x) / + (e[i].top.y - e[i].bot.y); + e[i].type= type; + e[i].outp[ABOVE]= NULL; + e[i].outp[BELOW]= NULL; + e[i].next= NULL; + e[i].prev= NULL; + e[i].succ= ((num_edges > 1) && (i < (num_edges - 1))) ? + &(e[i + 1]) : NULL; + e[i].pred= ((num_edges > 1) && (i > 0)) ? &(e[i - 1]) : NULL; + e[i].next_bound= NULL; e[i].bside[CLIP]= (op == GPC_DIFF) ? GPC_RIGHT : GPC_LEFT; e[i].bside[SUBJ]= GPC_LEFT; - } - insert_bound(bound_list(lmt, edge_table[min].vertex.y), e); - } - } - } + } + insert_bound(bound_list(lmt, edge_table[min].vertex.y), e); + } + } + } } return edge_table; } @@ -570,132 +572,132 @@ static void add_edge_to_aet(edge_node **aet, edge_node *edge, edge_node *prev) { if (!*aet) { - /* Append edge onto the tail end of the AET */ - *aet= edge; - edge->prev= prev; - edge->next= NULL; + /* Append edge onto the tail end of the AET */ + *aet= edge; + edge->prev= prev; + edge->next= NULL; } else { - /* Do primary sort on the xb field */ - if (edge->xb < (*aet)->xb) - { - /* Insert edge here (before the AET edge) */ - edge->prev= prev; - edge->next= *aet; - (*aet)->prev= edge; - *aet= edge; - } - else - { - if (edge->xb == (*aet)->xb) - { - /* Do secondary sort on the dx field */ - if (edge->dx < (*aet)->dx) - { - /* Insert edge here (before the AET edge) */ - edge->prev= prev; - edge->next= *aet; - (*aet)->prev= edge; - *aet= edge; - } - else - { - /* Head further into the AET */ - add_edge_to_aet(&((*aet)->next), edge, *aet); - } - } - else - { - /* Head further into the AET */ - add_edge_to_aet(&((*aet)->next), edge, *aet); - } - } + /* Do primary sort on the xb field */ + if (edge->xb < (*aet)->xb) + { + /* Insert edge here (before the AET edge) */ + edge->prev= prev; + edge->next= *aet; + (*aet)->prev= edge; + *aet= edge; + } + else + { + if (edge->xb == (*aet)->xb) + { + /* Do secondary sort on the dx field */ + if (edge->dx < (*aet)->dx) + { + /* Insert edge here (before the AET edge) */ + edge->prev= prev; + edge->next= *aet; + (*aet)->prev= edge; + *aet= edge; + } + else + { + /* Head further into the AET */ + add_edge_to_aet(&((*aet)->next), edge, *aet); + } + } + else + { + /* Head further into the AET */ + add_edge_to_aet(&((*aet)->next), edge, *aet); + } + } } } static void add_intersection(it_node **it, edge_node *edge0, edge_node *edge1, - double x, double y) + double x, double y) { it_node *existing_node; if (!*it) { - /* Append a new node to the tail of the list */ - MALLOC(*it, sizeof(it_node), "IT insertion", it_node); - (*it)->ie[0]= edge0; - (*it)->ie[1]= edge1; - (*it)->point.x= x; - (*it)->point.y= y; - (*it)->next= NULL; + /* Append a new node to the tail of the list */ + MALLOC(*it, sizeof(it_node), "IT insertion", it_node); + (*it)->ie[0]= edge0; + (*it)->ie[1]= edge1; + (*it)->point.x= x; + (*it)->point.y= y; + (*it)->next= NULL; } else { - if ((*it)->point.y > y) - { - /* Insert a new node mid-list */ - existing_node= *it; - MALLOC(*it, sizeof(it_node), "IT insertion", it_node); - (*it)->ie[0]= edge0; - (*it)->ie[1]= edge1; - (*it)->point.x= x; - (*it)->point.y= y; - (*it)->next= existing_node; - } - else - /* Head further down the list */ - add_intersection(&((*it)->next), edge0, edge1, x, y); + if ((*it)->point.y > y) + { + /* Insert a new node mid-list */ + existing_node= *it; + MALLOC(*it, sizeof(it_node), "IT insertion", it_node); + (*it)->ie[0]= edge0; + (*it)->ie[1]= edge1; + (*it)->point.x= x; + (*it)->point.y= y; + (*it)->next= existing_node; + } + else + /* Head further down the list */ + add_intersection(&((*it)->next), edge0, edge1, x, y); } } static void add_st_edge(st_node **st, it_node **it, edge_node *edge, - double dy) + double dy) { st_node *existing_node; double den, r, x, y; if (!*st) { - /* Append edge onto the tail end of the ST */ - MALLOC(*st, sizeof(st_node), "ST insertion", st_node); - (*st)->edge= edge; - (*st)->xb= edge->xb; - (*st)->xt= edge->xt; - (*st)->dx= edge->dx; - (*st)->prev= NULL; + /* Append edge onto the tail end of the ST */ + MALLOC(*st, sizeof(st_node), "ST insertion", st_node); + (*st)->edge= edge; + (*st)->xb= edge->xb; + (*st)->xt= edge->xt; + (*st)->dx= edge->dx; + (*st)->prev= NULL; } else { - den= ((*st)->xt - (*st)->xb) - (edge->xt - edge->xb); + den= ((*st)->xt - (*st)->xb) - (edge->xt - edge->xb); - /* If new edge and ST edge don't cross */ - if ((edge->xt >= (*st)->xt) || (edge->dx == (*st)->dx) || - (fabs(den) <= DBL_EPSILON)) - { - /* No intersection - insert edge here (before the ST edge) */ - existing_node= *st; - MALLOC(*st, sizeof(st_node), "ST insertion", st_node); - (*st)->edge= edge; - (*st)->xb= edge->xb; - (*st)->xt= edge->xt; - (*st)->dx= edge->dx; - (*st)->prev= existing_node; - } - else - { - /* Compute intersection between new edge and ST edge */ - r= (edge->xb - (*st)->xb) / den; - x= (*st)->xb + r * ((*st)->xt - (*st)->xb); - y= r * dy; + /* If new edge and ST edge don't cross */ + if ((edge->xt >= (*st)->xt) || (edge->dx == (*st)->dx) || + (fabs(den) <= DBL_EPSILON)) + { + /* No intersection - insert edge here (before the ST edge) */ + existing_node= *st; + MALLOC(*st, sizeof(st_node), "ST insertion", st_node); + (*st)->edge= edge; + (*st)->xb= edge->xb; + (*st)->xt= edge->xt; + (*st)->dx= edge->dx; + (*st)->prev= existing_node; + } + else + { + /* Compute intersection between new edge and ST edge */ + r= (edge->xb - (*st)->xb) / den; + x= (*st)->xb + r * ((*st)->xt - (*st)->xb); + y= r * dy; - /* Insert the edge pointers and the intersection point in the IT */ - add_intersection(it, (*st)->edge, edge, x, y); + /* Insert the edge pointers and the intersection point in the IT */ + add_intersection(it, (*st)->edge, edge, x, y); - /* Head further into the ST */ - add_st_edge(&((*st)->prev), it, edge, dy); - } + /* Head further into the ST */ + add_st_edge(&((*st)->prev), it, edge, dy); + } } } @@ -712,17 +714,17 @@ static void build_intersection_table(it_node **it, edge_node *aet, double dy) /* Process each AET edge */ for (edge= aet; edge; edge= edge->next) { - if ((edge->bstate[ABOVE] == BUNDLE_HEAD) || - edge->bundle[ABOVE][CLIP] || edge->bundle[ABOVE][SUBJ]) - add_st_edge(&st, it, edge, dy); + if ((edge->bstate[ABOVE] == BUNDLE_HEAD) || + edge->bundle[ABOVE][CLIP] || edge->bundle[ABOVE][SUBJ]) + add_st_edge(&st, it, edge, dy); } /* Free the sorted edge table */ while (st) { - stp= st->prev; - FREE(st); - st= stp; + stp= st->prev; + FREE(st); + st= stp; } } @@ -732,30 +734,30 @@ static int count_contours(polygon_node *polygon) vertex_node *v, *nextv; for (nc= 0; polygon; polygon= polygon->next) - if (polygon->active) - { - /* Count the vertices in the current contour */ - nv= 0; + if (polygon->active) + { + /* Count the vertices in the current contour */ + nv= 0; for (v= polygon->proxy->v[GPC_LEFT]; v; v= v->next) - nv++; + nv++; - /* Record valid vertex counts in the active field */ - if (nv > 2) - { - polygon->active= nv; - nc++; - } - else - { - /* Invalid contour: just free the heap */ + /* Record valid vertex counts in the active field */ + if (nv > 2) + { + polygon->active= nv; + nc++; + } + else + { + /* Invalid contour: just free the heap */ for (v= polygon->proxy->v[GPC_LEFT]; v; v= nextv) - { - nextv= v->next; - FREE(v); - } - polygon->active= 0; - } - } + { + nextv= v->next; + FREE(v); + } + polygon->active= 0; + } + } return nc; } @@ -786,20 +788,20 @@ static void merge_left(polygon_node *p, polygon_node *q, polygon_node *list) if (p->proxy != q->proxy) { - /* Assign p's vertex list to the left end of q's list */ + /* Assign p's vertex list to the left end of q's list */ p->proxy->v[GPC_RIGHT]->next= q->proxy->v[GPC_LEFT]; q->proxy->v[GPC_LEFT]= p->proxy->v[GPC_LEFT]; - /* Redirect any p->proxy references to q->proxy */ - - for (target= p->proxy; list; list= list->next) - { - if (list->proxy == target) - { - list->active= FALSE; - list->proxy= q->proxy; - } - } + /* Redirect any p->proxy references to q->proxy */ + + for (target= p->proxy; list; list= list->next) + { + if (list->proxy == target) + { + list->active= FALSE; + list->proxy= q->proxy; + } + } } } @@ -831,25 +833,25 @@ static void merge_right(polygon_node *p, polygon_node *q, polygon_node *list) if (p->proxy != q->proxy) { - /* Assign p's vertex list to the right end of q's list */ + /* Assign p's vertex list to the right end of q's list */ q->proxy->v[GPC_RIGHT]->next= p->proxy->v[GPC_LEFT]; q->proxy->v[GPC_RIGHT]= p->proxy->v[GPC_RIGHT]; - /* Redirect any p->proxy references to q->proxy */ - for (target= p->proxy; list; list= list->next) - { - if (list->proxy == target) - { - list->active= FALSE; - list->proxy= q->proxy; - } - } + /* Redirect any p->proxy references to q->proxy */ + for (target= p->proxy; list; list= list->next) + { + if (list->proxy == target) + { + list->active= FALSE; + list->proxy= q->proxy; + } + } } } static void add_local_min(polygon_node **p, edge_node *edge, - double x, double y) + double x, double y) { polygon_node *existing_min; vertex_node *nv; @@ -883,8 +885,8 @@ static int count_tristrips(polygon_node *tn) int total; for (total= 0; tn; tn= tn->next) - if (tn->active > 2) - total++; + if (tn->active > 2) + total++; return total; } @@ -893,33 +895,33 @@ static void add_vertex(vertex_node **t, double x, double y) { if (!(*t)) { - MALLOC(*t, sizeof(vertex_node), "tristrip vertex creation", vertex_node); - (*t)->x= x; - (*t)->y= y; - (*t)->next= NULL; + MALLOC(*t, sizeof(vertex_node), "tristrip vertex creation", vertex_node); + (*t)->x= x; + (*t)->y= y; + (*t)->next= NULL; } else - /* Head further down the list */ - add_vertex(&((*t)->next), x, y); + /* Head further down the list */ + add_vertex(&((*t)->next), x, y); } static void new_tristrip(polygon_node **tn, edge_node *edge, - double x, double y) + double x, double y) { if (!(*tn)) { - MALLOC(*tn, sizeof(polygon_node), "tristrip node creation", polygon_node); - (*tn)->next= NULL; + MALLOC(*tn, sizeof(polygon_node), "tristrip node creation", polygon_node); + (*tn)->next= NULL; (*tn)->v[GPC_LEFT]= NULL; (*tn)->v[GPC_RIGHT]= NULL; - (*tn)->active= 1; + (*tn)->active= 1; add_vertex(&((*tn)->v[GPC_LEFT]), x, y); - edge->outp[ABOVE]= *tn; + edge->outp[ABOVE]= *tn; } else - /* Head further down the list */ - new_tristrip(&((*tn)->next), edge, x, y); + /* Head further down the list */ + new_tristrip(&((*tn)->next), edge, x, y); } @@ -933,26 +935,26 @@ static bbox *create_contour_bboxes(gpc_polygon *p) /* Construct contour bounding boxes */ for (c= 0; c < p->num_contours; c++) { - /* Initialise bounding box extent */ - box[c].xmin= DBL_MAX; - box[c].ymin= DBL_MAX; - box[c].xmax= -DBL_MAX; - box[c].ymax= -DBL_MAX; + /* Initialise bounding box extent */ + box[c].xmin= DBL_MAX; + box[c].ymin= DBL_MAX; + box[c].xmax= -DBL_MAX; + box[c].ymax= -DBL_MAX; - for (v= 0; v < p->contour[c].num_vertices; v++) - { - /* Adjust bounding box */ - if (p->contour[c].vertex[v].x < box[c].xmin) - box[c].xmin= p->contour[c].vertex[v].x; - if (p->contour[c].vertex[v].y < box[c].ymin) - box[c].ymin= p->contour[c].vertex[v].y; - if (p->contour[c].vertex[v].x > box[c].xmax) - box[c].xmax= p->contour[c].vertex[v].x; - if (p->contour[c].vertex[v].y > box[c].ymax) - box[c].ymax= p->contour[c].vertex[v].y; - } + for (v= 0; v < p->contour[c].num_vertices; v++) + { + /* Adjust bounding box */ + if (p->contour[c].vertex[v].x < box[c].xmin) + box[c].xmin= p->contour[c].vertex[v].x; + if (p->contour[c].vertex[v].y < box[c].ymin) + box[c].ymin= p->contour[c].vertex[v].y; + if (p->contour[c].vertex[v].x > box[c].xmax) + box[c].xmax= p->contour[c].vertex[v].x; + if (p->contour[c].vertex[v].y > box[c].ymax) + box[c].ymax= p->contour[c].vertex[v].y; + } } - return box; + return box; } @@ -965,42 +967,42 @@ static void minimax_test(gpc_polygon *subj, gpc_polygon *clip, gpc_op op) c_bbox= create_contour_bboxes(clip); MALLOC(o_table, subj->num_contours * clip->num_contours * sizeof(int), - "overlap table creation", int); + "overlap table creation", int); /* Check all subject contour bounding boxes against clip boxes */ for (s= 0; s < subj->num_contours; s++) - for (c= 0; c < clip->num_contours; c++) - o_table[c * subj->num_contours + s]= - (!((s_bbox[s].xmax < c_bbox[c].xmin) || - (s_bbox[s].xmin > c_bbox[c].xmax))) && - (!((s_bbox[s].ymax < c_bbox[c].ymin) || - (s_bbox[s].ymin > c_bbox[c].ymax))); + for (c= 0; c < clip->num_contours; c++) + o_table[c * subj->num_contours + s]= + (!((s_bbox[s].xmax < c_bbox[c].xmin) || + (s_bbox[s].xmin > c_bbox[c].xmax))) && + (!((s_bbox[s].ymax < c_bbox[c].ymin) || + (s_bbox[s].ymin > c_bbox[c].ymax))); /* For each clip contour, search for any subject contour overlaps */ for (c= 0; c < clip->num_contours; c++) { - overlap= 0; - for (s= 0; (!overlap) && (s < subj->num_contours); s++) - overlap= o_table[c * subj->num_contours + s]; + overlap= 0; + for (s= 0; (!overlap) && (s < subj->num_contours); s++) + overlap= o_table[c * subj->num_contours + s]; - if (!overlap) - /* Flag non contributing status by negating vertex count */ - clip->contour[c].num_vertices = -clip->contour[c].num_vertices; - } + if (!overlap) + /* Flag non contributing status by negating vertex count */ + clip->contour[c].num_vertices = -clip->contour[c].num_vertices; + } if (op == GPC_INT) - { - /* For each subject contour, search for any clip contour overlaps */ - for (s= 0; s < subj->num_contours; s++) - { - overlap= 0; - for (c= 0; (!overlap) && (c < clip->num_contours); c++) - overlap= o_table[c * subj->num_contours + s]; + { + /* For each subject contour, search for any clip contour overlaps */ + for (s= 0; s < subj->num_contours; s++) + { + overlap= 0; + for (c= 0; (!overlap) && (c < clip->num_contours); c++) + overlap= o_table[c * subj->num_contours + s]; - if (!overlap) - /* Flag non contributing status by negating vertex count */ - subj->contour[s].num_vertices = -subj->contour[s].num_vertices; - } + if (!overlap) + /* Flag non contributing status by negating vertex count */ + subj->contour[s].num_vertices = -subj->contour[s].num_vertices; + } } FREE(s_bbox); @@ -1011,7 +1013,7 @@ static void minimax_test(gpc_polygon *subj, gpc_polygon *clip, gpc_op op) /* =========================================================================== - Public Functions + Public Functions =========================================================================== */ @@ -1020,7 +1022,7 @@ void gpc_free_polygon(gpc_polygon *p) int c; for (c= 0; c < p->num_contours; c++) - FREE(p->contour[c].vertex); + FREE(p->contour[c].vertex); FREE(p->hole); FREE(p->contour); p->num_contours= 0; @@ -1033,23 +1035,23 @@ void gpc_read_polygon(FILE *fp, int read_hole_flags, gpc_polygon *p) fscanf(fp, "%d", &(p->num_contours)); MALLOC(p->hole, p->num_contours * sizeof(int), - "hole flag array creation", int); + "hole flag array creation", int); MALLOC(p->contour, p->num_contours - * sizeof(gpc_vertex_list), "contour creation", gpc_vertex_list); + * sizeof(gpc_vertex_list), "contour creation", gpc_vertex_list); for (c= 0; c < p->num_contours; c++) { - fscanf(fp, "%d", &(p->contour[c].num_vertices)); + fscanf(fp, "%d", &(p->contour[c].num_vertices)); - if (read_hole_flags) - fscanf(fp, "%d", &(p->hole[c])); - else - p->hole[c]= FALSE; /* Assume all contours to be external */ + if (read_hole_flags) + fscanf(fp, "%d", &(p->hole[c])); + else + p->hole[c]= FALSE; /* Assume all contours to be external */ - MALLOC(p->contour[c].vertex, p->contour[c].num_vertices - * sizeof(gpc_vertex), "vertex creation", gpc_vertex); - for (v= 0; v < p->contour[c].num_vertices; v++) - fscanf(fp, "%lf %lf", &(p->contour[c].vertex[v].x), - &(p->contour[c].vertex[v].y)); + MALLOC(p->contour[c].vertex, p->contour[c].num_vertices + * sizeof(gpc_vertex), "vertex creation", gpc_vertex); + for (v= 0; v < p->contour[c].num_vertices; v++) + fscanf(fp, "%lf %lf", &(p->contour[c].vertex[v].x), + &(p->contour[c].vertex[v].y)); } } @@ -1061,15 +1063,15 @@ void gpc_write_polygon(FILE *fp, int write_hole_flags, gpc_polygon *p) fprintf(fp, "%d\n", p->num_contours); for (c= 0; c < p->num_contours; c++) { - fprintf(fp, "%d\n", p->contour[c].num_vertices); + fprintf(fp, "%d\n", p->contour[c].num_vertices); - if (write_hole_flags) - fprintf(fp, "%d\n", p->hole[c]); - - for (v= 0; v < p->contour[c].num_vertices; v++) - fprintf(fp, "% .*lf % .*lf\n", - DBL_DIG, p->contour[c].vertex[v].x, - DBL_DIG, p->contour[c].vertex[v].y); + if (write_hole_flags) + fprintf(fp, "%d\n", p->hole[c]); + + for (v= 0; v < p->contour[c].num_vertices; v++) + fprintf(fp, "% .*lf % .*lf\n", + DBL_DIG, p->contour[c].vertex[v].x, + DBL_DIG, p->contour[c].vertex[v].y); } } @@ -1081,17 +1083,17 @@ void gpc_add_contour(gpc_polygon *p, gpc_vertex_list *new_contour, int hole) /* Create an extended hole array */ MALLOC(extended_hole, (p->num_contours + 1) - * sizeof(int), "contour hole addition", int); + * sizeof(int), "contour hole addition", int); /* Create an extended contour array */ MALLOC(extended_contour, (p->num_contours + 1) - * sizeof(gpc_vertex_list), "contour addition", gpc_vertex_list); + * sizeof(gpc_vertex_list), "contour addition", gpc_vertex_list); /* Copy the old contour and hole data into the extended arrays */ for (c= 0; c < p->num_contours; c++) { - extended_hole[c]= p->hole[c]; - extended_contour[c]= p->contour[c]; + extended_hole[c]= p->hole[c]; + extended_contour[c]= p->contour[c]; } /* Copy the new contour and hole onto the end of the extended arrays */ @@ -1099,9 +1101,9 @@ void gpc_add_contour(gpc_polygon *p, gpc_vertex_list *new_contour, int hole) extended_hole[c]= hole; extended_contour[c].num_vertices= new_contour->num_vertices; MALLOC(extended_contour[c].vertex, new_contour->num_vertices - * sizeof(gpc_vertex), "contour addition", gpc_vertex); + * sizeof(gpc_vertex), "contour addition", gpc_vertex); for (v= 0; v < new_contour->num_vertices; v++) - extended_contour[c].vertex[v]= new_contour->vertex[v]; + extended_contour[c].vertex[v]= new_contour->vertex[v]; /* Dispose of the old contour */ FREE(p->contour); @@ -1115,7 +1117,7 @@ void gpc_add_contour(gpc_polygon *p, gpc_vertex_list *new_contour, int hole) void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, - gpc_polygon *result) + gpc_polygon *result) { sb_tree *sbtree= NULL; it_node *it= NULL, *intersect; @@ -1135,33 +1137,33 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, || ((subj->num_contours == 0) && ((op == GPC_INT) || (op == GPC_DIFF))) || ((clip->num_contours == 0) && (op == GPC_INT))) { - result->num_contours= 0; - result->hole= NULL; - result->contour= NULL; - return; + result->num_contours= 0; + result->hole= NULL; + result->contour= NULL; + return; } /* Identify potentialy contributing contours */ if (((op == GPC_INT) || (op == GPC_DIFF)) && (subj->num_contours > 0) && (clip->num_contours > 0)) - minimax_test(subj, clip, op); + minimax_test(subj, clip, op); /* Build LMT */ if (subj->num_contours > 0) - s_heap= build_lmt(&lmt, &sbtree, &sbt_entries, subj, SUBJ, op); + s_heap= build_lmt(&lmt, &sbtree, &sbt_entries, subj, SUBJ, op); if (clip->num_contours > 0) - c_heap= build_lmt(&lmt, &sbtree, &sbt_entries, clip, CLIP, op); + c_heap= build_lmt(&lmt, &sbtree, &sbt_entries, clip, CLIP, op); /* Return a NULL result if no contours contribute */ if (lmt == NULL) { - result->num_contours= 0; - result->hole= NULL; - result->contour= NULL; - reset_lmt(&lmt); - FREE(s_heap); - FREE(c_heap); - return; + result->num_contours= 0; + result->hole= NULL; + result->contour= NULL; + reset_lmt(&lmt); + FREE(s_heap); + FREE(c_heap); + return; } /* Build scanbeam table from scanbeam tree */ @@ -1172,9 +1174,9 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, /* Allow pointer re-use without causing memory leak */ if (subj == result) - gpc_free_polygon(subj); + gpc_free_polygon(subj); if (clip == result) - gpc_free_polygon(clip); + gpc_free_polygon(clip); /* Invert clip polygon for difference operation */ if (op == GPC_DIFF) @@ -1185,516 +1187,516 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, /* Process each scanbeam */ while (scanbeam < sbt_entries) { - /* Set yb and yt to the bottom and top of the scanbeam */ - yb= sbt[scanbeam++]; - if (scanbeam < sbt_entries) - { - yt= sbt[scanbeam]; - dy= yt - yb; - } - - /* === SCANBEAM BOUNDARY PROCESSING ================================ */ - - /* If LMT node corresponding to yb exists */ - if (local_min) - { - if (local_min->y == yb) - { - /* Add edges starting at this local minimum to the AET */ - for (edge= local_min->first_bound; edge; edge= edge->next_bound) - add_edge_to_aet(&aet, edge, NULL); - - local_min= local_min->next; - } - } - - /* Set dummy previous x value */ - px= -DBL_MAX; - - /* Create bundles within AET */ - e0= aet; - e1= aet; - - /* Set up bundle fields of first edge */ - aet->bundle[ABOVE][ aet->type]= (aet->top.y != yb); - aet->bundle[ABOVE][!aet->type]= FALSE; - aet->bstate[ABOVE]= UNBUNDLED; - - for (next_edge= aet->next; next_edge; next_edge= next_edge->next) - { - /* Set up bundle fields of next edge */ - next_edge->bundle[ABOVE][ next_edge->type]= (next_edge->top.y != yb); - next_edge->bundle[ABOVE][!next_edge->type]= FALSE; - next_edge->bstate[ABOVE]= UNBUNDLED; - - /* Bundle edges above the scanbeam boundary if they coincide */ - if (next_edge->bundle[ABOVE][next_edge->type]) - { - if (EQ(e0->xb, next_edge->xb) && EQ(e0->dx, next_edge->dx) - && (e0->top.y != yb)) - { - next_edge->bundle[ABOVE][ next_edge->type]^= - e0->bundle[ABOVE][ next_edge->type]; - next_edge->bundle[ABOVE][!next_edge->type]= - e0->bundle[ABOVE][!next_edge->type]; - next_edge->bstate[ABOVE]= BUNDLE_HEAD; - e0->bundle[ABOVE][CLIP]= FALSE; - e0->bundle[ABOVE][SUBJ]= FALSE; - e0->bstate[ABOVE]= BUNDLE_TAIL; - } - e0= next_edge; - } - } - - horiz[CLIP]= NH; - horiz[SUBJ]= NH; - - /* Process each edge at this scanbeam boundary */ - for (edge= aet; edge; edge= edge->next) - { - exists[CLIP]= edge->bundle[ABOVE][CLIP] + - (edge->bundle[BELOW][CLIP] << 1); - exists[SUBJ]= edge->bundle[ABOVE][SUBJ] + - (edge->bundle[BELOW][SUBJ] << 1); - - if (exists[CLIP] || exists[SUBJ]) - { - /* Set bundle side */ - edge->bside[CLIP]= parity[CLIP]; - edge->bside[SUBJ]= parity[SUBJ]; - - /* Determine contributing status and quadrant occupancies */ - switch (op) - { - case GPC_DIFF: - case GPC_INT: - contributing= (exists[CLIP] && (parity[SUBJ] || horiz[SUBJ])) - || (exists[SUBJ] && (parity[CLIP] || horiz[CLIP])) - || (exists[CLIP] && exists[SUBJ] - && (parity[CLIP] == parity[SUBJ])); - br= (parity[CLIP]) - && (parity[SUBJ]); - bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) - && (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); - tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) - && (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); - tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) - && (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); - break; - case GPC_XOR: - contributing= exists[CLIP] || exists[SUBJ]; - br= (parity[CLIP]) - ^ (parity[SUBJ]); - bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) - ^ (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); - tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) - ^ (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); - tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) - ^ (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); - break; - case GPC_UNION: - contributing= (exists[CLIP] && (!parity[SUBJ] || horiz[SUBJ])) - || (exists[SUBJ] && (!parity[CLIP] || horiz[CLIP])) - || (exists[CLIP] && exists[SUBJ] - && (parity[CLIP] == parity[SUBJ])); - br= (parity[CLIP]) - || (parity[SUBJ]); - bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) - || (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); - tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) - || (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); - tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) - || (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); - break; - } - - /* Update parity */ - parity[CLIP]^= edge->bundle[ABOVE][CLIP]; - parity[SUBJ]^= edge->bundle[ABOVE][SUBJ]; - - /* Update horizontal state */ - if (exists[CLIP]) - horiz[CLIP]= - next_h_state[horiz[CLIP]] - [((exists[CLIP] - 1) << 1) + parity[CLIP]]; - if (exists[SUBJ]) - horiz[SUBJ]= - next_h_state[horiz[SUBJ]] - [((exists[SUBJ] - 1) << 1) + parity[SUBJ]]; - - vclass= tr + (tl << 1) + (br << 2) + (bl << 3); - - if (contributing) - { - xb= edge->xb; - - switch (vclass) - { - case EMN: - case IMN: - add_local_min(&out_poly, edge, xb, yb); - px= xb; - cf= edge->outp[ABOVE]; - break; - case ERI: - if (xb != px) - { - add_right(cf, xb, yb); - px= xb; - } - edge->outp[ABOVE]= cf; - cf= NULL; - break; - case ELI: - add_left(edge->outp[BELOW], xb, yb); - px= xb; - cf= edge->outp[BELOW]; - break; - case EMX: - if (xb != px) - { - add_left(cf, xb, yb); - px= xb; - } - merge_right(cf, edge->outp[BELOW], out_poly); - cf= NULL; - break; - case ILI: - if (xb != px) - { - add_left(cf, xb, yb); - px= xb; - } - edge->outp[ABOVE]= cf; - cf= NULL; - break; - case IRI: - add_right(edge->outp[BELOW], xb, yb); - px= xb; - cf= edge->outp[BELOW]; - edge->outp[BELOW]= NULL; - break; - case IMX: - if (xb != px) - { - add_right(cf, xb, yb); - px= xb; - } - merge_left(cf, edge->outp[BELOW], out_poly); - cf= NULL; - edge->outp[BELOW]= NULL; - break; - case IMM: - if (xb != px) - { - add_right(cf, xb, yb); - px= xb; - } - merge_left(cf, edge->outp[BELOW], out_poly); - edge->outp[BELOW]= NULL; - add_local_min(&out_poly, edge, xb, yb); - cf= edge->outp[ABOVE]; - break; - case EMM: - if (xb != px) - { - add_left(cf, xb, yb); - px= xb; - } - merge_right(cf, edge->outp[BELOW], out_poly); - edge->outp[BELOW]= NULL; - add_local_min(&out_poly, edge, xb, yb); - cf= edge->outp[ABOVE]; - break; - case LED: - if (edge->bot.y == yb) - add_left(edge->outp[BELOW], xb, yb); - edge->outp[ABOVE]= edge->outp[BELOW]; - px= xb; - break; - case RED: - if (edge->bot.y == yb) - add_right(edge->outp[BELOW], xb, yb); - edge->outp[ABOVE]= edge->outp[BELOW]; - px= xb; - break; - default: - break; - } /* End of switch */ - } /* End of contributing conditional */ - } /* End of edge exists conditional */ - } /* End of AET loop */ - - /* Delete terminating edges from the AET, otherwise compute xt */ - for (edge= aet; edge; edge= edge->next) - { - if (edge->top.y == yb) - { - prev_edge= edge->prev; - next_edge= edge->next; - if (prev_edge) - prev_edge->next= next_edge; - else - aet= next_edge; - if (next_edge) - next_edge->prev= prev_edge; - - /* Copy bundle head state to the adjacent tail edge if required */ - if ((edge->bstate[BELOW] == BUNDLE_HEAD) && prev_edge) + /* Set yb and yt to the bottom and top of the scanbeam */ + yb= sbt[scanbeam++]; + if (scanbeam < sbt_entries) { - if (prev_edge->bstate[BELOW] == BUNDLE_TAIL) - { - prev_edge->outp[BELOW]= edge->outp[BELOW]; - prev_edge->bstate[BELOW]= UNBUNDLED; - if (prev_edge->prev) - if (prev_edge->prev->bstate[BELOW] == BUNDLE_TAIL) - prev_edge->bstate[BELOW]= BUNDLE_HEAD; + yt= sbt[scanbeam]; + dy= yt - yb; + } + + /* === SCANBEAM BOUNDARY PROCESSING ================================ */ + + /* If LMT node corresponding to yb exists */ + if (local_min) + { + if (local_min->y == yb) + { + /* Add edges starting at this local minimum to the AET */ + for (edge= local_min->first_bound; edge; edge= edge->next_bound) + add_edge_to_aet(&aet, edge, NULL); + + local_min= local_min->next; } } - } - else - { - if (edge->top.y == yt) - edge->xt= edge->top.x; - else - edge->xt= edge->bot.x + edge->dx * (yt - edge->bot.y); - } - } - if (scanbeam < sbt_entries) - { - /* === SCANBEAM INTERIOR PROCESSING ============================== */ + /* Set dummy previous x value */ + px= -DBL_MAX; - build_intersection_table(&it, aet, dy); + /* Create bundles within AET */ + e0= aet; + e1= aet; - /* Process each node in the intersection table */ - for (intersect= it; intersect; intersect= intersect->next) - { - e0= intersect->ie[0]; - e1= intersect->ie[1]; + /* Set up bundle fields of first edge */ + aet->bundle[ABOVE][ aet->type]= (aet->top.y != yb); + aet->bundle[ABOVE][!aet->type]= FALSE; + aet->bstate[ABOVE]= UNBUNDLED; - /* Only generate output for contributing intersections */ - if ((e0->bundle[ABOVE][CLIP] || e0->bundle[ABOVE][SUBJ]) - && (e1->bundle[ABOVE][CLIP] || e1->bundle[ABOVE][SUBJ])) + for (next_edge= aet->next; next_edge; next_edge= next_edge->next) { - p= e0->outp[ABOVE]; - q= e1->outp[ABOVE]; - ix= intersect->point.x; - iy= intersect->point.y + yb; - - in[CLIP]= ( e0->bundle[ABOVE][CLIP] && !e0->bside[CLIP]) - || ( e1->bundle[ABOVE][CLIP] && e1->bside[CLIP]) - || (!e0->bundle[ABOVE][CLIP] && !e1->bundle[ABOVE][CLIP] - && e0->bside[CLIP] && e1->bside[CLIP]); - in[SUBJ]= ( e0->bundle[ABOVE][SUBJ] && !e0->bside[SUBJ]) - || ( e1->bundle[ABOVE][SUBJ] && e1->bside[SUBJ]) - || (!e0->bundle[ABOVE][SUBJ] && !e1->bundle[ABOVE][SUBJ] - && e0->bside[SUBJ] && e1->bside[SUBJ]); - - /* Determine quadrant occupancies */ - switch (op) - { - case GPC_DIFF: - case GPC_INT: - tr= (in[CLIP]) - && (in[SUBJ]); - tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) - && (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); - br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) - && (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) - && (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - break; - case GPC_XOR: - tr= (in[CLIP]) - ^ (in[SUBJ]); - tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) - ^ (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); - br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) - ^ (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) - ^ (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - break; - case GPC_UNION: - tr= (in[CLIP]) - || (in[SUBJ]); - tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) - || (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); - br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) - || (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) - || (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - break; - } - - vclass= tr + (tl << 1) + (br << 2) + (bl << 3); + /* Set up bundle fields of next edge */ + next_edge->bundle[ABOVE][ next_edge->type]= (next_edge->top.y != yb); + next_edge->bundle[ABOVE][!next_edge->type]= FALSE; + next_edge->bstate[ABOVE]= UNBUNDLED; - switch (vclass) - { - case EMN: - add_local_min(&out_poly, e0, ix, iy); - e1->outp[ABOVE]= e0->outp[ABOVE]; - break; - case ERI: - if (p) - { - add_right(p, ix, iy); - e1->outp[ABOVE]= p; - e0->outp[ABOVE]= NULL; - } - break; - case ELI: - if (q) - { - add_left(q, ix, iy); - e0->outp[ABOVE]= q; - e1->outp[ABOVE]= NULL; - } - break; - case EMX: - if (p && q) - { - add_left(p, ix, iy); - merge_right(p, q, out_poly); - e0->outp[ABOVE]= NULL; - e1->outp[ABOVE]= NULL; - } - break; - case IMN: - add_local_min(&out_poly, e0, ix, iy); - e1->outp[ABOVE]= e0->outp[ABOVE]; - break; - case ILI: - if (p) - { - add_left(p, ix, iy); - e1->outp[ABOVE]= p; - e0->outp[ABOVE]= NULL; - } - break; - case IRI: - if (q) - { - add_right(q, ix, iy); - e0->outp[ABOVE]= q; - e1->outp[ABOVE]= NULL; - } - break; - case IMX: - if (p && q) - { - add_right(p, ix, iy); - merge_left(p, q, out_poly); - e0->outp[ABOVE]= NULL; - e1->outp[ABOVE]= NULL; - } - break; - case IMM: - if (p && q) - { - add_right(p, ix, iy); - merge_left(p, q, out_poly); - add_local_min(&out_poly, e0, ix, iy); - e1->outp[ABOVE]= e0->outp[ABOVE]; - } - break; - case EMM: - if (p && q) - { - add_left(p, ix, iy); - merge_right(p, q, out_poly); - add_local_min(&out_poly, e0, ix, iy); - e1->outp[ABOVE]= e0->outp[ABOVE]; - } - break; - default: - break; - } /* End of switch */ + /* Bundle edges above the scanbeam boundary if they coincide */ + if (next_edge->bundle[ABOVE][next_edge->type]) + { + if (EQ(e0->xb, next_edge->xb) && EQ(e0->dx, next_edge->dx) + && (e0->top.y != yb)) + { + next_edge->bundle[ABOVE][ next_edge->type]^= + e0->bundle[ABOVE][ next_edge->type]; + next_edge->bundle[ABOVE][!next_edge->type]= + e0->bundle[ABOVE][!next_edge->type]; + next_edge->bstate[ABOVE]= BUNDLE_HEAD; + e0->bundle[ABOVE][CLIP]= FALSE; + e0->bundle[ABOVE][SUBJ]= FALSE; + e0->bstate[ABOVE]= BUNDLE_TAIL; + } + e0= next_edge; + } + } + + horiz[CLIP]= NH; + horiz[SUBJ]= NH; + + /* Process each edge at this scanbeam boundary */ + for (edge= aet; edge; edge= edge->next) + { + exists[CLIP]= edge->bundle[ABOVE][CLIP] + + (edge->bundle[BELOW][CLIP] << 1); + exists[SUBJ]= edge->bundle[ABOVE][SUBJ] + + (edge->bundle[BELOW][SUBJ] << 1); + + if (exists[CLIP] || exists[SUBJ]) + { + /* Set bundle side */ + edge->bside[CLIP]= parity[CLIP]; + edge->bside[SUBJ]= parity[SUBJ]; + + /* Determine contributing status and quadrant occupancies */ + switch (op) + { + case GPC_DIFF: + case GPC_INT: + contributing= (exists[CLIP] && (parity[SUBJ] || horiz[SUBJ])) + || (exists[SUBJ] && (parity[CLIP] || horiz[CLIP])) + || (exists[CLIP] && exists[SUBJ] + && (parity[CLIP] == parity[SUBJ])); + br= (parity[CLIP]) + && (parity[SUBJ]); + bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) + && (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); + tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) + && (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); + tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) + && (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); + break; + case GPC_XOR: + contributing= exists[CLIP] || exists[SUBJ]; + br= (parity[CLIP]) + ^ (parity[SUBJ]); + bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) + ^ (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); + tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) + ^ (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); + tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) + ^ (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); + break; + case GPC_UNION: + contributing= (exists[CLIP] && (!parity[SUBJ] || horiz[SUBJ])) + || (exists[SUBJ] && (!parity[CLIP] || horiz[CLIP])) + || (exists[CLIP] && exists[SUBJ] + && (parity[CLIP] == parity[SUBJ])); + br= (parity[CLIP]) + || (parity[SUBJ]); + bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) + || (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); + tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) + || (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); + tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) + || (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); + break; + } + + /* Update parity */ + parity[CLIP]^= edge->bundle[ABOVE][CLIP]; + parity[SUBJ]^= edge->bundle[ABOVE][SUBJ]; + + /* Update horizontal state */ + if (exists[CLIP]) + horiz[CLIP]= + next_h_state[horiz[CLIP]] + [((exists[CLIP] - 1) << 1) + parity[CLIP]]; + if (exists[SUBJ]) + horiz[SUBJ]= + next_h_state[horiz[SUBJ]] + [((exists[SUBJ] - 1) << 1) + parity[SUBJ]]; + + vclass= tr + (tl << 1) + (br << 2) + (bl << 3); + + if (contributing) + { + xb= edge->xb; + + switch (vclass) + { + case EMN: + case IMN: + add_local_min(&out_poly, edge, xb, yb); + px= xb; + cf= edge->outp[ABOVE]; + break; + case ERI: + if (xb != px) + { + add_right(cf, xb, yb); + px= xb; + } + edge->outp[ABOVE]= cf; + cf= NULL; + break; + case ELI: + add_left(edge->outp[BELOW], xb, yb); + px= xb; + cf= edge->outp[BELOW]; + break; + case EMX: + if (xb != px) + { + add_left(cf, xb, yb); + px= xb; + } + merge_right(cf, edge->outp[BELOW], out_poly); + cf= NULL; + break; + case ILI: + if (xb != px) + { + add_left(cf, xb, yb); + px= xb; + } + edge->outp[ABOVE]= cf; + cf= NULL; + break; + case IRI: + add_right(edge->outp[BELOW], xb, yb); + px= xb; + cf= edge->outp[BELOW]; + edge->outp[BELOW]= NULL; + break; + case IMX: + if (xb != px) + { + add_right(cf, xb, yb); + px= xb; + } + merge_left(cf, edge->outp[BELOW], out_poly); + cf= NULL; + edge->outp[BELOW]= NULL; + break; + case IMM: + if (xb != px) + { + add_right(cf, xb, yb); + px= xb; + } + merge_left(cf, edge->outp[BELOW], out_poly); + edge->outp[BELOW]= NULL; + add_local_min(&out_poly, edge, xb, yb); + cf= edge->outp[ABOVE]; + break; + case EMM: + if (xb != px) + { + add_left(cf, xb, yb); + px= xb; + } + merge_right(cf, edge->outp[BELOW], out_poly); + edge->outp[BELOW]= NULL; + add_local_min(&out_poly, edge, xb, yb); + cf= edge->outp[ABOVE]; + break; + case LED: + if (edge->bot.y == yb) + add_left(edge->outp[BELOW], xb, yb); + edge->outp[ABOVE]= edge->outp[BELOW]; + px= xb; + break; + case RED: + if (edge->bot.y == yb) + add_right(edge->outp[BELOW], xb, yb); + edge->outp[ABOVE]= edge->outp[BELOW]; + px= xb; + break; + default: + break; + } /* End of switch */ + } /* End of contributing conditional */ + } /* End of edge exists conditional */ + } /* End of AET loop */ + + /* Delete terminating edges from the AET, otherwise compute xt */ + for (edge= aet; edge; edge= edge->next) + { + if (edge->top.y == yb) + { + prev_edge= edge->prev; + next_edge= edge->next; + if (prev_edge) + prev_edge->next= next_edge; + else + aet= next_edge; + if (next_edge) + next_edge->prev= prev_edge; + + /* Copy bundle head state to the adjacent tail edge if required */ + if ((edge->bstate[BELOW] == BUNDLE_HEAD) && prev_edge) + { + if (prev_edge->bstate[BELOW] == BUNDLE_TAIL) + { + prev_edge->outp[BELOW]= edge->outp[BELOW]; + prev_edge->bstate[BELOW]= UNBUNDLED; + if (prev_edge->prev) + if (prev_edge->prev->bstate[BELOW] == BUNDLE_TAIL) + prev_edge->bstate[BELOW]= BUNDLE_HEAD; + } + } + } + else + { + if (edge->top.y == yt) + edge->xt= edge->top.x; + else + edge->xt= edge->bot.x + edge->dx * (yt - edge->bot.y); + } + } + + if (scanbeam < sbt_entries) + { + /* === SCANBEAM INTERIOR PROCESSING ============================== */ + + build_intersection_table(&it, aet, dy); + + /* Process each node in the intersection table */ + for (intersect= it; intersect; intersect= intersect->next) + { + e0= intersect->ie[0]; + e1= intersect->ie[1]; + + /* Only generate output for contributing intersections */ + if ((e0->bundle[ABOVE][CLIP] || e0->bundle[ABOVE][SUBJ]) + && (e1->bundle[ABOVE][CLIP] || e1->bundle[ABOVE][SUBJ])) + { + p= e0->outp[ABOVE]; + q= e1->outp[ABOVE]; + ix= intersect->point.x; + iy= intersect->point.y + yb; + + in[CLIP]= ( e0->bundle[ABOVE][CLIP] && !e0->bside[CLIP]) + || ( e1->bundle[ABOVE][CLIP] && e1->bside[CLIP]) + || (!e0->bundle[ABOVE][CLIP] && !e1->bundle[ABOVE][CLIP] + && e0->bside[CLIP] && e1->bside[CLIP]); + in[SUBJ]= ( e0->bundle[ABOVE][SUBJ] && !e0->bside[SUBJ]) + || ( e1->bundle[ABOVE][SUBJ] && e1->bside[SUBJ]) + || (!e0->bundle[ABOVE][SUBJ] && !e1->bundle[ABOVE][SUBJ] + && e0->bside[SUBJ] && e1->bside[SUBJ]); + + /* Determine quadrant occupancies */ + switch (op) + { + case GPC_DIFF: + case GPC_INT: + tr= (in[CLIP]) + && (in[SUBJ]); + tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) + && (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); + br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) + && (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) + && (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + break; + case GPC_XOR: + tr= (in[CLIP]) + ^ (in[SUBJ]); + tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) + ^ (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); + br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) + ^ (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) + ^ (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + break; + case GPC_UNION: + tr= (in[CLIP]) + || (in[SUBJ]); + tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) + || (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); + br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) + || (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) + || (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + break; + } + + vclass= tr + (tl << 1) + (br << 2) + (bl << 3); + + switch (vclass) + { + case EMN: + add_local_min(&out_poly, e0, ix, iy); + e1->outp[ABOVE]= e0->outp[ABOVE]; + break; + case ERI: + if (p) + { + add_right(p, ix, iy); + e1->outp[ABOVE]= p; + e0->outp[ABOVE]= NULL; + } + break; + case ELI: + if (q) + { + add_left(q, ix, iy); + e0->outp[ABOVE]= q; + e1->outp[ABOVE]= NULL; + } + break; + case EMX: + if (p && q) + { + add_left(p, ix, iy); + merge_right(p, q, out_poly); + e0->outp[ABOVE]= NULL; + e1->outp[ABOVE]= NULL; + } + break; + case IMN: + add_local_min(&out_poly, e0, ix, iy); + e1->outp[ABOVE]= e0->outp[ABOVE]; + break; + case ILI: + if (p) + { + add_left(p, ix, iy); + e1->outp[ABOVE]= p; + e0->outp[ABOVE]= NULL; + } + break; + case IRI: + if (q) + { + add_right(q, ix, iy); + e0->outp[ABOVE]= q; + e1->outp[ABOVE]= NULL; + } + break; + case IMX: + if (p && q) + { + add_right(p, ix, iy); + merge_left(p, q, out_poly); + e0->outp[ABOVE]= NULL; + e1->outp[ABOVE]= NULL; + } + break; + case IMM: + if (p && q) + { + add_right(p, ix, iy); + merge_left(p, q, out_poly); + add_local_min(&out_poly, e0, ix, iy); + e1->outp[ABOVE]= e0->outp[ABOVE]; + } + break; + case EMM: + if (p && q) + { + add_left(p, ix, iy); + merge_right(p, q, out_poly); + add_local_min(&out_poly, e0, ix, iy); + e1->outp[ABOVE]= e0->outp[ABOVE]; + } + break; + default: + break; + } /* End of switch */ } /* End of contributing intersection conditional */ - /* Swap bundle sides in response to edge crossing */ - if (e0->bundle[ABOVE][CLIP]) + /* Swap bundle sides in response to edge crossing */ + if (e0->bundle[ABOVE][CLIP]) e1->bside[CLIP]= !e1->bside[CLIP]; - if (e1->bundle[ABOVE][CLIP]) + if (e1->bundle[ABOVE][CLIP]) e0->bside[CLIP]= !e0->bside[CLIP]; - if (e0->bundle[ABOVE][SUBJ]) + if (e0->bundle[ABOVE][SUBJ]) e1->bside[SUBJ]= !e1->bside[SUBJ]; - if (e1->bundle[ABOVE][SUBJ]) + if (e1->bundle[ABOVE][SUBJ]) e0->bside[SUBJ]= !e0->bside[SUBJ]; - /* Swap e0 and e1 bundles in the AET */ - prev_edge= e0->prev; - next_edge= e1->next; - if (next_edge) - next_edge->prev= e0; + /* Swap e0 and e1 bundles in the AET */ + prev_edge= e0->prev; + next_edge= e1->next; + if (next_edge) + next_edge->prev= e0; - if (e0->bstate[ABOVE] == BUNDLE_HEAD) - { - search= TRUE; - while (search) - { - prev_edge= prev_edge->prev; - if (prev_edge) - { - if (prev_edge->bstate[ABOVE] != BUNDLE_TAIL) - search= FALSE; - } - else - search= FALSE; - } - } - if (!prev_edge) - { - aet->prev= e1; - e1->next= aet; - aet= e0->next; - } - else - { - prev_edge->next->prev= e1; - e1->next= prev_edge->next; - prev_edge->next= e0->next; - } - e0->next->prev= prev_edge; - e1->next->prev= e1; - e0->next= next_edge; - } /* End of IT loop*/ + if (e0->bstate[ABOVE] == BUNDLE_HEAD) + { + search= TRUE; + while (search) + { + prev_edge= prev_edge->prev; + if (prev_edge) + { + if (prev_edge->bstate[ABOVE] != BUNDLE_TAIL) + search= FALSE; + } + else + search= FALSE; + } + } + if (!prev_edge) + { + aet->prev= e1; + e1->next= aet; + aet= e0->next; + } + else + { + prev_edge->next->prev= e1; + e1->next= prev_edge->next; + prev_edge->next= e0->next; + } + e0->next->prev= prev_edge; + e1->next->prev= e1; + e0->next= next_edge; + } /* End of IT loop*/ - /* Prepare for next scanbeam */ - for (edge= aet; edge; edge= next_edge) - { - next_edge= edge->next; - succ_edge= edge->succ; + /* Prepare for next scanbeam */ + for (edge= aet; edge; edge= next_edge) + { + next_edge= edge->next; + succ_edge= edge->succ; - if ((edge->top.y == yt) && succ_edge) - { - /* Replace AET edge by its successor */ - succ_edge->outp[BELOW]= edge->outp[ABOVE]; - succ_edge->bstate[BELOW]= edge->bstate[ABOVE]; - succ_edge->bundle[BELOW][CLIP]= edge->bundle[ABOVE][CLIP]; - succ_edge->bundle[BELOW][SUBJ]= edge->bundle[ABOVE][SUBJ]; - prev_edge= edge->prev; - if (prev_edge) - prev_edge->next= succ_edge; - else - aet= succ_edge; - if (next_edge) - next_edge->prev= succ_edge; - succ_edge->prev= prev_edge; - succ_edge->next= next_edge; - } - else - { - /* Update this edge */ - edge->outp[BELOW]= edge->outp[ABOVE]; - edge->bstate[BELOW]= edge->bstate[ABOVE]; - edge->bundle[BELOW][CLIP]= edge->bundle[ABOVE][CLIP]; - edge->bundle[BELOW][SUBJ]= edge->bundle[ABOVE][SUBJ]; - edge->xb= edge->xt; - } - edge->outp[ABOVE]= NULL; - } - } + if ((edge->top.y == yt) && succ_edge) + { + /* Replace AET edge by its successor */ + succ_edge->outp[BELOW]= edge->outp[ABOVE]; + succ_edge->bstate[BELOW]= edge->bstate[ABOVE]; + succ_edge->bundle[BELOW][CLIP]= edge->bundle[ABOVE][CLIP]; + succ_edge->bundle[BELOW][SUBJ]= edge->bundle[ABOVE][SUBJ]; + prev_edge= edge->prev; + if (prev_edge) + prev_edge->next= succ_edge; + else + aet= succ_edge; + if (next_edge) + next_edge->prev= succ_edge; + succ_edge->prev= prev_edge; + succ_edge->next= next_edge; + } + else + { + /* Update this edge */ + edge->outp[BELOW]= edge->outp[ABOVE]; + edge->bstate[BELOW]= edge->bstate[ABOVE]; + edge->bundle[BELOW][CLIP]= edge->bundle[ABOVE][CLIP]; + edge->bundle[BELOW][SUBJ]= edge->bundle[ABOVE][SUBJ]; + edge->xb= edge->xt; + } + edge->outp[ABOVE]= NULL; + } + } } /* === END OF SCANBEAM PROCESSING ================================== */ /* Generate result polygon from out_poly */ @@ -1703,44 +1705,44 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, result->num_contours= count_contours(out_poly); if (result->num_contours > 0) { - MALLOC(result->hole, result->num_contours - * sizeof(int), "hole flag table creation", int); - MALLOC(result->contour, result->num_contours - * sizeof(gpc_vertex_list), "contour creation", gpc_vertex_list); + MALLOC(result->hole, result->num_contours + * sizeof(int), "hole flag table creation", int); + MALLOC(result->contour, result->num_contours + * sizeof(gpc_vertex_list), "contour creation", gpc_vertex_list); - c= 0; - for (poly= out_poly; poly; poly= npoly) - { - npoly= poly->next; - if (poly->active) - { - result->hole[c]= poly->proxy->hole; - result->contour[c].num_vertices= poly->active; - MALLOC(result->contour[c].vertex, - result->contour[c].num_vertices * sizeof(gpc_vertex), - "vertex creation", gpc_vertex); - - v= result->contour[c].num_vertices - 1; + c= 0; + for (poly= out_poly; poly; poly= npoly) + { + npoly= poly->next; + if (poly->active) + { + result->hole[c]= poly->proxy->hole; + result->contour[c].num_vertices= poly->active; + MALLOC(result->contour[c].vertex, + result->contour[c].num_vertices * sizeof(gpc_vertex), + "vertex creation", gpc_vertex); + + v= result->contour[c].num_vertices - 1; for (vtx= poly->proxy->v[GPC_LEFT]; vtx; vtx= nv) - { - nv= vtx->next; - result->contour[c].vertex[v].x= vtx->x; - result->contour[c].vertex[v].y= vtx->y; - FREE(vtx); - v--; - } - c++; - } - FREE(poly); - } + { + nv= vtx->next; + result->contour[c].vertex[v].x= vtx->x; + result->contour[c].vertex[v].y= vtx->y; + FREE(vtx); + v--; + } + c++; + } + FREE(poly); + } } else { - for (poly= out_poly; poly; poly= npoly) - { - npoly= poly->next; - FREE(poly); - } + for (poly= out_poly; poly; poly= npoly) + { + npoly= poly->next; + FREE(poly); + } } /* Tidy up */ @@ -1757,7 +1759,7 @@ void gpc_free_tristrip(gpc_tristrip *t) int s; for (s= 0; s < t->num_strips; s++) - FREE(t->strip[s].vertex); + FREE(t->strip[s].vertex); FREE(t->strip); t->num_strips= 0; } @@ -1775,7 +1777,7 @@ void gpc_polygon_to_tristrip(gpc_polygon *s, gpc_tristrip *t) void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, - gpc_tristrip *result) + gpc_tristrip *result) { sb_tree *sbtree= NULL; it_node *it= NULL, *intersect; @@ -1796,31 +1798,31 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, || ((subj->num_contours == 0) && ((op == GPC_INT) || (op == GPC_DIFF))) || ((clip->num_contours == 0) && (op == GPC_INT))) { - result->num_strips= 0; - result->strip= NULL; - return; + result->num_strips= 0; + result->strip= NULL; + return; } /* Identify potentialy contributing contours */ if (((op == GPC_INT) || (op == GPC_DIFF)) && (subj->num_contours > 0) && (clip->num_contours > 0)) - minimax_test(subj, clip, op); + minimax_test(subj, clip, op); /* Build LMT */ if (subj->num_contours > 0) - s_heap= build_lmt(&lmt, &sbtree, &sbt_entries, subj, SUBJ, op); + s_heap= build_lmt(&lmt, &sbtree, &sbt_entries, subj, SUBJ, op); if (clip->num_contours > 0) - c_heap= build_lmt(&lmt, &sbtree, &sbt_entries, clip, CLIP, op); + c_heap= build_lmt(&lmt, &sbtree, &sbt_entries, clip, CLIP, op); /* Return a NULL result if no contours contribute */ if (lmt == NULL) { - result->num_strips= 0; - result->strip= NULL; - reset_lmt(&lmt); - FREE(s_heap); - FREE(c_heap); - return; + result->num_strips= 0; + result->strip= NULL; + reset_lmt(&lmt); + FREE(s_heap); + FREE(c_heap); + return; } /* Build scanbeam table from scanbeam tree */ @@ -1838,552 +1840,552 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, /* Process each scanbeam */ while (scanbeam < sbt_entries) { - /* Set yb and yt to the bottom and top of the scanbeam */ - yb= sbt[scanbeam++]; - if (scanbeam < sbt_entries) - { - yt= sbt[scanbeam]; - dy= yt - yb; - } + /* Set yb and yt to the bottom and top of the scanbeam */ + yb= sbt[scanbeam++]; + if (scanbeam < sbt_entries) + { + yt= sbt[scanbeam]; + dy= yt - yb; + } - /* === SCANBEAM BOUNDARY PROCESSING ================================ */ + /* === SCANBEAM BOUNDARY PROCESSING ================================ */ - /* If LMT node corresponding to yb exists */ - if (local_min) - { - if (local_min->y == yb) - { - /* Add edges starting at this local minimum to the AET */ - for (edge= local_min->first_bound; edge; edge= edge->next_bound) - add_edge_to_aet(&aet, edge, NULL); + /* If LMT node corresponding to yb exists */ + if (local_min) + { + if (local_min->y == yb) + { + /* Add edges starting at this local minimum to the AET */ + for (edge= local_min->first_bound; edge; edge= edge->next_bound) + add_edge_to_aet(&aet, edge, NULL); - local_min= local_min->next; - } - } + local_min= local_min->next; + } + } - /* Set dummy previous x value */ - px= -DBL_MAX; + /* Set dummy previous x value */ + px= -DBL_MAX; - /* Create bundles within AET */ - e0= aet; - e1= aet; + /* Create bundles within AET */ + e0= aet; + e1= aet; - /* Set up bundle fields of first edge */ - aet->bundle[ABOVE][ aet->type]= (aet->top.y != yb); - aet->bundle[ABOVE][!aet->type]= FALSE; - aet->bstate[ABOVE]= UNBUNDLED; + /* Set up bundle fields of first edge */ + aet->bundle[ABOVE][ aet->type]= (aet->top.y != yb); + aet->bundle[ABOVE][!aet->type]= FALSE; + aet->bstate[ABOVE]= UNBUNDLED; - for (next_edge= aet->next; next_edge; next_edge= next_edge->next) - { - /* Set up bundle fields of next edge */ - next_edge->bundle[ABOVE][ next_edge->type]= (next_edge->top.y != yb); - next_edge->bundle[ABOVE][!next_edge->type]= FALSE; - next_edge->bstate[ABOVE]= UNBUNDLED; + for (next_edge= aet->next; next_edge; next_edge= next_edge->next) + { + /* Set up bundle fields of next edge */ + next_edge->bundle[ABOVE][ next_edge->type]= (next_edge->top.y != yb); + next_edge->bundle[ABOVE][!next_edge->type]= FALSE; + next_edge->bstate[ABOVE]= UNBUNDLED; - /* Bundle edges above the scanbeam boundary if they coincide */ - if (next_edge->bundle[ABOVE][next_edge->type]) - { - if (EQ(e0->xb, next_edge->xb) && EQ(e0->dx, next_edge->dx) + /* Bundle edges above the scanbeam boundary if they coincide */ + if (next_edge->bundle[ABOVE][next_edge->type]) + { + if (EQ(e0->xb, next_edge->xb) && EQ(e0->dx, next_edge->dx) && (e0->top.y != yb)) - { - next_edge->bundle[ABOVE][ next_edge->type]^= - e0->bundle[ABOVE][ next_edge->type]; - next_edge->bundle[ABOVE][!next_edge->type]= - e0->bundle[ABOVE][!next_edge->type]; - next_edge->bstate[ABOVE]= BUNDLE_HEAD; - e0->bundle[ABOVE][CLIP]= FALSE; - e0->bundle[ABOVE][SUBJ]= FALSE; - e0->bstate[ABOVE]= BUNDLE_TAIL; - } - e0= next_edge; - } - } + { + next_edge->bundle[ABOVE][ next_edge->type]^= + e0->bundle[ABOVE][ next_edge->type]; + next_edge->bundle[ABOVE][!next_edge->type]= + e0->bundle[ABOVE][!next_edge->type]; + next_edge->bstate[ABOVE]= BUNDLE_HEAD; + e0->bundle[ABOVE][CLIP]= FALSE; + e0->bundle[ABOVE][SUBJ]= FALSE; + e0->bstate[ABOVE]= BUNDLE_TAIL; + } + e0= next_edge; + } + } - horiz[CLIP]= NH; - horiz[SUBJ]= NH; + horiz[CLIP]= NH; + horiz[SUBJ]= NH; - /* Process each edge at this scanbeam boundary */ - for (edge= aet; edge; edge= edge->next) - { - exists[CLIP]= edge->bundle[ABOVE][CLIP] + - (edge->bundle[BELOW][CLIP] << 1); - exists[SUBJ]= edge->bundle[ABOVE][SUBJ] + - (edge->bundle[BELOW][SUBJ] << 1); + /* Process each edge at this scanbeam boundary */ + for (edge= aet; edge; edge= edge->next) + { + exists[CLIP]= edge->bundle[ABOVE][CLIP] + + (edge->bundle[BELOW][CLIP] << 1); + exists[SUBJ]= edge->bundle[ABOVE][SUBJ] + + (edge->bundle[BELOW][SUBJ] << 1); - if (exists[CLIP] || exists[SUBJ]) - { - /* Set bundle side */ - edge->bside[CLIP]= parity[CLIP]; - edge->bside[SUBJ]= parity[SUBJ]; + if (exists[CLIP] || exists[SUBJ]) + { + /* Set bundle side */ + edge->bside[CLIP]= parity[CLIP]; + edge->bside[SUBJ]= parity[SUBJ]; - /* Determine contributing status and quadrant occupancies */ - switch (op) - { - case GPC_DIFF: - case GPC_INT: - contributing= (exists[CLIP] && (parity[SUBJ] || horiz[SUBJ])) - || (exists[SUBJ] && (parity[CLIP] || horiz[CLIP])) - || (exists[CLIP] && exists[SUBJ] - && (parity[CLIP] == parity[SUBJ])); - br= (parity[CLIP]) - && (parity[SUBJ]); - bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) - && (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); - tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) - && (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); - tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) - && (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); - break; - case GPC_XOR: - contributing= exists[CLIP] || exists[SUBJ]; - br= (parity[CLIP]) - ^ (parity[SUBJ]); - bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) - ^ (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); - tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) - ^ (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); - tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) - ^ (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); - break; - case GPC_UNION: - contributing= (exists[CLIP] && (!parity[SUBJ] || horiz[SUBJ])) - || (exists[SUBJ] && (!parity[CLIP] || horiz[CLIP])) - || (exists[CLIP] && exists[SUBJ] - && (parity[CLIP] == parity[SUBJ])); - br= (parity[CLIP]) - || (parity[SUBJ]); - bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) - || (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); - tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) - || (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); - tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) - || (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); - break; - } + /* Determine contributing status and quadrant occupancies */ + switch (op) + { + case GPC_DIFF: + case GPC_INT: + contributing= (exists[CLIP] && (parity[SUBJ] || horiz[SUBJ])) + || (exists[SUBJ] && (parity[CLIP] || horiz[CLIP])) + || (exists[CLIP] && exists[SUBJ] + && (parity[CLIP] == parity[SUBJ])); + br= (parity[CLIP]) + && (parity[SUBJ]); + bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) + && (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); + tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) + && (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); + tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) + && (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); + break; + case GPC_XOR: + contributing= exists[CLIP] || exists[SUBJ]; + br= (parity[CLIP]) + ^ (parity[SUBJ]); + bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) + ^ (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); + tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) + ^ (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); + tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) + ^ (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); + break; + case GPC_UNION: + contributing= (exists[CLIP] && (!parity[SUBJ] || horiz[SUBJ])) + || (exists[SUBJ] && (!parity[CLIP] || horiz[CLIP])) + || (exists[CLIP] && exists[SUBJ] + && (parity[CLIP] == parity[SUBJ])); + br= (parity[CLIP]) + || (parity[SUBJ]); + bl= (parity[CLIP] ^ edge->bundle[ABOVE][CLIP]) + || (parity[SUBJ] ^ edge->bundle[ABOVE][SUBJ]); + tr= (parity[CLIP] ^ (horiz[CLIP]!=NH)) + || (parity[SUBJ] ^ (horiz[SUBJ]!=NH)); + tl= (parity[CLIP] ^ (horiz[CLIP]!=NH) ^ edge->bundle[BELOW][CLIP]) + || (parity[SUBJ] ^ (horiz[SUBJ]!=NH) ^ edge->bundle[BELOW][SUBJ]); + break; + } - /* Update parity */ - parity[CLIP]^= edge->bundle[ABOVE][CLIP]; - parity[SUBJ]^= edge->bundle[ABOVE][SUBJ]; + /* Update parity */ + parity[CLIP]^= edge->bundle[ABOVE][CLIP]; + parity[SUBJ]^= edge->bundle[ABOVE][SUBJ]; - /* Update horizontal state */ - if (exists[CLIP]) - horiz[CLIP]= - next_h_state[horiz[CLIP]] - [((exists[CLIP] - 1) << 1) + parity[CLIP]]; - if (exists[SUBJ]) - horiz[SUBJ]= - next_h_state[horiz[SUBJ]] - [((exists[SUBJ] - 1) << 1) + parity[SUBJ]]; - - vclass= tr + (tl << 1) + (br << 2) + (bl << 3); + /* Update horizontal state */ + if (exists[CLIP]) + horiz[CLIP]= + next_h_state[horiz[CLIP]] + [((exists[CLIP] - 1) << 1) + parity[CLIP]]; + if (exists[SUBJ]) + horiz[SUBJ]= + next_h_state[horiz[SUBJ]] + [((exists[SUBJ] - 1) << 1) + parity[SUBJ]]; - if (contributing) - { - xb= edge->xb; + vclass= tr + (tl << 1) + (br << 2) + (bl << 3); - switch (vclass) - { - case EMN: - new_tristrip(&tlist, edge, xb, yb); - cf= edge; - break; - case ERI: - edge->outp[ABOVE]= cf->outp[ABOVE]; - if (xb != cf->xb) + if (contributing) + { + xb= edge->xb; + + switch (vclass) + { + case EMN: + new_tristrip(&tlist, edge, xb, yb); + cf= edge; + break; + case ERI: + edge->outp[ABOVE]= cf->outp[ABOVE]; + if (xb != cf->xb) VERTEX(edge, ABOVE, GPC_RIGHT, xb, yb); - cf= NULL; - break; - case ELI: + cf= NULL; + break; + case ELI: VERTEX(edge, BELOW, GPC_LEFT, xb, yb); - edge->outp[ABOVE]= NULL; - cf= edge; - break; - case EMX: - if (xb != cf->xb) + edge->outp[ABOVE]= NULL; + cf= edge; + break; + case EMX: + if (xb != cf->xb) VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); - edge->outp[ABOVE]= NULL; - cf= NULL; - break; - case IMN: - if (cft == LED) - { - if (cf->bot.y != yb) + edge->outp[ABOVE]= NULL; + cf= NULL; + break; + case IMN: + if (cft == LED) + { + if (cf->bot.y != yb) VERTEX(cf, BELOW, GPC_LEFT, cf->xb, yb); - new_tristrip(&tlist, cf, cf->xb, yb); - } - edge->outp[ABOVE]= cf->outp[ABOVE]; + new_tristrip(&tlist, cf, cf->xb, yb); + } + edge->outp[ABOVE]= cf->outp[ABOVE]; VERTEX(edge, ABOVE, GPC_RIGHT, xb, yb); - break; - case ILI: - new_tristrip(&tlist, edge, xb, yb); - cf= edge; - cft= ILI; - break; - case IRI: - if (cft == LED) - { - if (cf->bot.y != yb) + break; + case ILI: + new_tristrip(&tlist, edge, xb, yb); + cf= edge; + cft= ILI; + break; + case IRI: + if (cft == LED) + { + if (cf->bot.y != yb) VERTEX(cf, BELOW, GPC_LEFT, cf->xb, yb); - new_tristrip(&tlist, cf, cf->xb, yb); - } + new_tristrip(&tlist, cf, cf->xb, yb); + } VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); - edge->outp[ABOVE]= NULL; - break; - case IMX: + edge->outp[ABOVE]= NULL; + break; + case IMX: VERTEX(edge, BELOW, GPC_LEFT, xb, yb); - edge->outp[ABOVE]= NULL; - cft= IMX; - break; + edge->outp[ABOVE]= NULL; + cft= IMX; + break; case IMM: VERTEX(edge, BELOW, GPC_LEFT, xb, yb); - edge->outp[ABOVE]= cf->outp[ABOVE]; - if (xb != cf->xb) + edge->outp[ABOVE]= cf->outp[ABOVE]; + if (xb != cf->xb) VERTEX(cf, ABOVE, GPC_RIGHT, xb, yb); - cf= edge; - break; - case EMM: + cf= edge; + break; + case EMM: VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); - edge->outp[ABOVE]= NULL; - new_tristrip(&tlist, edge, xb, yb); - cf= edge; - break; - case LED: - if (edge->bot.y == yb) + edge->outp[ABOVE]= NULL; + new_tristrip(&tlist, edge, xb, yb); + cf= edge; + break; + case LED: + if (edge->bot.y == yb) VERTEX(edge, BELOW, GPC_LEFT, xb, yb); - edge->outp[ABOVE]= edge->outp[BELOW]; - cf= edge; - cft= LED; - break; - case RED: - edge->outp[ABOVE]= cf->outp[ABOVE]; - if (cft == LED) - { - if (cf->bot.y == yb) - { + edge->outp[ABOVE]= edge->outp[BELOW]; + cf= edge; + cft= LED; + break; + case RED: + edge->outp[ABOVE]= cf->outp[ABOVE]; + if (cft == LED) + { + if (cf->bot.y == yb) + { VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); - } - else - { - if (edge->bot.y == yb) + } + else + { + if (edge->bot.y == yb) { VERTEX(cf, BELOW, GPC_LEFT, cf->xb, yb); VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); } - } - } - else - { + } + } + else + { VERTEX(edge, BELOW, GPC_RIGHT, xb, yb); VERTEX(edge, ABOVE, GPC_RIGHT, xb, yb); - } - cf= NULL; - break; - default: - break; - } /* End of switch */ - } /* End of contributing conditional */ - } /* End of edge exists conditional */ - } /* End of AET loop */ + } + cf= NULL; + break; + default: + break; + } /* End of switch */ + } /* End of contributing conditional */ + } /* End of edge exists conditional */ + } /* End of AET loop */ - /* Delete terminating edges from the AET, otherwise compute xt */ - for (edge= aet; edge; edge= edge->next) - { - if (edge->top.y == yb) - { - prev_edge= edge->prev; - next_edge= edge->next; - if (prev_edge) - prev_edge->next= next_edge; - else - aet= next_edge; - if (next_edge) - next_edge->prev= prev_edge; - - /* Copy bundle head state to the adjacent tail edge if required */ - if ((edge->bstate[BELOW] == BUNDLE_HEAD) && prev_edge) + /* Delete terminating edges from the AET, otherwise compute xt */ + for (edge= aet; edge; edge= edge->next) { - if (prev_edge->bstate[BELOW] == BUNDLE_TAIL) - { - prev_edge->outp[BELOW]= edge->outp[BELOW]; - prev_edge->bstate[BELOW]= UNBUNDLED; - if (prev_edge->prev) - if (prev_edge->prev->bstate[BELOW] == BUNDLE_TAIL) - prev_edge->bstate[BELOW]= BUNDLE_HEAD; + if (edge->top.y == yb) + { + prev_edge= edge->prev; + next_edge= edge->next; + if (prev_edge) + prev_edge->next= next_edge; + else + aet= next_edge; + if (next_edge) + next_edge->prev= prev_edge; + + /* Copy bundle head state to the adjacent tail edge if required */ + if ((edge->bstate[BELOW] == BUNDLE_HEAD) && prev_edge) + { + if (prev_edge->bstate[BELOW] == BUNDLE_TAIL) + { + prev_edge->outp[BELOW]= edge->outp[BELOW]; + prev_edge->bstate[BELOW]= UNBUNDLED; + if (prev_edge->prev) + if (prev_edge->prev->bstate[BELOW] == BUNDLE_TAIL) + prev_edge->bstate[BELOW]= BUNDLE_HEAD; + } + } + } + else + { + if (edge->top.y == yt) + edge->xt= edge->top.x; + else + edge->xt= edge->bot.x + edge->dx * (yt - edge->bot.y); } } - } - else - { - if (edge->top.y == yt) - edge->xt= edge->top.x; - else - edge->xt= edge->bot.x + edge->dx * (yt - edge->bot.y); - } - } - if (scanbeam < sbt_entries) - { - /* === SCANBEAM INTERIOR PROCESSING ============================== */ - - build_intersection_table(&it, aet, dy); - - /* Process each node in the intersection table */ - for (intersect= it; intersect; intersect= intersect->next) - { - e0= intersect->ie[0]; - e1= intersect->ie[1]; - - /* Only generate output for contributing intersections */ - if ((e0->bundle[ABOVE][CLIP] || e0->bundle[ABOVE][SUBJ]) - && (e1->bundle[ABOVE][CLIP] || e1->bundle[ABOVE][SUBJ])) + if (scanbeam < sbt_entries) { - p= e0->outp[ABOVE]; - q= e1->outp[ABOVE]; - ix= intersect->point.x; - iy= intersect->point.y + yb; + /* === SCANBEAM INTERIOR PROCESSING ============================== */ - in[CLIP]= ( e0->bundle[ABOVE][CLIP] && !e0->bside[CLIP]) - || ( e1->bundle[ABOVE][CLIP] && e1->bside[CLIP]) - || (!e0->bundle[ABOVE][CLIP] && !e1->bundle[ABOVE][CLIP] - && e0->bside[CLIP] && e1->bside[CLIP]); - in[SUBJ]= ( e0->bundle[ABOVE][SUBJ] && !e0->bside[SUBJ]) - || ( e1->bundle[ABOVE][SUBJ] && e1->bside[SUBJ]) - || (!e0->bundle[ABOVE][SUBJ] && !e1->bundle[ABOVE][SUBJ] - && e0->bside[SUBJ] && e1->bside[SUBJ]); + build_intersection_table(&it, aet, dy); - /* Determine quadrant occupancies */ - switch (op) - { - case GPC_DIFF: - case GPC_INT: - tr= (in[CLIP]) - && (in[SUBJ]); - tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) - && (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); - br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) - && (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) - && (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - break; - case GPC_XOR: - tr= (in[CLIP]) - ^ (in[SUBJ]); - tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) - ^ (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); - br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) - ^ (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) - ^ (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - break; - case GPC_UNION: - tr= (in[CLIP]) - || (in[SUBJ]); - tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) - || (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); - br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) - || (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) - || (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); - break; - } + /* Process each node in the intersection table */ + for (intersect= it; intersect; intersect= intersect->next) + { + e0= intersect->ie[0]; + e1= intersect->ie[1]; - vclass= tr + (tl << 1) + (br << 2) + (bl << 3); + /* Only generate output for contributing intersections */ + if ((e0->bundle[ABOVE][CLIP] || e0->bundle[ABOVE][SUBJ]) + && (e1->bundle[ABOVE][CLIP] || e1->bundle[ABOVE][SUBJ])) + { + p= e0->outp[ABOVE]; + q= e1->outp[ABOVE]; + ix= intersect->point.x; + iy= intersect->point.y + yb; - switch (vclass) - { - case EMN: - new_tristrip(&tlist, e1, ix, iy); - e0->outp[ABOVE]= e1->outp[ABOVE]; - break; - case ERI: - if (p) - { - P_EDGE(prev_edge, e0, ABOVE, px, iy); + in[CLIP]= ( e0->bundle[ABOVE][CLIP] && !e0->bside[CLIP]) + || ( e1->bundle[ABOVE][CLIP] && e1->bside[CLIP]) + || (!e0->bundle[ABOVE][CLIP] && !e1->bundle[ABOVE][CLIP] + && e0->bside[CLIP] && e1->bside[CLIP]); + in[SUBJ]= ( e0->bundle[ABOVE][SUBJ] && !e0->bside[SUBJ]) + || ( e1->bundle[ABOVE][SUBJ] && e1->bside[SUBJ]) + || (!e0->bundle[ABOVE][SUBJ] && !e1->bundle[ABOVE][SUBJ] + && e0->bside[SUBJ] && e1->bside[SUBJ]); + + /* Determine quadrant occupancies */ + switch (op) + { + case GPC_DIFF: + case GPC_INT: + tr= (in[CLIP]) + && (in[SUBJ]); + tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) + && (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); + br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) + && (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) + && (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + break; + case GPC_XOR: + tr= (in[CLIP]) + ^ (in[SUBJ]); + tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) + ^ (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); + br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) + ^ (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) + ^ (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + break; + case GPC_UNION: + tr= (in[CLIP]) + || (in[SUBJ]); + tl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP]) + || (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ]); + br= (in[CLIP] ^ e0->bundle[ABOVE][CLIP]) + || (in[SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + bl= (in[CLIP] ^ e1->bundle[ABOVE][CLIP] ^ e0->bundle[ABOVE][CLIP]) + || (in[SUBJ] ^ e1->bundle[ABOVE][SUBJ] ^ e0->bundle[ABOVE][SUBJ]); + break; + } + + vclass= tr + (tl << 1) + (br << 2) + (bl << 3); + + switch (vclass) + { + case EMN: + new_tristrip(&tlist, e1, ix, iy); + e0->outp[ABOVE]= e1->outp[ABOVE]; + break; + case ERI: + if (p) + { + P_EDGE(prev_edge, e0, ABOVE, px, iy); VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); VERTEX(e0, ABOVE, GPC_RIGHT, ix, iy); - e1->outp[ABOVE]= e0->outp[ABOVE]; - e0->outp[ABOVE]= NULL; - } - break; - case ELI: - if (q) - { - N_EDGE(next_edge, e1, ABOVE, nx, iy); + e1->outp[ABOVE]= e0->outp[ABOVE]; + e0->outp[ABOVE]= NULL; + } + break; + case ELI: + if (q) + { + N_EDGE(next_edge, e1, ABOVE, nx, iy); VERTEX(e1, ABOVE, GPC_LEFT, ix, iy); VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); - e0->outp[ABOVE]= e1->outp[ABOVE]; - e1->outp[ABOVE]= NULL; - } - break; - case EMX: - if (p && q) - { + e0->outp[ABOVE]= e1->outp[ABOVE]; + e1->outp[ABOVE]= NULL; + } + break; + case EMX: + if (p && q) + { VERTEX(e0, ABOVE, GPC_LEFT, ix, iy); - e0->outp[ABOVE]= NULL; - e1->outp[ABOVE]= NULL; - } - break; - case IMN: - P_EDGE(prev_edge, e0, ABOVE, px, iy); + e0->outp[ABOVE]= NULL; + e1->outp[ABOVE]= NULL; + } + break; + case IMN: + P_EDGE(prev_edge, e0, ABOVE, px, iy); VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); - N_EDGE(next_edge, e1, ABOVE, nx, iy); + N_EDGE(next_edge, e1, ABOVE, nx, iy); VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); - new_tristrip(&tlist, prev_edge, px, iy); - e1->outp[ABOVE]= prev_edge->outp[ABOVE]; + new_tristrip(&tlist, prev_edge, px, iy); + e1->outp[ABOVE]= prev_edge->outp[ABOVE]; VERTEX(e1, ABOVE, GPC_RIGHT, ix, iy); - new_tristrip(&tlist, e0, ix, iy); - next_edge->outp[ABOVE]= e0->outp[ABOVE]; + new_tristrip(&tlist, e0, ix, iy); + next_edge->outp[ABOVE]= e0->outp[ABOVE]; VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); - break; - case ILI: - if (p) - { + break; + case ILI: + if (p) + { VERTEX(e0, ABOVE, GPC_LEFT, ix, iy); - N_EDGE(next_edge, e1, ABOVE, nx, iy); + N_EDGE(next_edge, e1, ABOVE, nx, iy); VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); - e1->outp[ABOVE]= e0->outp[ABOVE]; - e0->outp[ABOVE]= NULL; - } - break; - case IRI: - if (q) - { + e1->outp[ABOVE]= e0->outp[ABOVE]; + e0->outp[ABOVE]= NULL; + } + break; + case IRI: + if (q) + { VERTEX(e1, ABOVE, GPC_RIGHT, ix, iy); - P_EDGE(prev_edge, e0, ABOVE, px, iy); + P_EDGE(prev_edge, e0, ABOVE, px, iy); VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); - e0->outp[ABOVE]= e1->outp[ABOVE]; - e1->outp[ABOVE]= NULL; - } - break; - case IMX: - if (p && q) - { + e0->outp[ABOVE]= e1->outp[ABOVE]; + e1->outp[ABOVE]= NULL; + } + break; + case IMX: + if (p && q) + { VERTEX(e0, ABOVE, GPC_RIGHT, ix, iy); VERTEX(e1, ABOVE, GPC_LEFT, ix, iy); - e0->outp[ABOVE]= NULL; - e1->outp[ABOVE]= NULL; - P_EDGE(prev_edge, e0, ABOVE, px, iy); + e0->outp[ABOVE]= NULL; + e1->outp[ABOVE]= NULL; + P_EDGE(prev_edge, e0, ABOVE, px, iy); VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); - new_tristrip(&tlist, prev_edge, px, iy); - N_EDGE(next_edge, e1, ABOVE, nx, iy); + new_tristrip(&tlist, prev_edge, px, iy); + N_EDGE(next_edge, e1, ABOVE, nx, iy); VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); - next_edge->outp[ABOVE]= prev_edge->outp[ABOVE]; + next_edge->outp[ABOVE]= prev_edge->outp[ABOVE]; VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); - } - break; - case IMM: - if (p && q) - { + } + break; + case IMM: + if (p && q) + { VERTEX(e0, ABOVE, GPC_RIGHT, ix, iy); VERTEX(e1, ABOVE, GPC_LEFT, ix, iy); - P_EDGE(prev_edge, e0, ABOVE, px, iy); + P_EDGE(prev_edge, e0, ABOVE, px, iy); VERTEX(prev_edge, ABOVE, GPC_LEFT, px, iy); - new_tristrip(&tlist, prev_edge, px, iy); - N_EDGE(next_edge, e1, ABOVE, nx, iy); + new_tristrip(&tlist, prev_edge, px, iy); + N_EDGE(next_edge, e1, ABOVE, nx, iy); VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); - e1->outp[ABOVE]= prev_edge->outp[ABOVE]; + e1->outp[ABOVE]= prev_edge->outp[ABOVE]; VERTEX(e1, ABOVE, GPC_RIGHT, ix, iy); - new_tristrip(&tlist, e0, ix, iy); - next_edge->outp[ABOVE]= e0->outp[ABOVE]; + new_tristrip(&tlist, e0, ix, iy); + next_edge->outp[ABOVE]= e0->outp[ABOVE]; VERTEX(next_edge, ABOVE, GPC_RIGHT, nx, iy); - } - break; - case EMM: - if (p && q) - { + } + break; + case EMM: + if (p && q) + { VERTEX(e0, ABOVE, GPC_LEFT, ix, iy); - new_tristrip(&tlist, e1, ix, iy); - e0->outp[ABOVE]= e1->outp[ABOVE]; - } - break; - default: - break; - } /* End of switch */ + new_tristrip(&tlist, e1, ix, iy); + e0->outp[ABOVE]= e1->outp[ABOVE]; + } + break; + default: + break; + } /* End of switch */ } /* End of contributing intersection conditional */ - /* Swap bundle sides in response to edge crossing */ - if (e0->bundle[ABOVE][CLIP]) + /* Swap bundle sides in response to edge crossing */ + if (e0->bundle[ABOVE][CLIP]) e1->bside[CLIP]= !e1->bside[CLIP]; - if (e1->bundle[ABOVE][CLIP]) + if (e1->bundle[ABOVE][CLIP]) e0->bside[CLIP]= !e0->bside[CLIP]; - if (e0->bundle[ABOVE][SUBJ]) + if (e0->bundle[ABOVE][SUBJ]) e1->bside[SUBJ]= !e1->bside[SUBJ]; - if (e1->bundle[ABOVE][SUBJ]) + if (e1->bundle[ABOVE][SUBJ]) e0->bside[SUBJ]= !e0->bside[SUBJ]; - /* Swap e0 and e1 bundles in the AET */ - prev_edge= e0->prev; - next_edge= e1->next; - if (e1->next) - e1->next->prev= e0; + /* Swap e0 and e1 bundles in the AET */ + prev_edge= e0->prev; + next_edge= e1->next; + if (e1->next) + e1->next->prev= e0; - if (e0->bstate[ABOVE] == BUNDLE_HEAD) - { - search= TRUE; - while (search) - { - prev_edge= prev_edge->prev; - if (prev_edge) - { - if (prev_edge->bundle[ABOVE][CLIP] - || prev_edge->bundle[ABOVE][SUBJ] - || (prev_edge->bstate[ABOVE] == BUNDLE_HEAD)) - search= FALSE; - } - else - search= FALSE; - } - } - if (!prev_edge) - { - e1->next= aet; - aet= e0->next; - } - else - { - e1->next= prev_edge->next; - prev_edge->next= e0->next; - } - e0->next->prev= prev_edge; - e1->next->prev= e1; - e0->next= next_edge; - } /* End of IT loop*/ + if (e0->bstate[ABOVE] == BUNDLE_HEAD) + { + search= TRUE; + while (search) + { + prev_edge= prev_edge->prev; + if (prev_edge) + { + if (prev_edge->bundle[ABOVE][CLIP] + || prev_edge->bundle[ABOVE][SUBJ] + || (prev_edge->bstate[ABOVE] == BUNDLE_HEAD)) + search= FALSE; + } + else + search= FALSE; + } + } + if (!prev_edge) + { + e1->next= aet; + aet= e0->next; + } + else + { + e1->next= prev_edge->next; + prev_edge->next= e0->next; + } + e0->next->prev= prev_edge; + e1->next->prev= e1; + e0->next= next_edge; + } /* End of IT loop*/ - /* Prepare for next scanbeam */ - for (edge= aet; edge; edge= next_edge) - { - next_edge= edge->next; - succ_edge= edge->succ; + /* Prepare for next scanbeam */ + for (edge= aet; edge; edge= next_edge) + { + next_edge= edge->next; + succ_edge= edge->succ; - if ((edge->top.y == yt) && succ_edge) - { - /* Replace AET edge by its successor */ - succ_edge->outp[BELOW]= edge->outp[ABOVE]; - succ_edge->bstate[BELOW]= edge->bstate[ABOVE]; - succ_edge->bundle[BELOW][CLIP]= edge->bundle[ABOVE][CLIP]; - succ_edge->bundle[BELOW][SUBJ]= edge->bundle[ABOVE][SUBJ]; - prev_edge= edge->prev; - if (prev_edge) - prev_edge->next= succ_edge; - else - aet= succ_edge; - if (next_edge) - next_edge->prev= succ_edge; - succ_edge->prev= prev_edge; - succ_edge->next= next_edge; - } - else - { - /* Update this edge */ - edge->outp[BELOW]= edge->outp[ABOVE]; - edge->bstate[BELOW]= edge->bstate[ABOVE]; - edge->bundle[BELOW][CLIP]= edge->bundle[ABOVE][CLIP]; - edge->bundle[BELOW][SUBJ]= edge->bundle[ABOVE][SUBJ]; - edge->xb= edge->xt; - } - edge->outp[ABOVE]= NULL; - } - } + if ((edge->top.y == yt) && succ_edge) + { + /* Replace AET edge by its successor */ + succ_edge->outp[BELOW]= edge->outp[ABOVE]; + succ_edge->bstate[BELOW]= edge->bstate[ABOVE]; + succ_edge->bundle[BELOW][CLIP]= edge->bundle[ABOVE][CLIP]; + succ_edge->bundle[BELOW][SUBJ]= edge->bundle[ABOVE][SUBJ]; + prev_edge= edge->prev; + if (prev_edge) + prev_edge->next= succ_edge; + else + aet= succ_edge; + if (next_edge) + next_edge->prev= succ_edge; + succ_edge->prev= prev_edge; + succ_edge->next= next_edge; + } + else + { + /* Update this edge */ + edge->outp[BELOW]= edge->outp[ABOVE]; + edge->bstate[BELOW]= edge->bstate[ABOVE]; + edge->bundle[BELOW][CLIP]= edge->bundle[ABOVE][CLIP]; + edge->bundle[BELOW][SUBJ]= edge->bundle[ABOVE][SUBJ]; + edge->xb= edge->xt; + } + edge->outp[ABOVE]= NULL; + } + } } /* === END OF SCANBEAM PROCESSING ================================== */ /* Generate result tristrip from tlist */ @@ -2391,70 +2393,70 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, result->num_strips= count_tristrips(tlist); if (result->num_strips > 0) { - MALLOC(result->strip, result->num_strips * sizeof(gpc_vertex_list), - "tristrip list creation", gpc_vertex_list); + MALLOC(result->strip, result->num_strips * sizeof(gpc_vertex_list), + "tristrip list creation", gpc_vertex_list); - s= 0; - for (tn= tlist; tn; tn= tnn) - { - tnn= tn->next; + s= 0; + for (tn= tlist; tn; tn= tnn) + { + tnn= tn->next; - if (tn->active > 2) - { - /* Valid tristrip: copy the vertices and free the heap */ - result->strip[s].num_vertices= tn->active; - MALLOC(result->strip[s].vertex, tn->active * sizeof(gpc_vertex), - "tristrip creation", gpc_vertex); - v= 0; - if (INVERT_TRISTRIPS) - { + if (tn->active > 2) + { + /* Valid tristrip: copy the vertices and free the heap */ + result->strip[s].num_vertices= tn->active; + MALLOC(result->strip[s].vertex, tn->active * sizeof(gpc_vertex), + "tristrip creation", gpc_vertex); + v= 0; + if (INVERT_TRISTRIPS) + { lt= tn->v[GPC_RIGHT]; rt= tn->v[GPC_LEFT]; - } - else - { + } + else + { lt= tn->v[GPC_LEFT]; rt= tn->v[GPC_RIGHT]; - } - while (lt || rt) - { - if (lt) - { - ltn= lt->next; - result->strip[s].vertex[v].x= lt->x; - result->strip[s].vertex[v].y= lt->y; - v++; - FREE(lt); - lt= ltn; - } - if (rt) - { - rtn= rt->next; - result->strip[s].vertex[v].x= rt->x; - result->strip[s].vertex[v].y= rt->y; - v++; - FREE(rt); - rt= rtn; - } - } - s++; - } - else - { - /* Invalid tristrip: just free the heap */ + } + while (lt || rt) + { + if (lt) + { + ltn= lt->next; + result->strip[s].vertex[v].x= lt->x; + result->strip[s].vertex[v].y= lt->y; + v++; + FREE(lt); + lt= ltn; + } + if (rt) + { + rtn= rt->next; + result->strip[s].vertex[v].x= rt->x; + result->strip[s].vertex[v].y= rt->y; + v++; + FREE(rt); + rt= rtn; + } + } + s++; + } + else + { + /* Invalid tristrip: just free the heap */ for (lt= tn->v[GPC_LEFT]; lt; lt= ltn) - { - ltn= lt->next; - FREE(lt); - } + { + ltn= lt->next; + FREE(lt); + } for (rt= tn->v[GPC_RIGHT]; rt; rt=rtn) - { - rtn= rt->next; - FREE(rt); - } - } - FREE(tn); - } + { + rtn= rt->next; + FREE(rt); + } + } + FREE(tn); + } } /* Tidy up */ @@ -2467,6 +2469,8 @@ void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, /* =========================================================================== - End of file: gpc.c + End of file: gpc.c =========================================================================== */ + +#endif // GPC_CPP_H diff --git a/main.cpp b/main.cpp index 3c2ccd8..ea7aebb 100755 --- a/main.cpp +++ b/main.cpp @@ -83,7 +83,7 @@ void wifi() { int main(int argc, char** argv) { - wifi(); return 0; + //wifi(); return 0; #ifdef WITH_TESTS @@ -109,7 +109,7 @@ int main(int argc, char** argv) { //::testing::GTEST_FLAG(filter) = "*Matrix4*"; //::testing::GTEST_FLAG(filter) = "*Sphere3*"; - ::testing::GTEST_FLAG(filter) = "WiFiVAPGrouper*"; + ::testing::GTEST_FLAG(filter) = "NavMesh*"; //::testing::GTEST_FLAG(filter) = "Timestamp*"; //::testing::GTEST_FLAG(filter) = "*RayTrace3*"; diff --git a/navMesh/NavMesh.h b/navMesh/NavMesh.h new file mode 100644 index 0000000..1740bc0 --- /dev/null +++ b/navMesh/NavMesh.h @@ -0,0 +1,99 @@ +#ifndef NAV_MESH_H +#define NAV_MESH_H + +#include "NavMeshTriangle.h" +#include +#include "../geo/BBox3.h" +#include +#include "../math/DrawList.h" +#include "NavMeshRandom.h" + +template class NavMesh { + + /** all triangles within the mesh */ + std::vector triangles; + + BBox3 bbox; + +public: + + NavMesh() { + + } + + /** the overall bounding-box */ + const BBox3 getBBox() const { + return bbox; + } + + /** add a new triangle */ + void add(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) { + triangles.push_back(Tria(p1,p2,p3,type)); + bbox.add(p1); + bbox.add(p2); + bbox.add(p3); + } + + /** connect both triangles */ + void connectBiDir(int idx1, int idx2) { + connectUniDir(idx1,idx2); + connectUniDir(idx2,idx1); + } + + /** connect both triangles */ + void connectUniDir(int idxFrom, int idxTo) { + NavMeshTriangle& tria = triangles[idxFrom]; + tria._neighbors[tria._numNeighbors] = idxTo; + } + + /** allows for-each iteration over all included triangles */ + decltype(triangles.begin()) begin() {return triangles.begin();} + + /** allows for-each iteration over all included triangles */ + decltype(triangles.end()) end() {return triangles.end();} + + /** array access */ + Tria& operator [] (const size_t idx) { + Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds"); + return triangles[idx]; + } + + /** get the number of triangles used */ + size_t getNumTriangles() const { + return triangles.size(); + } + + /** ---------------- MISC ---------------- */ + + + NavMeshRandom getRandomizer() { + return NavMeshRandom(triangles); + } + + /** ---------------- NEIGHBORS ---------------- */ + + /** get the number of neighbors for the given element */ + int getNumNeighbors(const size_t idx) const { + return getNumNeighbors(triangles[idx]); + } + + /** get the number of neighbors for the given element */ + int getNumNeighbors(const Tria& e) const { + return e._numNeighbors; + } + + /** get the n-th neighbor for the given node */ + Tria& getNeighbor(const size_t nodeIdx, const size_t nth) const { + const Tria& node = triangles[nodeIdx]; + return getNeighbor(node, nth); + } + + /** get the n-th neighbor for the given node */ + Tria& getNeighbor(const Tria& tria, const size_t nth) const { + const Tria& neighbor = triangles[tria._neighbors[nth]]; + return (Tria&) neighbor; + } + +}; + +#endif diff --git a/navMesh/NavMeshDebug.h b/navMesh/NavMeshDebug.h new file mode 100644 index 0000000..85a4578 --- /dev/null +++ b/navMesh/NavMeshDebug.h @@ -0,0 +1,75 @@ +#ifndef NAVMESHDEBUG_H +#define NAVMESHDEBUG_H + +#include "NavMesh.h" + +#include +#include +#include +#include +#include + +class NavMeshDebug { + +public: + + template static void show(NavMesh& nm) { + + K::GnuplotFill gFill[3] = { + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#111111"), 1), + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaaa"), 1), + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#aaaaff"), 1) + }; + + K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600")); + + K::Gnuplot gp; + gp << "set view equal xy\n"; + + K::GnuplotSplot plot; + K::GnuplotSplotElementLines lines; plot.add(&lines); lines.setShowPoints(true); + K::GnuplotSplotElementPoints points; plot.add(&points); + + const BBox3 bbox = nm.getBBox(); + + points.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z)); + points.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z)); +// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(bbox.getMax().x, 0, 0)); +// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,bbox.getMax().y,0)); +// lines.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z)); + + //stairs in eigene group? vlt gehen dann auch die dellen weg? + + for (const Tria& tria : nm) { + uint8_t type = tria.type; + if (type < 0 || type > 2) { + throw std::runtime_error("out of type-bounds"); + } + K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke); + pol->add(K::GnuplotCoordinate3(tria.p1.x, tria.p1.y, tria.p1.z, K::GnuplotCoordinateSystem::FIRST)); + pol->add(K::GnuplotCoordinate3(tria.p2.x, tria.p2.y, tria.p2.z, K::GnuplotCoordinateSystem::FIRST)); + pol->add(K::GnuplotCoordinate3(tria.p3.x, tria.p3.y, tria.p3.z, K::GnuplotCoordinateSystem::FIRST)); + pol->close(); + pol->setZIndex(tria.p3.z); + plot.getObjects().add(pol); + + for (int i = 0; i < nm.getNumNeighbors(tria); ++i) { + const Tria& o = nm.getNeighbor(tria, i); + const Point3 p1 = tria.getCenter(); + const Point3 p2 = o.getCenter(); + //lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1)); + } + + } + + plot.getObjects().reOrderByZIndex(); + + gp.draw(plot); + gp.flush(); + sleep(1); + + } + +}; + +#endif // NAVMESHDEBUG_H diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h new file mode 100644 index 0000000..1899a33 --- /dev/null +++ b/navMesh/NavMeshFactory.h @@ -0,0 +1,573 @@ +#ifndef NAV_MESH_FACTORY_H +#define NAV_MESH_FACTORY_H + +#include "../floorplan/v2/Floorplan.h" +#include "../floorplan/v2/FloorplanHelper.h" + +#include "NavMesh.h" +#include "NavMeshPoly.h" +#include "NavMeshTriangle.h" + +#include "../lib/Recast/Recast.h" + +enum SamplePartitionType { + SAMPLE_PARTITION_WATERSHED, + SAMPLE_PARTITION_MONOTONE, + SAMPLE_PARTITION_LAYERS, +}; + +struct TriangleIn { + Point3 p1; + Point3 p2; + Point3 p3; + uint8_t type; + TriangleIn(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : p1(p1), p2(p2), p3(p3), type(type) {;} +}; + +struct TriangleOut { + + Point3 p1; + Point3 p2; + Point3 p3; + + int numNeighbors = 0; + int neighbors[3]; // each triangle has max 3 neighbors + + TriangleOut(const Point3 p1, const Point3 p2, const Point3 p3) : p1(p1), p2(p2), p3(p3), neighbors() {;} + + Point3 center() const { + return (p1+p2+p3) / 3; + } + +}; + +template class NavMeshFactory { + +private: + + NavMesh* dst = nullptr; + + std::vector triangles; + +public: + + NavMeshFactory(NavMesh* dst) : dst(dst) { + + } + + void build(Floorplan::IndoorMap* map) { + const BBox3 bbox = FloorplanHelper::getBBox(map); + for (const Floorplan::Floor* floor : map->floors) { + add(floor); + } + fire(bbox); + } + +private: + + /** add one floor */ + void add(const Floorplan::Floor* floor) { + + NavMeshPoly nmPoly(floor->atHeight); + + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + if (poly->method == Floorplan::OutlineMethod::ADD) { + nmPoly.add(poly->poly); + } + } + + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + if (poly->method == Floorplan::OutlineMethod::REMOVE) { + nmPoly.remove(poly->poly); + } + } + + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + if (line != nullptr) { + nmPoly.remove(getPolygon(line)); + } + } + + std::vector> tmp = nmPoly.get(); + for (const std::vector& tria : tmp) { + const TriangleIn t(tria[0], tria[1], tria[2], 1); // TODO outdoor + triangles.push_back(t); + } + + // add all stairs + for (const Floorplan::Stair* stair : floor->stairs) { + const std::vector quads = Floorplan::getQuads(stair->getParts(), floor); + for (const Floorplan::Quad3& quad : quads) { + const TriangleIn t1(quad.p1, quad.p2, quad.p3, 2); // TODO type + const TriangleIn t2(quad.p1, quad.p3, quad.p4, 2); + triangles.push_back(t1); + triangles.push_back(t2); + } + } + + } + + bool fire(BBox3 bbox) { + + std::vector tData; + std::vector vData; + std::vector typeData; + + // floor outlines + for (const TriangleIn& t : triangles) { + + // swap YZ and polygon order + int startVert = vData.size() / 3; + + // invert triangle ? (CW vs CCW) + // ensure normal points UP + const Point3 norm = cross((t.p2-t.p1), (t.p3-t.p1)); + if (norm.z > 0) { + tData.push_back(startVert + 0); + tData.push_back(startVert + 2); + tData.push_back(startVert + 1); + } else { + tData.push_back(startVert + 0); + tData.push_back(startVert + 1); + tData.push_back(startVert + 2); + } + + typeData.push_back(t.type); + + vData.push_back(t.p1.x); + vData.push_back(t.p1.z); + vData.push_back(t.p1.y); + + vData.push_back(t.p2.x); + vData.push_back(t.p2.z); + vData.push_back(t.p2.y); + + vData.push_back(t.p3.x); + vData.push_back(t.p3.z); + vData.push_back(t.p3.y); + + } + + unsigned char* m_triareas = typeData.data(); + const float* verts = vData.data(); + const int* tris = tData.data(); + + int ntris = tData.size() / 3; + int nverts = vData.size() / 3; + + + //unsigned char* m_triareas; + rcHeightfield* m_solid; + rcCompactHeightfield* m_chf; + rcContourSet* m_cset; + rcPolyMesh* m_pmesh; + rcConfig m_cfg; + rcPolyMeshDetail* m_dmesh; + rcContext* m_ctx = new rcContext(); + + float m_cellSize = 0.1f; //0.3f; // needed for 20cm walls to work! + float m_cellHeight = 0.1f; //0.2f; + float m_agentHeight = 2.0f; + float m_agentRadius = 0.1f;//0.6f; + float m_agentMaxClimb = 0.5f; // 0.9f; + float m_agentMaxSlope = 45.0f; + float m_regionMinSize = 2;//8; + float m_regionMergeSize = 20; + float m_edgeMaxLen = 10.0f; // maximal size for one triangle. too high = too many samples when walking! + float m_edgeMaxError = 1.0f; //1.3f; + float m_vertsPerPoly = 3;//6.0f; + float m_detailSampleDist = 6.0f; + float m_detailSampleMaxError = 1.0f;//1.0f; + int m_partitionType = SAMPLE_PARTITION_WATERSHED; + + + // Init build configuration from GUI + memset(&m_cfg, 0, sizeof(m_cfg)); + m_cfg.cs = m_cellSize; + m_cfg.ch = m_cellHeight; + m_cfg.walkableSlopeAngle = m_agentMaxSlope; + m_cfg.walkableHeight = (int)ceilf(m_agentHeight / m_cfg.ch); + m_cfg.walkableClimb = (int)floorf(m_agentMaxClimb / m_cfg.ch); + m_cfg.walkableRadius = (int)ceilf(m_agentRadius / m_cfg.cs); + m_cfg.maxEdgeLen = (int)(m_edgeMaxLen / m_cellSize); + m_cfg.maxSimplificationError = m_edgeMaxError; + m_cfg.minRegionArea = (int)rcSqr(m_regionMinSize); // Note: area = size*size + m_cfg.mergeRegionArea = (int)rcSqr(m_regionMergeSize); // Note: area = size*size + m_cfg.maxVertsPerPoly = (int)m_vertsPerPoly; + m_cfg.detailSampleDist = m_detailSampleDist < 0.9f ? 0 : m_cellSize * m_detailSampleDist; + m_cfg.detailSampleMaxError = m_cellHeight * m_detailSampleMaxError; + + float bmin[3] = {bbox.getMin().x, bbox.getMin().z, bbox.getMin().y}; + float bmax[3] = {bbox.getMax().x, bbox.getMax().z, bbox.getMax().y};// x/z swapped? + + // Set the area where the navigation will be build. + // Here the bounds of the input mesh are used, but the + // area could be specified by an user defined box, etc. + rcVcopy(m_cfg.bmin, bmin); + rcVcopy(m_cfg.bmax, bmax); + rcCalcGridSize(m_cfg.bmin, m_cfg.bmax, m_cfg.cs, &m_cfg.width, &m_cfg.height); + + // Reset build times gathering. + m_ctx->resetTimers(); + + // Start the build process. + m_ctx->startTimer(RC_TIMER_TOTAL); + + m_ctx->log(RC_LOG_PROGRESS, "Building navigation:"); + m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height); + m_ctx->log(RC_LOG_PROGRESS, " - %.1fK verts, %.1fK tris", nverts/1000.0f, ntris/1000.0f); + + // + // Step 2. Rasterize input polygon soup. + // + + // Allocate voxel heightfield where we rasterize our input data to. + m_solid = rcAllocHeightfield(); + if (!m_solid) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'."); + return false; + } + if (!rcCreateHeightfield(m_ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield."); + return false; + } + + // Allocate array that can hold triangle area types. + // If you have multiple meshes you need to process, allocate + // and array which can hold the max number of triangles you need to process. +// m_triareas = new unsigned char[ntris]; +// if (!m_triareas) +// { +// m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'm_triareas' (%d).", ntris); +// return false; +// } + + // Find triangles which are walkable based on their slope and rasterize them. + // If your input data is multiple meshes, you can transform them here, calculate + // the are type for each of the meshes and rasterize them. + //memset(m_triareas, 0, ntris*sizeof(unsigned char)); + //rcMarkWalkableTriangles(m_ctx, m_cfg.walkableSlopeAngle, verts, nverts, tris, ntris, m_triareas); + if (!rcRasterizeTriangles(m_ctx, verts, nverts, tris, m_triareas, ntris, *m_solid, m_cfg.walkableClimb)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not rasterize triangles."); + return false; + } + + bool m_keepInterResults = false; + bool m_filterLowHangingObstacles = false; + bool m_filterLedgeSpans = false; + bool m_filterWalkableLowHeightSpans = false; + + // std::vector! +// if (!m_keepInterResults) +// { +// delete [] m_triareas; +// m_triareas = 0; +// } + + // + // Step 3. Filter walkables surfaces. + // + + + + // Once all geoemtry is rasterized, we do initial pass of filtering to + // remove unwanted overhangs caused by the conservative rasterization + // as well as filter spans where the character cannot possibly stand. + if (m_filterLowHangingObstacles) + rcFilterLowHangingWalkableObstacles(m_ctx, m_cfg.walkableClimb, *m_solid); + if (m_filterLedgeSpans) + rcFilterLedgeSpans(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid); + if (m_filterWalkableLowHeightSpans) + rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid); + + + // + // Step 4. Partition walkable surface to simple regions. + // + + // Compact the heightfield so that it is faster to handle from now on. + // This will result more cache coherent data as well as the neighbours + // between walkable cells will be calculated. + m_chf = rcAllocCompactHeightfield(); + if (!m_chf) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'."); + return false; + } + if (!rcBuildCompactHeightfield(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data."); + return false; + } + + if (!m_keepInterResults) + { + rcFreeHeightField(m_solid); + m_solid = 0; + } + + // Erode the walkable area by agent radius. + if (!rcErodeWalkableArea(m_ctx, m_cfg.walkableRadius, *m_chf)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode."); + return false; + } + + // (Optional) Mark areas. + // const ConvexVolume* vols = m_geom->getConvexVolumes(); + // for (int i = 0; i < m_geom->getConvexVolumeCount(); ++i) + // rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf); + + + // Partition the heightfield so that we can use simple algorithm later to triangulate the walkable areas. + // There are 3 martitioning methods, each with some pros and cons: + // 1) Watershed partitioning + // - the classic Recast partitioning + // - creates the nicest tessellation + // - usually slowest + // - partitions the heightfield into nice regions without holes or overlaps + // - the are some corner cases where this method creates produces holes and overlaps + // - holes may appear when a small obstacles is close to large open area (triangulation can handle this) + // - overlaps may occur if you have narrow spiral corridors (i.e stairs), this make triangulation to fail + // * generally the best choice if you precompute the nacmesh, use this if you have large open areas + // 2) Monotone partioning + // - fastest + // - partitions the heightfield into regions without holes and overlaps (guaranteed) + // - creates long thin polygons, which sometimes causes paths with detours + // * use this if you want fast navmesh generation + // 3) Layer partitoining + // - quite fast + // - partitions the heighfield into non-overlapping regions + // - relies on the triangulation code to cope with holes (thus slower than monotone partitioning) + // - produces better triangles than monotone partitioning + // - does not have the corner cases of watershed partitioning + // - can be slow and create a bit ugly tessellation (still better than monotone) + // if you have large open areas with small obstacles (not a problem if you use tiles) + // * good choice to use for tiled navmesh with medium and small sized tiles + + if (m_partitionType == SAMPLE_PARTITION_WATERSHED) + { + // Prepare for region partitioning, by calculating distance field along the walkable surface. + if (!rcBuildDistanceField(m_ctx, *m_chf)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field."); + return false; + } + + // Partition the walkable surface into simple regions without holes. + if (!rcBuildRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build watershed regions."); + return false; + } + } + else if (m_partitionType == SAMPLE_PARTITION_MONOTONE) + { + // Partition the walkable surface into simple regions without holes. + // Monotone partitioning does not need distancefield. + if (!rcBuildRegionsMonotone(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build monotone regions."); + return false; + } + } + else // SAMPLE_PARTITION_LAYERS + { + // Partition the walkable surface into simple regions without holes. + if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions."); + return false; + } + } + + // + // Step 5. Trace and simplify region contours. + // + + // Create contours. + m_cset = rcAllocContourSet(); + if (!m_cset) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'."); + return false; + } + if (!rcBuildContours(m_ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours."); + return false; + } + + // + // Step 6. Build polygons mesh from contours. + // + + // Build polygon navmesh from the contours. + m_pmesh = rcAllocPolyMesh(); + if (!m_pmesh) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'."); + return false; + } + if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours."); + return false; + } + + // + // Step 7. Create detail mesh which allows to access approximate height on each polygon. + // + + m_dmesh = rcAllocPolyMeshDetail(); + if (!m_dmesh) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'."); + return false; + } + + if (!rcBuildPolyMeshDetail(m_ctx, *m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh)) + { + m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh."); + return false; + } + + if (!m_keepInterResults) + { + rcFreeCompactHeightfield(m_chf); + m_chf = 0; + rcFreeContourSet(m_cset); + m_cset = 0; + } + + + std::vector res; + + const float* orig = m_pmesh->bmin; + + // https://github.com/recastnavigation/recastnavigation/blob/master/Docs/Extern/Recast_api.txt + for (int i = 0; i < m_pmesh->npolys; ++i) { + + const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2]; + + const uint8_t type = m_pmesh->areas[i]; + +// Each entry is 2 * #nvp in length. The first half of the entry +// contains the indices of the polygon. The first instance of #RC_MESH_NULL_IDX +// indicates the end of the indices for the entry. The second half contains +// indices to neighbor polygons. A value of #RC_MESH_NULL_IDX indicates no +// connection for the associated edge. (I.e. The edge is a solid border.) + +// we only use exactly 3 vertices per polygon, no iteration needed + +// for (int j = 0; j < m_pmesh->nvp; ++j) { +// if (p[j] == RC_MESH_NULL_IDX) {break;} + +// const unsigned short* v = &m_pmesh->verts[p[j]*3]; +// const float x = orig[0] + v[0]*m_pmesh->cs; +// const float z = orig[1] + v[1]*m_pmesh->ch; +// const float y = orig[2] + v[2]*m_pmesh->cs; + +// pol->add(K::GnuplotCoordinate3(x, y, z, K::GnuplotCoordinateSystem::FIRST)); + +// } + + // un-swap Y/Z + const unsigned short* v0 = &m_pmesh->verts[p[0]*3]; + const Point3 p0(orig[0] + v0[0]*m_pmesh->cs, orig[2] + v0[2]*m_pmesh->cs, orig[1] + v0[1]*m_pmesh->ch); + + const unsigned short* v1 = &m_pmesh->verts[p[1]*3]; + const Point3 p1(orig[0] + v1[0]*m_pmesh->cs, orig[2] + v1[2]*m_pmesh->cs, orig[1] + v1[1]*m_pmesh->ch); + + const unsigned short* v2 = &m_pmesh->verts[p[2]*3]; + const Point3 p2(orig[0] + v2[0]*m_pmesh->cs, orig[2] + v2[2]*m_pmesh->cs, orig[1] + v2[1]*m_pmesh->ch); + + dst->add(p0,p1,p2,type); + + } + + // now, connect neighbors + for (int i = 0; i < m_pmesh->npolys; ++i) { + + const unsigned short* p = &m_pmesh->polys[i*m_pmesh->nvp*2]; + + // find all neighbor polygons using their index + for (int j = 0; j < m_pmesh->nvp; ++j) { + int jj = j + m_pmesh->nvp; // offset, 2nd half of the array [size: 2*nvp] + if (p[jj] == RC_MESH_NULL_IDX) {continue;} // no neighbor for the current edge! + const int idx = p[jj]; + dst->connectUniDir(i, idx); + } + + } + + return true; + + } + + +// void dump() { + +// std::ofstream out("/tmp/1.dat"); +// for (const std::vector tria : mesh.get(0)) { +// for (int i = 0; i < 4; ++i) { +// const Point3 p = tria[i%3]; +// out << p.x << " " << p.y << " " << p.z << "\r\n"; +// } +// out << "\r\n"; +// out << "\r\n"; +// } +// out.close(); + +// K::Gnuplot gp; +// gp << "set view equal xyz\n"; + +// K::GnuplotSplot plot; +// K::GnuplotSplotElementLines lines; plot.add(&lines); +// lines.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(20,0,0)); +// lines.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(0,20,0)); + + +// for (const std::vector tria : mesh.get(0)) { +// K::GnuplotFill gFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#888888"), 1); +// K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#000000")); +// K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill, gStroke); +// for (const Point3 p : tria) { +// K::GnuplotCoordinate3 coord(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST); +// pol->add(coord); +// } +// pol->close(); +// plot.getObjects().add(pol); +// } + +// gp.draw(plot); +// gp.flush(); +// sleep(1000); + +// } + + /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ + static Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) { + //const Line2 base(line->from*100, line->to*100); + const float thickness_m = line->thickness_m; + const Point2 dir = (line->to - line->from); // obstacle's direction + const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree) + const Point2 p1 = line->from + perp * thickness_m/2; // start-up + const Point2 p2 = line->from - perp * thickness_m/2; // start-down + const Point2 p3 = line->to + perp * thickness_m/2; // end-up + const Point2 p4 = line->to - perp * thickness_m/2; // end-down + Floorplan::Polygon2 res; + res.points.push_back(p1); + res.points.push_back(p2); + res.points.push_back(p4); + res.points.push_back(p3); + return res; + } + +}; + +#endif diff --git a/navMesh/NavMeshPoly.h b/navMesh/NavMeshPoly.h new file mode 100644 index 0000000..014338a --- /dev/null +++ b/navMesh/NavMeshPoly.h @@ -0,0 +1,123 @@ +#ifndef POLYGON_H +#define POLYGON_H + +#include +#include "../lib/gpc/gpc.cpp.h" + +class NavMeshPoly { + + struct GPCPolygon : gpc_polygon { + GPCPolygon() { +// contour = (gpc_vertex_list*) calloc(0, 1024); +// contour->num_vertices = 0; +// contour->vertex = (gpc_vertex*) calloc(0, 1024); +// hole = (int*) calloc(0, 1024); + num_contours = 0; + contour = nullptr; + hole = nullptr; + } + ~GPCPolygon() { + if (contour) { + gpc_free_polygon(this); + //free(contour->vertex); contour->vertex = nullptr; + } + free(contour); contour = nullptr; + free(hole); hole = nullptr; + + } + GPCPolygon& operator = (const GPCPolygon& o) = delete; + GPCPolygon& operator = (GPCPolygon& o) { + this->contour = o.contour; + this->hole = o.hole; + this->num_contours = o.num_contours; + o.contour = nullptr; + o.hole = nullptr; + return *this; + } + }; + +private: + + GPCPolygon state; + float z; + +public: + + NavMeshPoly(float z) : z(z) { + ; + } + + void add(const Floorplan::Polygon2& poly) { + GPCPolygon cur = toGPC(poly); + //GPCPolygon out; + gpc_polygon_clip(GPC_UNION, &state, &cur, &state); + //state = out; + } + + void remove(const Floorplan::Polygon2& poly) { + GPCPolygon cur = toGPC(poly); + //GPCPolygon out; + gpc_polygon_clip(GPC_DIFF, &state, &cur, &state); + //state = out; + } + + std::vector> get() { + + gpc_tristrip res; + res.num_strips = 0; + res.strip = nullptr; + + //res.strip = (gpc_vertex_list*) malloc(1024); + gpc_polygon_to_tristrip(&state, &res); + + std::vector> trias; + + for (int i = 0; i < res.num_strips; ++i) { + gpc_vertex_list lst = res.strip[i]; +// for (int j = 0; j < lst.num_vertices; ++j) { +// gpc_vertex& vert = lst.vertex[j]; +// Point3 p3(vert.x, vert.y, z); +// tria.push_back(p3); +// } + for (int j = 2; j < lst.num_vertices; ++j) { + std::vector tria; + gpc_vertex& v1 = lst.vertex[j-2]; + gpc_vertex& v2 = lst.vertex[j-1]; + gpc_vertex& v3 = lst.vertex[j]; + tria.push_back(Point3(v1.x, v1.y, z)); + tria.push_back(Point3(v2.x, v2.y, z)); + tria.push_back(Point3(v3.x, v3.y, z)); + trias.push_back(tria); + } + + } + + gpc_free_tristrip(&res); + + return std::move(trias); + + } + +private: + + GPCPolygon toGPC(Floorplan::Polygon2 poly) { + + std::vector verts; + for (Point2 p2 : poly.points) { + gpc_vertex vert; vert.x = p2.x; vert.y = p2.y; + verts.push_back(vert); + } + + GPCPolygon gpol; + gpc_vertex_list list; + list.num_vertices = verts.size(); + list.vertex = verts.data(); + gpc_add_contour(&gpol, &list, 0); + + return gpol; + + } + +}; + +#endif // POLYGON_H diff --git a/navMesh/NavMeshRandom.h b/navMesh/NavMeshRandom.h new file mode 100644 index 0000000..2fcd98a --- /dev/null +++ b/navMesh/NavMeshRandom.h @@ -0,0 +1,49 @@ +#ifndef NAVMESHRANDOM_H +#define NAVMESHRANDOM_H + +#include +#include +#include "../math/DrawList.h" +#include "../geo/Point3.h" + +template class NavMeshRandom { + + std::minstd_rand gen; + std::uniform_real_distribution dOnTriangle = std::uniform_real_distribution(0.0f, 1.0f); + const std::vector& triangles; + DrawList lst; + +public: + + struct Result { + Point3 pos; + size_t triaIdx; + Result(const Point3 pos, const size_t triaIdx) : pos(pos), triaIdx(triaIdx) {;} + }; + + /** ctor */ + NavMeshRandom(const std::vector& triangles) : triangles(triangles) { + for (size_t idx = 0; idx < triangles.size(); ++idx) { + lst.add(idx, triangles[idx].getArea()); + } + } + + /** draw a random point within the map */ + Result draw() { + + const size_t idx = lst.get(); + const Tria& tria = triangles[idx]; + + while (true) { + const float u = dOnTriangle(gen); + const float v = dOnTriangle(gen); + if (u+v > 1) {continue;} + const Point3 pos = tria.getA() + (tria.getAB() * u) + (tria.getAC() * v); + return Result(pos, idx); + } + + } + +}; + +#endif // NAVMESHRANDOM_H diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h new file mode 100644 index 0000000..6397ac3 --- /dev/null +++ b/navMesh/NavMeshTriangle.h @@ -0,0 +1,126 @@ +#ifndef NAVMESHTRIANGLE_H +#define NAVMESHTRIANGLE_H + +#include "../geo/Point3.h" +#include "../geo/Point2.h" + +class NavMeshTriangle { + +public: + + Point3 p1; + Point3 p2; + Point3 p3; + uint8_t type; + +private: + + template friend class NavMesh; + + int _neighbors[3]; + int _numNeighbors; + + /** precalculated stuff */ + +private: + + Point2 v0; + Point2 v1; + float dot00; + float dot01; + float dot11; + float invDenom; + float area; + + const Point3 center; + const Point3 v12; + const Point3 v13; + +public: + + /** ctor */ + NavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) : + p1(p1), p2(p2), p3(p3), type(type), + _neighbors(), _numNeighbors(0), + center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1) { + precompute(); + } + + bool operator == (const NavMeshTriangle& o) const { + return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3); + } + + + Point3 getA() const { + return p1; + } + + Point3 getAB() const { + return v12; + } + + Point3 getAC() const { + return v13; + } + + bool contains(const Point3 p) const { + + const Point2 v2 = p.xy() - p1.xy(); + + // Compute dot products + float dot02 = dot(v0, v2); + float dot12 = dot(v1, v2); + + // Compute barycentric coordinates + float u = (dot11 * dot02 - dot01 * dot12) * invDenom; + float v = (dot00 * dot12 - dot01 * dot02) * invDenom; + + // Check if point is in triangle + return (u >= 0) && (v >= 0) && (u + v <= 1); + + } + + /** get the triangle's size */ + float getArea() const { + return area; + } + + /** get the triangle's center-point */ + Point3 getCenter() const { + return center; + } + + + +private: + + /** perform some pre-calculations to speed things up */ + void precompute() { + + // Compute vectors + v0 = p3.xy() - p1.xy(); + v1 = p2.xy() - p1.xy(); + + // Compute dot products + dot00 = dot(v0, v0); + dot01 = dot(v0, v1); + dot11 = dot(v1, v1); + + // Compute barycentric coordinates + invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + + + + + const float a = (p2-p1).length(); + const float b = (p3-p1).length(); + const float c = (p2-p3).length(); + const float s = 0.5f * (a+b+c); + area = std::sqrt( s * (s-a) * (s-b) * (s-c) ); + + } + + +}; + +#endif // NAVMESHTRIANGLE_H diff --git a/navMesh/walk/NavMeshWalkSimple.h b/navMesh/walk/NavMeshWalkSimple.h new file mode 100644 index 0000000..2f197ec --- /dev/null +++ b/navMesh/walk/NavMeshWalkSimple.h @@ -0,0 +1,43 @@ +#ifndef NAVMESHWALKSIMPLE_H +#define NAVMESHWALKSIMPLE_H + +#include "../NavMesh.h" + +template class NavMeshWalkSimpel { + +private: + + const NavMesh& mesh; + +public: + + struct Location { + size_t idx; + Point3 pos; + }; + + struct Result { + Location loc; + }; + + struct Params { + Location loc; + float distance_m; + float heading_rad; + }; + +public: + + /** ctor */ + NavMeshWalkSimpel(const NavMesh& mesh) : mesh(mesh) { + + } + + Result walk(const Params& params) { + + } + + +} + +#endif // NAVMESHWALKSIMPLE_H diff --git a/sensors/imu/MagnetometerData.h b/sensors/imu/MagnetometerData.h index 24136c7..847ee49 100644 --- a/sensors/imu/MagnetometerData.h +++ b/sensors/imu/MagnetometerData.h @@ -73,7 +73,7 @@ private: }; namespace std { - MagnetometerData sqrt(const MagnetometerData& o) { + inline MagnetometerData sqrt(const MagnetometerData& o) { return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z)); } } diff --git a/tests/math/filter/TestButter.cpp b/tests/math/filter/TestButter.cpp index 1946e1a..cf34153 100644 --- a/tests/math/filter/TestButter.cpp +++ b/tests/math/filter/TestButter.cpp @@ -5,7 +5,7 @@ #include "../../../misc/Time.h" #include "../../../math/Interpolator.h" -#include"../../../sensors/pressure/ActivityButterPressure.h" +#include"../../../sensors/activity/ActivityButterPressure.h" #include #include @@ -23,234 +23,234 @@ TEST(Butterworth, offlineSinus) { - //input data - std::minstd_rand gen; - std::uniform_real_distribution noise (-0.1, +0.1); + //input data + std::minstd_rand gen; + std::uniform_real_distribution noise (-0.1, +0.1); - int size = 1100; //Fs - double* input = new double[size]; - double* output = new double[size]; + int size = 1100; //Fs + double* input = new double[size]; + double* output = new double[size]; - // 17.5hz sin signal with random noise [-0.1, 0.1] - for( int i=0; i < size; ++i ){ - input[i] = sin(0.1 * i) + noise(gen); - } + // 17.5hz sin signal with random noise [-0.1, 0.1] + for( int i=0; i < size; ++i ){ + input[i] = sin(0.1 * i) + noise(gen); + } - //butterworth - Filter::ButterworthLP butter(size,20,5); - butter.stepInitialization(0); - butter.filter(input, output, size, 0, true); + //butterworth + Filter::ButterworthLP butter(size,20,5); + butter.stepInitialization(0); + butter.filter(input, output, size, 0, true); - K::Gnuplot gp; - K::GnuplotPlot plot; - K::GnuplotPlotElementLines linesInput; - K::GnuplotPlotElementLines linesOutput; + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines linesInput; + K::GnuplotPlotElementLines linesOutput; - for(int i=0; i < size-1; ++i){ + for(int i=0; i < size-1; ++i){ - K::GnuplotPoint2 input_p1(i, input[i]); - K::GnuplotPoint2 input_p2(i+1, input[i+1]); + K::GnuplotPoint2 input_p1(i, input[i]); + K::GnuplotPoint2 input_p2(i+1, input[i+1]); - K::GnuplotPoint2 output_p1(i, output[i]); - K::GnuplotPoint2 output_p2(i+1, output[i+1]); + K::GnuplotPoint2 output_p1(i, output[i]); + K::GnuplotPoint2 output_p2(i+1, output[i+1]); - linesInput.addSegment(input_p1, input_p2); - linesOutput.addSegment(output_p1, output_p2); - } + linesInput.addSegment(input_p1, input_p2); + linesOutput.addSegment(output_p1, output_p2); + } linesOutput.getStroke().getColor().setHexStr("#00FF00"); - plot.add(&linesInput); - plot.add(&linesOutput); + plot.add(&linesInput); + plot.add(&linesOutput); - gp.draw(plot); - gp.flush(); + gp.draw(plot); + gp.flush(); - sleep(10); + sleep(10); } TEST(Butterworth, onlineSinus) { - int size = 1100; //Fs - double* input = new double[size]; - double* output = new double[size]; + int size = 1100; //Fs + double* input = new double[size]; + double* output = new double[size]; - Filter::ButterworthLP butter(size,20,5); - butter.stepInitialization(0); + Filter::ButterworthLP butter(size,20,5); + butter.stepInitialization(0); - //input data - std::minstd_rand gen; - std::uniform_real_distribution noise (-0.1, +0.1); + //input data + std::minstd_rand gen; + std::uniform_real_distribution noise (-0.1, +0.1); - // 17.5hz sin signal with random noise [-0.1, 0.1] - for( int i=0; i < size; ++i ){ - input[i] = sin(0.1 * i) + noise(gen); + // 17.5hz sin signal with random noise [-0.1, 0.1] + for( int i=0; i < size; ++i ){ + input[i] = sin(0.1 * i) + noise(gen); - output[i] = butter.process(input[i]); - } + output[i] = butter.process(input[i]); + } - K::Gnuplot gp; - K::GnuplotPlot plot; - K::GnuplotPlotElementLines linesInput; - K::GnuplotPlotElementLines linesOutput; + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines linesInput; + K::GnuplotPlotElementLines linesOutput; - for(int i=0; i < size-1; ++i){ + for(int i=0; i < size-1; ++i){ - K::GnuplotPoint2 input_p1(i, input[i]); - K::GnuplotPoint2 input_p2(i+1, input[i+1]); + K::GnuplotPoint2 input_p1(i, input[i]); + K::GnuplotPoint2 input_p2(i+1, input[i+1]); - K::GnuplotPoint2 output_p1(i, output[i]); - K::GnuplotPoint2 output_p2(i+1, output[i+1]); + K::GnuplotPoint2 output_p1(i, output[i]); + K::GnuplotPoint2 output_p2(i+1, output[i+1]); - linesInput.addSegment(input_p1, input_p2); - linesOutput.addSegment(output_p1, output_p2); - } + linesInput.addSegment(input_p1, input_p2); + linesOutput.addSegment(output_p1, output_p2); + } linesOutput.getStroke().getColor().setHexStr("#00FF00"); - plot.add(&linesInput); - plot.add(&linesOutput); + plot.add(&linesInput); + plot.add(&linesOutput); - gp.draw(plot); - gp.flush(); + gp.draw(plot); + gp.flush(); - sleep(1); + sleep(1); } TEST(Butterworth, offlineOctaveBaro) { - double* input = new double[100000]; - double* output = new double[100000]; + double* input = new double[100000]; + double* output = new double[100000]; - Interpolator interp; + Interpolator interp; - //read file - std::string line; - std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat"); - std::ifstream infile(filename); + //read file + std::string line; + std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat"); + std::ifstream infile(filename); - int counter = 0; - while (std::getline(infile, line)) - { - std::istringstream iss(line); - int ts; - double value; + int counter = 0; + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; - while (iss >> ts >> value) { + while (iss >> ts >> value) { - interp.add(ts, value); + interp.add(ts, value); - while(interp.getMaxKey() > counter*20 ){ - double interpValue = interp.get(counter*20); + while(interp.getMaxKey() > counter*20 ){ + double interpValue = interp.get(counter*20); - input[counter] = interpValue; - //std::cout << counter*20 << " " << interpValue << " i" << std::endl; - ++counter; - } + input[counter] = interpValue; + //std::cout << counter*20 << " " << interpValue << " i" << std::endl; + ++counter; + } - //std::cout << ts << " " << value << " r" << std::endl; + //std::cout << ts << " " << value << " r" << std::endl; - } - } + } + } - Filter::ButterworthLP butter(50,0.2,2); - butter.filter(input, output, counter, 938.15, true); + Filter::ButterworthLP butter(50,0.2,2); + butter.filter(input, output, counter, 938.15, true); - K::Gnuplot gp; - K::GnuplotPlot plot; - K::GnuplotPlotElementLines linesInput; - K::GnuplotPlotElementLines linesOutput; + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines linesInput; + K::GnuplotPlotElementLines linesOutput; - for(int i=0; i < counter-1; ++i){ + for(int i=0; i < counter-1; ++i){ - K::GnuplotPoint2 input_p1(i, input[i]); - K::GnuplotPoint2 input_p2(i+1, input[i+1]); + K::GnuplotPoint2 input_p1(i, input[i]); + K::GnuplotPoint2 input_p2(i+1, input[i+1]); - K::GnuplotPoint2 output_p1(i, output[i]); - K::GnuplotPoint2 output_p2(i+1, output[i+1]); + K::GnuplotPoint2 output_p1(i, output[i]); + K::GnuplotPoint2 output_p2(i+1, output[i+1]); - linesInput.addSegment(input_p1, input_p2); - linesOutput.addSegment(output_p1, output_p2); - } + linesInput.addSegment(input_p1, input_p2); + linesOutput.addSegment(output_p1, output_p2); + } linesOutput.getStroke().getColor().setHexStr("#00FF00"); - plot.add(&linesInput); - plot.add(&linesOutput); + plot.add(&linesInput); + plot.add(&linesOutput); - gp.draw(plot); - gp.flush(); + gp.draw(plot); + gp.flush(); - sleep(1); + sleep(1); } TEST(Butterworth, onlineOctaveBaro) { - std::vector input; - std::vector output; + std::vector input; + std::vector output; - Interpolator interp; + Interpolator interp; - Filter::ButterworthLP butter(50,0.02,2); - butter.stepInitialization(938.15); + Filter::ButterworthLP butter(50,0.02,2); + butter.stepInitialization(938.15); - //read file - std::string line; - std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat"); - std::ifstream infile(filename); + //read file + std::string line; + std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat"); + std::ifstream infile(filename); - int counter = 1; - while (std::getline(infile, line)) - { - std::istringstream iss(line); - int ts; - double value; + int counter = 1; + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; - while (iss >> ts >> value) { + while (iss >> ts >> value) { - interp.add(ts, value); + interp.add(ts, value); - while(interp.getMaxKey() > counter*20 ){ - double interpValue = interp.get(counter*20); + while(interp.getMaxKey() > counter*20 ){ + double interpValue = interp.get(counter*20); - //std::cout << counter*20 << " " << interpValue << " i" << std::endl; + //std::cout << counter*20 << " " << interpValue << " i" << std::endl; - input.push_back(interpValue); + input.push_back(interpValue); - output.push_back(butter.process(interpValue)); + output.push_back(butter.process(interpValue)); - ++counter; - } + ++counter; + } - //std::cout << ts << " " << value << " r" << std::endl; + //std::cout << ts << " " << value << " r" << std::endl; - } - } + } + } - K::Gnuplot gp; - K::GnuplotPlot plot; - K::GnuplotPlotElementLines linesInput; - K::GnuplotPlotElementLines linesOutput; + K::Gnuplot gp; + K::GnuplotPlot plot; + K::GnuplotPlotElementLines linesInput; + K::GnuplotPlotElementLines linesOutput; - for(int i=0; i < input.size()-1; ++i){ + for(int i=0; i < input.size()-1; ++i){ - K::GnuplotPoint2 input_p1(i, input[i]); - K::GnuplotPoint2 input_p2(i+1, input[i+1]); + K::GnuplotPoint2 input_p1(i, input[i]); + K::GnuplotPoint2 input_p2(i+1, input[i+1]); - K::GnuplotPoint2 output_p1(i, output[i]); - K::GnuplotPoint2 output_p2(i+1, output[i+1]); + K::GnuplotPoint2 output_p1(i, output[i]); + K::GnuplotPoint2 output_p2(i+1, output[i+1]); - linesInput.addSegment(input_p1, input_p2); - linesOutput.addSegment(output_p1, output_p2); - } + linesInput.addSegment(input_p1, input_p2); + linesOutput.addSegment(output_p1, output_p2); + } linesOutput.getStroke().getColor().setHexStr("#00FF00"); - plot.add(&linesInput); - plot.add(&linesOutput); + plot.add(&linesInput); + plot.add(&linesOutput); - gp.draw(plot); - gp.flush(); + gp.draw(plot); + gp.flush(); - sleep(1); + sleep(1); } diff --git a/tests/navMesh/TestNavMeshFactory.cpp b/tests/navMesh/TestNavMeshFactory.cpp new file mode 100644 index 0000000..805bba1 --- /dev/null +++ b/tests/navMesh/TestNavMeshFactory.cpp @@ -0,0 +1,38 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../navMesh/NavMeshFactory.h" + +TEST(NavMeshFactory, build1) { + + Floorplan::IndoorMap map; + Floorplan::Floor floor; map.floors.push_back(&floor); floor.atHeight = 0; floor.height = 3; + Floorplan::FloorOutlinePolygon outline; floor.outline.push_back(&outline); + outline.poly.points.push_back(Point2(0,0)); + outline.poly.points.push_back(Point2(10,0)); + outline.poly.points.push_back(Point2(10,10)); + outline.poly.points.push_back(Point2(0,10)); + outline.outdoor = false; + outline.method = Floorplan::OutlineMethod::ADD; + + NavMesh nm; + NavMeshFactory fac(&nm); + fac.build(&map); + + ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5); + ASSERT_NEAR(0, nm.getBBox().getMin().y, 0.5); + ASSERT_NEAR(0, nm.getBBox().getMin().z, 0.5); + + ASSERT_NEAR(10, nm.getBBox().getMax().x, 0.5); + ASSERT_NEAR(10, nm.getBBox().getMax().y, 0.5); + ASSERT_NEAR( 0, nm.getBBox().getMax().z, 0.5); + + ASSERT_EQ(2, nm.getNumTriangles()); + + ASSERT_EQ(nm.getNeighbor(0,0), nm[1]); + ASSERT_EQ(nm.getNeighbor(1,0), nm[0]); + +} + +#endif diff --git a/tests/navMesh/TestNavMeshTriangle.cpp b/tests/navMesh/TestNavMeshTriangle.cpp new file mode 100644 index 0000000..1234161 --- /dev/null +++ b/tests/navMesh/TestNavMeshTriangle.cpp @@ -0,0 +1,35 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../navMesh/NavMeshTriangle.h" + +TEST(NavMeshTriangle, contains) { + + NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0)); + + ASSERT_TRUE(t1.contains(Point3(0,0,0))); + ASSERT_TRUE(t1.contains(Point3(1,0,0))); + ASSERT_TRUE(t1.contains(Point3(0,1,0))); + ASSERT_TRUE(t1.contains(Point3(0.5,0.5,0))); + + ASSERT_FALSE(t1.contains(Point3(0.501,0.5,0))); + ASSERT_FALSE(t1.contains(Point3(0.5,0.501,0))); + ASSERT_FALSE(t1.contains(Point3(1,1,0))); + +} + +TEST(NavMeshTriangle, area) { + + NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0)); + ASSERT_NEAR(0.5, t1.getArea(), 0.0001); + + NavMeshTriangle t2(Point3(0,0,9), Point3(1,0,9), Point3(0,1,9)); + ASSERT_NEAR(0.5, t2.getArea(), 0.0001); + +} + + + + +#endif diff --git a/tests/sensors/imu/TestMotionDetection.cpp b/tests/sensors/imu/TestMotionDetection.cpp index eb76a40..ca2e2ed 100644 --- a/tests/sensors/imu/TestMotionDetection.cpp +++ b/tests/sensors/imu/TestMotionDetection.cpp @@ -1,4 +1,4 @@ -#ifdef WITH_TESTS +#ifdef TODO_________WITH_TESTS #include "../../Tests.h" @@ -9,190 +9,190 @@ /** visualize the motionAxis */ TEST(MotionDetection, motionAxis) { - MotionDetection md; + MotionDetection md; - //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; + //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; - //Walking with smartphone straight and always parallel to motion axis - //std::string filename = getDataFile("motion/straight_potrait.csv"); + //Walking with smartphone straight and always parallel to motion axis + //std::string filename = getDataFile("motion/straight_potrait.csv"); - //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right. - std::string filename = getDataFile("motion/straight_landscape_left.csv"); - //std::string filename = getDataFile("motion/straight_landscape_right.csv"); + //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right. + std::string filename = getDataFile("motion/straight_landscape_left.csv"); + //std::string filename = getDataFile("motion/straight_landscape_right.csv"); - //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase - //std::string filename = getDataFile("motion/straight_inturn_landscape.csv"); + //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase + //std::string filename = getDataFile("motion/straight_inturn_landscape.csv"); - //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns. - //std::string filename = getDataFile("motion/rounds_potrait.csv"); + //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait.csv"); - //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns. - //std::string filename = getDataFile("motion/rounds_landscape.csv"); + //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_landscape.csv"); - //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns. - //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv"); + //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv"); - //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me) - //std::string filename = getDataFile("motion/rounds_pocket.csv"); + //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me) + //std::string filename = getDataFile("motion/rounds_pocket.csv"); - //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. - //std::string filename = getDataFile("motion/table_flat.csv"); + //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. + //std::string filename = getDataFile("motion/table_flat.csv"); Offline::FileReader fr(filename); - K::Gnuplot gp; - K::GnuplotPlot plot; + K::Gnuplot gp; + K::GnuplotPlot plot; - gp << "set xrange[-1:1]\n set yrange[-1:1]\n"; + gp << "set xrange[-1:1]\n set yrange[-1:1]\n"; - Eigen::Vector2f curVec; - float motionAxisAngleRad; - Timestamp ts; - Timestamp lastTs; + Eigen::Vector2f curVec; + float motionAxisAngleRad; + Timestamp ts; + Timestamp lastTs; - //calc motion axis + //calc motion axis for (const Offline::Entry& e : fr.getEntries()) { - ts = Timestamp::fromMS(e.ts); + ts = Timestamp::fromMS(e.ts); if (e.type == Offline::Sensor::LIN_ACC) { - md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); } else if (e.type == Offline::Sensor::GRAVITY) { - md.addGravity(ts, fr.getGravity()[e.idx].data); - curVec = md.getCurrentMotionAxis(); - motionAxisAngleRad = md.getMotionChangeInRad(); - } + md.addGravity(ts, fr.getGravity()[e.idx].data); + curVec = md.getCurrentMotionAxis(); + motionAxisAngleRad = md.getMotionChangeInRad(); + } - // start with the first available timestamp - if (lastTs.isZero()) {lastTs = ts;} + // start with the first available timestamp + if (lastTs.isZero()) {lastTs = ts;} - if(ts - lastTs > Timestamp::fromMS(500)) { + if(ts - lastTs > Timestamp::fromMS(500)) { - lastTs = ts; + lastTs = ts; - K::GnuplotPoint2 raw_p1(0, 0); - K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0)); - K::GnuplotPlotElementLines motionLines; - motionLines.addSegment(raw_p1, raw_p2); - plot.add(&motionLines); + K::GnuplotPoint2 raw_p1(0, 0); + K::GnuplotPoint2 raw_p2(curVec(0,0), curVec(1,0)); + K::GnuplotPlotElementLines motionLines; + motionLines.addSegment(raw_p1, raw_p2); + plot.add(&motionLines); - gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159 << "' at screen 0.1,0.1\n"; + gp << "set label 111 ' Angle: " << motionAxisAngleRad * 180 / 3.14159 << "' at screen 0.1,0.1\n"; - gp.draw(plot); - gp.flush(); - //usleep(5000*33); - } - } + gp.draw(plot); + gp.flush(); + //usleep(5000*33); + } + } - //was passiert bei grenzwerten. 90° oder sowas. - //wie stabil ist die motion axis eigentlich? - //erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen? - //wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading? + //was passiert bei grenzwerten. 90° oder sowas. + //wie stabil ist die motion axis eigentlich? + //erkenn wir aktuell überhaupt einen turn, wenn wir das telefon drehen? + //wie hilft mir die motion achse? über einen faktor? in welchem verhältnis stehen motion axis und heading? } /** comparing motionAngle and turnAngle */ TEST(MotionDetection, motionAngle) { - MotionDetection md; - TurnDetection td; + MotionDetection md; + TurnDetection td; - //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; + //plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n"; - //Walking with smartphone straight and always parallel to motion axis - std::string filename = getDataFile("motion/straight_potrait.csv"); + //Walking with smartphone straight and always parallel to motion axis + std::string filename = getDataFile("motion/straight_potrait.csv"); - //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right. - //std::string filename = getDataFile("motion/straight_landscape_left.csv"); - //std::string filename = getDataFile("motion/straight_landscape_right.csv"); + //straight_landscape_left/right: walking ~40 sec straight and changing every 5 seconds the mode. started with potrait. landscape routed either to left or right. + //std::string filename = getDataFile("motion/straight_landscape_left.csv"); + //std::string filename = getDataFile("motion/straight_landscape_right.csv"); - //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase - //std::string filename = getDataFile("motion/straight_inturn_landscape.csv"); + //straight_inturn_landscape: walked straight made a left turn and change the phone to landscape mode during the turn-phase + //std::string filename = getDataFile("motion/straight_inturn_landscape.csv"); - //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns. - //std::string filename = getDataFile("motion/rounds_potrait.csv"); + //rounds_potrait: walked 3 rounds holding the phone in potrait mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait.csv"); - //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns. - //std::string filename = getDataFile("motion/rounds_landscape.csv"); + //round_landscape: walked 3 rounds holding the phone in landscape mode. always making left turns. + //std::string filename = getDataFile("motion/rounds_landscape.csv"); - //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns. - //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv"); + //round potrait_to_landscape: walked 1 round with potrait, 1 with landscape and again potrait. the mode was change while walking straight not in a turn. always making left turns. + //std::string filename = getDataFile("motion/rounds_potrait_to_landscape.csv"); - //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me) - //std::string filename = getDataFile("motion/rounds_pocket.csv"); + //rounds_pocket: had the phone in my jeans pocket screen pointed at my body and the phone was headfirst. pulled it shortly out after 2 rounds and rotated the phone 180° z-wise (screen not showing at me) + //std::string filename = getDataFile("motion/rounds_pocket.csv"); - //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. - //std::string filename = getDataFile("motion/table_flat.csv"); + //table_flat: phone was flat on the table and moved slowly forward/backward for 60 cm. + //std::string filename = getDataFile("motion/table_flat.csv"); Offline::FileReader fr(filename); - Timestamp ts; + Timestamp ts; - //save for later plotting - std::vector delta_motionAngles; - std::vector delta_turnAngles; + //save for later plotting + std::vector delta_motionAngles; + std::vector delta_turnAngles; - //calc motion axis + //calc motion axis for (const Offline::Entry& e : fr.getEntries()) { - ts = Timestamp::fromMS(e.ts); + ts = Timestamp::fromMS(e.ts); if (e.type == Offline::Sensor::LIN_ACC) { - md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); } else if (e.type == Offline::Sensor::GRAVITY) { - md.addGravity(ts, fr.getGravity()[e.idx].data); - delta_motionAngles.push_back(md.getMotionChangeInRad()); + md.addGravity(ts, fr.getGravity()[e.idx].data); + delta_motionAngles.push_back(md.getMotionChangeInRad()); } else if (e.type == Offline::Sensor::ACC) { const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; - td.addAccelerometer(ts, _acc.data); + td.addAccelerometer(ts, _acc.data); } else if (e.type == Offline::Sensor::GYRO) { const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; - delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data)); - } + delta_turnAngles.push_back(td.addGyroscope(ts, _gyr.data)); + } - } + } - //draw motion - static K::Gnuplot gpMotion; - K::GnuplotPlot plotMotion; - K::GnuplotPlotElementLines motionLines; + //draw motion + static K::Gnuplot gpMotion; + K::GnuplotPlot plotMotion; + K::GnuplotPlotElementLines motionLines; - for(int i = 0; i < delta_motionAngles.size() - 1; ++i){ + for(int i = 0; i < delta_motionAngles.size() - 1; ++i){ - K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]); - K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]); - motionLines.addSegment(raw_p1, raw_p2); + K::GnuplotPoint2 raw_p1(i, delta_motionAngles[i]); + K::GnuplotPoint2 raw_p2(i + 1, delta_motionAngles[i+1]); + motionLines.addSegment(raw_p1, raw_p2); - } + } - gpMotion << "set title 'Motion Detection'\n"; - plotMotion.add(&motionLines); - gpMotion.draw(plotMotion); - gpMotion.flush(); + gpMotion << "set title 'Motion Detection'\n"; + plotMotion.add(&motionLines); + gpMotion.draw(plotMotion); + gpMotion.flush(); - //draw rotation - static K::Gnuplot gpTurn; - K::GnuplotPlot plotTurn; - K::GnuplotPlotElementLines turnLines; + //draw rotation + static K::Gnuplot gpTurn; + K::GnuplotPlot plotTurn; + K::GnuplotPlotElementLines turnLines; - for(int i = 0; i < delta_turnAngles.size() - 1; ++i){ + for(int i = 0; i < delta_turnAngles.size() - 1; ++i){ - K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]); - K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]); - turnLines.addSegment(raw_p1, raw_p2); - } + K::GnuplotPoint2 raw_p1(i, delta_turnAngles[i]); + K::GnuplotPoint2 raw_p2(i + 1, delta_turnAngles[i+1]); + turnLines.addSegment(raw_p1, raw_p2); + } - gpTurn << "set title 'Turn Detection'\n"; - plotTurn.add(&turnLines); - gpTurn.draw(plotTurn); - gpTurn.flush(); + gpTurn << "set title 'Turn Detection'\n"; + plotTurn.add(&turnLines); + gpTurn.draw(plotTurn); + gpTurn.flush(); - sleep(1); + sleep(1); } diff --git a/tests/sensors/imu/TestTurnDetection.cpp b/tests/sensors/imu/TestTurnDetection.cpp index 9af88f3..3f79dad 100644 --- a/tests/sensors/imu/TestTurnDetection.cpp +++ b/tests/sensors/imu/TestTurnDetection.cpp @@ -6,33 +6,33 @@ TEST(TurnDetection, rotationMatrix) { - Eigen::Vector3f dst; dst << 0, 0, 1; - Eigen::Vector3f src; src << 1, 1, 0; src.normalize(); + Vector3 dst(0, 0, 1); + Vector3 src(1, 1, 0); src = src.normalized(); // get a matrix that rotates "src" into "dst" - Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst); + Matrix3 rot = PoseDetection::getRotationMatrix(src, dst); - Eigen::Vector3f res = rot * src; + Vector3 res = rot * src; - ASSERT_NEAR(dst(0), res(0), 0.01); - ASSERT_NEAR(dst(1), res(1), 0.01); - ASSERT_NEAR(dst(2), res(2), 0.01); + ASSERT_NEAR(dst.x, res.x, 0.01); + ASSERT_NEAR(dst.y, res.y, 0.01); + ASSERT_NEAR(dst.z, res.z, 0.01); } TEST(TurnDetection, gyroRotate) { - Eigen::Vector3f zAxis; zAxis << 0, 0, 1; - Eigen::Vector3f acc; acc << 0, 7.0, 7.0; + Vector3 zAxis(0, 0, 1); + Vector3 acc(0, 7.0, 7.0); - Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(acc, zAxis); + Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis); - Eigen::Vector3f gyro; gyro << 0, 60, 60; + Vector3 gyro(0, 60, 60); - Eigen::Vector3f gyro2; gyro2 << 0, 0, 84; + Vector3 gyro2(0, 0, 84); - Eigen::Vector3f gyro3 = rot * gyro; + Vector3 gyro3 = rot * gyro; ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0); @@ -41,10 +41,10 @@ TEST(TurnDetection, gyroRotate) { TEST(TurnDetection, xx) { - Eigen::Vector3f dst; dst << 0, 0, 1; - Eigen::Vector3f src; src << 0.0, 2.9, -10.0; src.normalize(); // sample accelerometer readings + Vector3 dst(0, 0, 1); + Vector3 src(0.0, 2.9, -10.0); src = src.normalized(); // sample accelerometer readings - Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst); + Matrix3 rot = PoseDetection::getRotationMatrix(src, dst); // Eigen::Vector3f x; x << 1, 0, 0; // Eigen::Vector3f z = src.normalized(); @@ -55,14 +55,14 @@ TEST(TurnDetection, xx) { // rot.row(1) = y; // rot.row(2) = z; - Eigen::Vector3f res = rot * src; + Vector3 res = rot * src; // ASSERT_NEAR(dst(0), res(0), 0.01); // ASSERT_NEAR(dst(1), res(1), 0.01); // ASSERT_NEAR(dst(2), res(2), 0.01); - Eigen::Vector3f gyro; gyro << 0, 10, 30; + Vector3 gyro(0, 10, 30); - Eigen::Vector3f gyro2 = rot * gyro; + Vector3 gyro2 = rot * gyro; int i = 0; (void) i; } diff --git a/tests/sensors/pressure/TestBarometer.cpp b/tests/sensors/pressure/TestBarometer.cpp index 7c3451f..60378e9 100644 --- a/tests/sensors/pressure/TestBarometer.cpp +++ b/tests/sensors/pressure/TestBarometer.cpp @@ -1,11 +1,11 @@ -#ifdef WITH_TESTS +#ifdef TODO_______WITH_TESTS #include "../../Tests.h" #include "../../../sensors/pressure/RelativePressure.h" #include "../../../sensors/pressure/PressureTendence.h" -#include "../../../sensors/pressure/ActivityButterPressure.h" -#include "../../../sensors/pressure/ActivityButterPressurePercent.h" +#include "../../../sensors/activity/ActivityButterPressure.h" +#include "../../../sensors/activity/ActivityButterPressurePercent.h" #include @@ -79,7 +79,7 @@ TEST(Barometer, LIVE_tendence) { } - sleep(1); + sleep(1); } @@ -115,7 +115,7 @@ TEST(Barometer, LIVE_tendence2) { } - sleep(1); + sleep(1); // tendence must be clear and smaller than the sigma @@ -124,119 +124,119 @@ TEST(Barometer, LIVE_tendence2) { } TEST(Barometer, Activity) { - ActivityButterPressure act; + ActivityButterPressure act; - //read file - std::string line; + //read file + std::string line; std::string filename = getDataFile("barometer/baro1.dat"); - std::ifstream infile(filename); + std::ifstream infile(filename); - std::vector actHist; - std::vector rawHist; + std::vector actHist; + std::vector rawHist; - while (std::getline(infile, line)) - { - std::istringstream iss(line); - int ts; - double value; + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; - while (iss >> ts >> value) { - ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value)); - rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value))); - actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct))); - } - } + while (iss >> ts >> value) { + ActivityButterPressure::Activity currentAct = act.add(Timestamp::fromMS(ts), BarometerData(value)); + rawHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(value))); + actHist.push_back(ActivityButterPressure::History(Timestamp::fromMS(ts), BarometerData(currentAct))); + } + } - K::Gnuplot gp; - K::Gnuplot gpRaw; - K::GnuplotPlot plot; - K::GnuplotPlot plotRaw; - K::GnuplotPlotElementLines rawLines; - K::GnuplotPlotElementLines resultLines; + K::Gnuplot gp; + K::Gnuplot gpRaw; + K::GnuplotPlot plot; + K::GnuplotPlot plotRaw; + K::GnuplotPlotElementLines rawLines; + K::GnuplotPlotElementLines resultLines; - for(int i=0; i < actHist.size()-1; ++i){ + for(int i=0; i < actHist.size()-1; ++i){ - //raw - K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa); - K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa); + //raw + K::GnuplotPoint2 raw_p1(rawHist[i].ts.sec(), rawHist[i].data.hPa); + K::GnuplotPoint2 raw_p2(rawHist[i+1].ts.sec(), rawHist[i+1].data.hPa); - rawLines.addSegment(raw_p1, raw_p2); + rawLines.addSegment(raw_p1, raw_p2); - //results - K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa); - K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa); + //results + K::GnuplotPoint2 input_p1(actHist[i].ts.sec(), actHist[i].data.hPa); + K::GnuplotPoint2 input_p2(actHist[i+1].ts.sec(), actHist[i+1].data.hPa); - resultLines.addSegment(input_p1, input_p2); - } + resultLines.addSegment(input_p1, input_p2); + } - plotRaw.add(&rawLines); - plot.add(&resultLines); + plotRaw.add(&rawLines); + plot.add(&resultLines); - gp.draw(plot); - gp.flush(); + gp.draw(plot); + gp.flush(); - gpRaw.draw(plotRaw); - gpRaw.flush(); + gpRaw.draw(plotRaw); + gpRaw.flush(); - sleep(5); + sleep(5); } TEST(Barometer, ActivityPercent) { - ActivityButterPressurePercent act; + ActivityButterPressurePercent act; - //read file - std::string line; - std::string filename = getDataFile("barometer/baro1.dat"); - std::ifstream infile(filename); + //read file + std::string line; + std::string filename = getDataFile("barometer/baro1.dat"); + std::ifstream infile(filename); - std::vector actHist; - std::vector rawHist; + std::vector actHist; + std::vector rawHist; - while (std::getline(infile, line)) - { - std::istringstream iss(line); - int ts; - double value; + while (std::getline(infile, line)) + { + std::istringstream iss(line); + int ts; + double value; - while (iss >> ts >> value) { - ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value)); - rawHist.push_back(value); - actHist.push_back(activity); - } - } + while (iss >> ts >> value) { + ActivityButterPressurePercent::ActivityProbabilities activity = act.add(Timestamp::fromMS(ts), BarometerData(value)); + rawHist.push_back(value); + actHist.push_back(activity); + } + } - K::Gnuplot gp; - K::Gnuplot gpRaw; - K::GnuplotPlot plot; - K::GnuplotPlot plotRaw; - K::GnuplotPlotElementLines rawLines; - K::GnuplotPlotElementLines resultLines; + K::Gnuplot gp; + K::Gnuplot gpRaw; + K::GnuplotPlot plot; + K::GnuplotPlot plotRaw; + K::GnuplotPlotElementLines rawLines; + K::GnuplotPlotElementLines resultLines; - for(int i=0; i < actHist.size()-1; ++i){ + for(int i=0; i < actHist.size()-1; ++i){ - K::GnuplotPoint2 raw_p1(i, rawHist[i]); - K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]); + K::GnuplotPoint2 raw_p1(i, rawHist[i]); + K::GnuplotPoint2 raw_p2(i+1, rawHist[i+1]); - rawLines.addSegment(raw_p1, raw_p2); + rawLines.addSegment(raw_p1, raw_p2); - K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown); - K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown); + K::GnuplotPoint2 input_p1(i, actHist[i].elevatorDown); + K::GnuplotPoint2 input_p2(i+1, actHist[i+1].elevatorDown); - resultLines.addSegment(input_p1, input_p2); - } + resultLines.addSegment(input_p1, input_p2); + } - plotRaw.add(&rawLines); - plot.add(&resultLines); + plotRaw.add(&rawLines); + plot.add(&resultLines); - gp.draw(plot); - gp.flush(); + gp.draw(plot); + gp.flush(); - gpRaw.draw(plotRaw); - gpRaw.flush(); + gpRaw.draw(plotRaw); + gpRaw.flush(); - sleep(5); + sleep(5); }