From 7af5131ccf8d27f5c03ea1bf90f727b8405a3a54 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 15 Nov 2017 16:41:57 +0100 Subject: [PATCH 01/12] worked on grid walker --- grid/walk/v3/Helper.h | 50 +++++++--- grid/walk/v3/WalkEvaluator.h | 27 ++++-- grid/walk/v3/Walker.h | 173 +++++++++++++++++++++++------------ synthetic/SyntheticTurns.h | 32 ++++--- 4 files changed, 190 insertions(+), 92 deletions(-) diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h index 9d51178..a47b3f6 100644 --- a/grid/walk/v3/Helper.h +++ b/grid/walk/v3/Helper.h @@ -84,43 +84,67 @@ namespace GW3 { }; + /** + * data-structure to track to-be-visited nodes + * push_back, pop_front + * as pop_front is costly, we omit the pop and use a head-index instead + * memory-consumption vs speed + */ + struct ToVisit { + size_t nextIdx = 0; + std::vector vec; + ToVisit() {vec.reserve(256);} + void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);} + uint32_t next() {return vec[nextIdx++];} + bool empty() const {return nextIdx >= vec.size();} + }; + /** get an iterator over all nodes reachable from the given start */ - template class ReachableIteratorUnsorted { + template class ReachableIteratorUnsorted { const Grid& grid; const Node& start; Node* curNode = nullptr; std::unordered_set visited; - std::vector toVisit; + ToVisit toVisit; + + Conditions cond; public: - ReachableIteratorUnsorted(const Grid& grid, const Node& start) : grid(grid), start(start) { - toVisit.push_back(start.getIdx()); + ReachableIteratorUnsorted(const Grid& grid, const Node& start, const Conditions cond) : grid(grid), start(start), cond(cond) { + toVisit.add(start.getIdx()); } bool hasNext() const { return !toVisit.empty(); } - const Node& next(const std::function& skip) { - - const uint32_t curIdx = toVisit.front(); //visit from inside out (needed for correct distance) - toVisit.erase(toVisit.begin()); - visited.insert(curIdx); + //const Node& next(const std::function& skip) { + //template const Node& next(const Skip skip) { + const Node& next() { + // get the next to-be-visited node + const uint32_t curIdx = toVisit.next(); //visit from inside out (needed for correct distance) const Node& curNode = grid[curIdx]; - for (int i = 0; i < curNode.getNumNeighbors(); ++i) { + // mark as "visited" + visited.insert(curIdx); - const int neighborIdx = curNode.getNeighborIdx(i); + // get all neighbors + const int numNeighbors = curNode.getNumNeighbors(); + for (int i = 0; i < numNeighbors; ++i) { + + const uint32_t neighborIdx = curNode.getNeighborIdx(i); const Node& neighbor = grid[neighborIdx]; + const bool visit = cond.visit(neighbor) ; + // not yet reached -> store distance - if (!skip(neighbor)) { + if (visit) { if (visited.find(neighborIdx) == visited.end()) { - toVisit.push_back(neighborIdx); + toVisit.add(neighborIdx); } } diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h index 8b73dbe..ccc9029 100644 --- a/grid/walk/v3/WalkEvaluator.h +++ b/grid/walk/v3/WalkEvaluator.h @@ -39,7 +39,8 @@ namespace GW3 { const GridPoint gp = Helper::p3ToGp(pEnd); const Node& node = grid->getNodeFor(gp); const double p = node.getWalkImportance(); - return std::pow(p,10); + return p; + //return std::pow(p,10); } @@ -50,11 +51,19 @@ namespace GW3 { /** evaluate the difference between head(start,end) and the requested heading */ template class WalkEvalHeadingStartEnd : public WalkEvaluator { - const double sigma; + const double sigma_rad; + const double kappa; + Distribution::VonMises _dist; + Distribution::LUT dist; public: - WalkEvalHeadingStartEnd(const double sigma = 0.04) : sigma(sigma) {;} + // kappa = 1/var = 1/sigma^2 + // https://en.wikipedia.org/wiki/Von_Mises_distribution + WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) : + sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) { + ; + } virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { @@ -68,7 +77,8 @@ namespace GW3 { const Heading head(pStart.xy(), pEnd.xy()); const float diff = head.getDiffHalfRAD(params.heading); //const float diff = Heading::getSignedDiff(params.heading, head); - return Distribution::Normal::getProbability(0, sigma, diff); + //return Distribution::Normal::getProbability(0, sigma, diff); + return dist.getProbability(diff); } @@ -79,14 +89,19 @@ namespace GW3 { const double sigma; + const Distribution::Normal dist; + public: - WalkEvalDistance(const double sigma = 0.1) : sigma(sigma) {;} + WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;} virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + const float requestedDistance_m = params.distance_m; const float walkedDistance_m = pStart.getDistance(pEnd); - return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); + const float diff = walkedDistance_m - requestedDistance_m; + return dist.getProbability(diff); + //return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); } diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index 543059b..92f2d67 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -26,18 +26,22 @@ namespace GW3 { public: /** get a new destination for the given start */ - virtual const WalkResult getDestination(Grid& grid, const WalkParams& params) const = 0; + virtual const WalkResult getDestination(const WalkParams& params) const = 0; }; template class WalkerDirectDestination : public WalkerBase { - //RandomGenerator rnd; - + Grid& grid; std::vector*> evals; public: + /** ctor */ + WalkerDirectDestination(Grid& grid) : grid(grid) { + ; + } + /** make the code a little more readable */ using Helper = GW3::Helper; using Walk = typename GW3::Walk; @@ -50,7 +54,7 @@ namespace GW3 { } /** perform the walk based on the configured setup */ - const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + const WalkResult getDestination(const WalkParams& params) const override { Assert::isNot0(params.distance_m, "walking distance must be > 0"); @@ -142,8 +146,20 @@ namespace GW3 { std::vector*> evals; + Grid& grid; + const float gridSize_m; + + mutable std::minstd_rand rndGen; + mutable std::uniform_real_distribution dFinal; + public: + /** ctor */ + WalkerWeightedRandom(Grid& grid) : + grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f) { + ; + } + /** make the code a little more readable */ using Helper = GW3::Helper; using Walk = typename GW3::Walk; @@ -156,79 +172,71 @@ namespace GW3 { } /** perform the walk based on the configured setup */ - const WalkResult getDestination(Grid& grid, const WalkParams& params) const override { + const WalkResult getDestination(const WalkParams& params) const override { Assert::isNot0(params.distance_m, "walking distance must be > 0"); - static std::minstd_rand rndGen; - const GridPoint gpStart = Helper::p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); + if (!startNode) {throw Exception("start node not found!");} -// // include one additional grid-cell (increased distance) -// const float secBuffer_m = params.lookFurther_m + (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 1.05); + const float maxDist = params.distance_m + gridSize_m; -// ReachableSettings set; -// set.limitDistance = true; -// set.limitHeading = true; -// set.dist_m = params.distance_m + secBuffer_m; -// set.heading = params.heading; -// set.maxHeadingDiff_rad = M_PI/2; -// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); - - const float gridSize_m = grid.getGridSize_cm() / 100.0f; - //std::uniform_int_distribution dNode(0, (int)reachableNodes.size() - 1); - - - Point3 best; - double bestP = 0; -// DrawList drawer; + Point3 best; double bestP = 0; + //DrawList drawer; const Point3 start = params.start; - // try X random destinations, evaluate them, draw one of em according to probability (reduces the number of "stupid particles") - //for (int i = 0; i < 500; ++i) { - - // const Node* dstNode = reachableNodes[dNode(rndGen)]; - - std::uniform_real_distribution dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f); - - ReachableIteratorUnsorted ri(grid, *startNode); - const float maxDist = params.distance_m * 1.25 + gridSize_m; - - auto skip = [&] (const Node& n) { - const float dist_m = n.getDistanceInMeter(gpStart); - return dist_m > maxDist; + struct RICond { + const GridPoint gpStart; + const float maxDist; + RICond(const GridPoint gpStart, const float maxDist) : gpStart(gpStart), maxDist(maxDist) {;} + bool visit (const Node& n) const { + const float dist_m = n.getDistanceInMeter(gpStart); + return dist_m < maxDist; + } }; + RICond riCond(gpStart, maxDist); - //for (const Node* dstNode : reachableNodes) { - while(ri.hasNext()) { + // iterate over all reachable nodes that satisfy a certain criteria (e.g. max distance) + ReachableIteratorUnsorted ri(grid, *startNode, riCond); - const Node* dstNode = &ri.next(skip); -// const float dist_m = dstNode->getDistanceInMeter(gpStart); + int numVisitedNodes = 0; -// if (dist_m > maxDist) { -// break; -// } - for (int i = 0; i < 25; ++i) { + #define MODE 1 + + #if (MODE == 1) + + double bestNodeP = 0; + const Node* bestNode = nullptr; + + while(ri.hasNext()) { + const Node* dstNode = &ri.next(); + const Point3 nodeCenter = Helper::gpToP3(*dstNode); + double p = 1.0; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, nodeCenter, params); + p *= p1; + } + if (p > bestNodeP) { + bestNodeP = p; + bestNode = dstNode; + } + } + + for (int i = 0; i < 10; ++i) { + + const Point3 nodeCenter = Helper::gpToP3(*bestNode); // random position within destination-node - const Point3 dstOffset(dFinal(rndGen), dFinal(rndGen), 0); + const float ox = dFinal(rndGen); + const float oy = dFinal(rndGen); - // destination = node-center + offset (within the node's bbox) - const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; - - // sanity check - if (start == end) {continue;} - if (!grid.hasNodeFor(Helper::p3ToGp(end))) { - std::cout << "random destination is not part of the grid" << std::endl; - continue; - } - - //Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); + // destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is) + const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z); double p = 1; for (const WalkEvaluator* eval : evals) { @@ -237,20 +245,63 @@ namespace GW3 { } if (p > bestP) {bestP = p; best = end;} - //drawer.add(end, p); } - } + #elif (MODE == 2) - //const Point3 end = drawer.get(); + // all reachable nodes + while(ri.hasNext()) { + + const Node* dstNode = &ri.next(); + ++numVisitedNodes; + + const Point3 nodeCenter = Helper::gpToP3(*dstNode); + + // try multiple locations within each reachable node + for (int i = 0; i < 1; ++i) { + + // random position within destination-node + const float ox = dFinal(rndGen); + const float oy = dFinal(rndGen); + + // destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is) + const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z); + + // sanity check + if (start == end) {continue;} + // if (!grid.hasNodeFor(Helper::p3ToGp(end))) { + // std::cout << "random destination is not part of the grid" << std::endl; + // continue; + // } + //Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); + + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, end, params); + p *= p1; + } + + if (p > bestP) {bestP = p; best = end;} + // drawer.add(end, p); + + } + + } + + #endif + + //std::cout << numVisitedNodes << std::endl; + + //double drawProb = 0; const Point3 end = drawer.get(drawProb); const Point3 end = best; WalkResult res; if (start == end) { res.probability = 0; } else { res.heading = Heading(start.xy(), end.xy()); - res.probability = bestP; + //res.probability = drawProb; // when using DrawList + res.probability = bestP; // when using bestP } res.position = end; return res; diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h index bec5bf7..f94db0c 100644 --- a/synthetic/SyntheticTurns.h +++ b/synthetic/SyntheticTurns.h @@ -7,7 +7,7 @@ #include "../sensors/imu/GyroscopeData.h" #include "../geo/Heading.h" -#include "../math/distribution/Normal.h" +#include "../math/Distributions.h" /** * simulates acceleromter and gyroscope data @@ -41,6 +41,8 @@ private: Distribution::Normal dChange = Distribution::Normal(1.0, 0.25); Distribution::Normal dHeadErr = Distribution::Normal(0.15, 0.10); // heading error, slightly biased + Distribution::Uniform dRadDiff = Distribution::Uniform(40,100); + std::vector listeners; @@ -68,7 +70,7 @@ protected: Heading desiredHead = Heading(0); Heading curHead = Heading(0); Point3 lastPos = Point3(NAN, NAN, NAN); - float change; + double change = 0; inline float clamp(const float val, const float min, const float max) { if (val < min) {return min;} @@ -86,27 +88,33 @@ protected: if (lastPos.x != lastPos.x) { lastPos = curPos; } else { - desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();; + desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw(); lastPos = curPos; } // difference between current-heading and desired-heading - const float diffRad = Heading::getSignedDiff(curHead, desiredHead); + const double maxChange = dMaxChange.draw(); + const double diffRad = Heading::getSignedDiff(curHead, desiredHead); + //change = clamp(diffRad / dRadDiff.draw(), -maxChange, +maxChange); + change = clamp(diffRad / 25, -maxChange, +maxChange); - // slowly change the current heading to match the desired one - const float maxChange = dMaxChange.draw(); - const float toChange = clamp(diffRad, -maxChange, +maxChange); - //if (change < toChange) {change += toChange*0.01;} - if (change > toChange) {change *= 0.93;} - if (change < toChange) {change += dChange.draw()/10000;} - //if (change > toChange) {change -= dChange.draw();} + +// // slowly change the current heading to match the desired one +// //const double maxChange = dMaxChange.draw(); +// //const double toChange = clamp(diffRad, -maxChange, +maxChange); +// const double toChange = diffRad; +// //if (change < toChange) {change += toChange*0.01;} +// if (change > toChange) {change *= 0.93;} +// //if (change < toChange) {change += dChange.draw()/10000;} // does not work for small changes?! +// if (change < toChange) {change += (toChange-change) * 0.07;} +// //if (change > toChange) {change -= dChange.draw();} curHead += change; // convert to gyro's radians-per-second - const float radPerSec = change * 1000 / deltaTs.ms();; + const double radPerSec = change * 1000 / deltaTs.ms();; const float accX = 0.00 + dAccX.draw(); const float accY = 0.00 + dAccY.draw(); From d03372ad3da425159d024f79d093621a2b75eee5 Mon Sep 17 00:00:00 2001 From: frank Date: Wed, 22 Nov 2017 13:00:02 +0100 Subject: [PATCH 02/12] worked on grid-walking --- grid/walk/v3/Reachable.h | 257 ++++++++++++++++++++++++++++++++ grid/walk/v3/ReachableSampler.h | 81 ++++++++++ grid/walk/v3/WalkEvaluator.h | 10 +- grid/walk/v3/Walker.h | 67 +++++++-- 4 files changed, 401 insertions(+), 14 deletions(-) create mode 100644 grid/walk/v3/Reachable.h create mode 100644 grid/walk/v3/ReachableSampler.h diff --git a/grid/walk/v3/Reachable.h b/grid/walk/v3/Reachable.h new file mode 100644 index 0000000..e353bcf --- /dev/null +++ b/grid/walk/v3/Reachable.h @@ -0,0 +1,257 @@ +#ifndef INDOOR_GW3_REACHABLE_H +#define INDOOR_GW3_REACHABLE_H + +#include +#include +#include "../../Grid.h" + +namespace GW3 { + +#define likely(x) __builtin_expect((x),1) +#define unlikely(x) __builtin_expect((x),0) + + + +/** + * get all grid nodes that are reachable within x-edges (depth) + */ +template class ReachableByDepthUnsorted { + + struct VisitEntry { + const Node* gn; + int depth; + VisitEntry() {;} + VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;} + }; + + struct Visits { + VisitEntry visits[512];// __attribute__((aligned(16))); + size_t head = 0; + size_t tail = 0; + VisitEntry& getNext() { + return visits[tail++]; + } + void add(const VisitEntry& e) { + visits[head++] = e; + assert(head < 512); + //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! + } + bool hasMore() const { + return head > tail; + } + }; + + const Grid& grid; + +public: + + ReachableByDepthUnsorted(const Grid& grid) : grid(grid) { + ; + } + + /** get all nodes reachable from start using maxDepth steps */ + std::unordered_set get(const Node& start, const int maxDepth) { + + std::unordered_set checked; + + // assuming max 8 neighbors per node, we need + // we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth) + // which is 1 + (1+2+3+4+5)*neighbors + // which is 1 + (n*n + n)/2*neighbors + // however this seems to be slow?! + //const int n = maxDepth + 1; + //const int maxEntries = (n * n + n) / 2 * 10 + 1; + //const int toAlloc = 4096 / sizeof(VisitEntry); + //if ( unlikely(toAlloc < maxEntries) ) {return checked;} + //if (maxDepth > 9) {throw Exception("will not fit!");} + + Visits toVisit; + + // directly start with the node itself and all its neighbors + checked.insert(&start); + for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { + const int nIdx = start.getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + checked.insert(&gnNext); + toVisit.add(VisitEntry(&gnNext, 1)); + } + + // check all to-be-visited nodes + while ( likely(toVisit.hasMore()) ) { + + const VisitEntry& e = toVisit.getNext(); + + if ( likely(e.depth <= maxDepth) ) { + + const Node* gnCur = e.gn; + for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { + const int nIdx = gnCur->getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + if ( unlikely(checked.find(&gnNext) == checked.end()) ) { + toVisit.add(VisitEntry(&gnNext, e.depth+1)); + checked.insert(&gnNext); + } + } + + } + + } + + return checked; + + } + +}; + + +/** + * get all grid nodes that are reachable within x-edges (depth) + * additionally returns the needed walking distance in meter + */ +template class ReachableByDepthWithDistanceSorted { + + struct VisitEntry { + const Node* gn; + int depth; + float dist_m; + int myIdx; + VisitEntry() {;} + VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) : + gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;} + }; + + struct Visits { + VisitEntry visits[1024];// __attribute__((aligned(16))); + size_t head = 0; + size_t tail = 0; + VisitEntry& getNext() { + return visits[tail++]; + } + void add(const VisitEntry& e) { + visits[head++] = e; + assert(head < 1024); + //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! + } + bool hasMore() const { + return head > tail; + } + void sort() { + const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { + return e1.dist_m < e2.dist_m; + }; + std::sort(&visits[tail], &visits[head], comp); + } + }; + + const Grid& grid; + +public: + + /** result */ + struct Entry { + + const Node* node; + const float walkDistToStart_m; + const int prevIdx; + + Entry(const Node* node, const float dist, const size_t prevIdx) : + node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;} + + bool hasPrev() const { + return prevIdx >= 0; + } + + }; + + ReachableByDepthWithDistanceSorted(const Grid& grid) : grid(grid) { + ; + } + + /** get all nodes reachable from start using maxDepth steps */ + std::vector get(const Node& start, const int maxDepth) { + + std::unordered_set checked; + std::vector res; + + Visits toVisit; + + // directly start with the node itself and all its neighbors + checked.insert(&start); + res.push_back(Entry(&start, 0, -1)); + for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { + const int nIdx = start.getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + const float dist_m = gnNext.getDistanceInMeter(start); + toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size())); + res.push_back(Entry(&gnNext, dist_m, 0)); + checked.insert(&gnNext); + } + toVisit.sort(); + + // check all to-be-visited nodes + while ( likely(toVisit.hasMore()) ) { + + const VisitEntry& e = toVisit.getNext(); + + if ( likely(e.depth <= maxDepth) ) { + + const Node* gnCur = e.gn; + +// for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { +// const int nIdx = gnCur->getNeighborIdx(i); +// const Node& gnNext = grid[nIdx]; +// if ( unlikely(checked.find(&gnNext) == checked.end()) ) { +// const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); +// const float dist_m = e.dist_m + nodeNodeDist_m; +// toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); +// res.push_back(Entry(&gnNext, dist_m, e.myIdx)); +// checked.insert(&gnNext); +// } +// } + +// const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01; + + std::vector sub; + + for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { + const int nIdx = gnCur->getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + if ( unlikely(checked.find(&gnNext) == checked.end()) ) { + const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); + const float dist_m = e.dist_m + nodeNodeDist_m; + //toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); + sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); + res.push_back(Entry(&gnNext, dist_m, e.myIdx)); + checked.insert(&gnNext); + + } + } + + // dijkstra.. sort the new nodes by destination to start + // only sorting the 8 new nodes seems enough due to the graph's layout + const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { + return e1.dist_m < e2.dist_m; + }; + + std::sort(sub.begin(), sub.end(), comp); + + for (const VisitEntry& e : sub) { + toVisit.add(e); + } + + } + + // slower with same result ;) + //toVisit.sort(); + + } + + return res; + + } + +}; + +} + +#endif // REACHABLE_H diff --git a/grid/walk/v3/ReachableSampler.h b/grid/walk/v3/ReachableSampler.h new file mode 100644 index 0000000..7222ece --- /dev/null +++ b/grid/walk/v3/ReachableSampler.h @@ -0,0 +1,81 @@ +#ifndef INDOOR_GW3_REACHABLESAMPLER_H +#define INDOOR_GW3_REACHABLESAMPLER_H + +#include "../../../math/Random.h" + +#include "Reachable.h" +#include "Helper.h" + +namespace GW3 { + + template class ReachableSamplerByDepth { + + public: + + using Entry = typename ReachableByDepthWithDistanceSorted::Entry; + + struct SampleResult { + Point3 pos; + float walkDistToStart_m; + SampleResult(const Point3 pos, const float dist_m) : pos(pos), walkDistToStart_m(dist_m) {;} + }; + + private: + + const Grid& grid; + const float gridSize_m; + + const std::vector& reachableNodes; + + mutable RandomGenerator gen; + + mutable std::uniform_real_distribution dOffset; + + + public: + + /** ctor */ + ReachableSamplerByDepth(const Grid& grid, const std::vector& reachableNodes) : + grid(grid), reachableNodes(reachableNodes), gridSize_m(grid.getGridSize_cm() / 100.0f), dOffset(-gridSize_m*0.48f, +gridSize_m*0.48f) { + ; + } + + SampleResult sample() { + + std::uniform_int_distribution dIdx(0, reachableNodes.size() - 1); + + const int idx = dIdx(gen); + + const Entry* e = &reachableNodes[idx]; + const Entry* ePrev1 = (e->prevIdx == -1) ? (nullptr) : (&reachableNodes[e->prevIdx]); + const Node* nDst = e->node; + + // center of the destination node + const Point3 nodeCenter = Helper::gpToP3(*nDst); + + // random position within destination-node + const float ox = dOffset(gen); + const float oy = dOffset(gen); + + // destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is) + const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z); + + // calculate end's walking-distance towards the start + float distToStart_m; + if (ePrev1) { + distToStart_m = ePrev1->walkDistToStart_m + (Helper::gpToP3(*(ePrev1->node)).getDistance(end)); + } else { + distToStart_m = nodeCenter.getDistance(end); + } + + // done + return SampleResult(end, distToStart_m); + + } + + + }; + +} + +#endif // REACHABLESAMPLER_H diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h index ccc9029..c34080a 100644 --- a/grid/walk/v3/WalkEvaluator.h +++ b/grid/walk/v3/WalkEvaluator.h @@ -17,7 +17,7 @@ namespace GW3 { /** get the probability for the given walk */ //virtual double getProbability(const Walk& walk) const = 0; - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0; + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const = 0; }; @@ -31,7 +31,7 @@ namespace GW3 { WalkEvalEndNodeProbability(Grid* grid) : grid(grid) {;} - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const override { (void) params; (void) pStart; @@ -65,7 +65,7 @@ namespace GW3 { ; } - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override { (void) params; @@ -95,10 +95,10 @@ namespace GW3 { WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;} - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override { + virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override { const float requestedDistance_m = params.distance_m; - const float walkedDistance_m = pStart.getDistance(pEnd); + const float walkedDistance_m = walkDist_m;//pStart.getDistance(pEnd); const float diff = walkedDistance_m - requestedDistance_m; return dist.getProbability(diff); //return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index 92f2d67..f15101d 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -14,6 +14,8 @@ #include "Helper.h" #include "Structs.h" #include "WalkEvaluator.h" +#include "Reachable.h" +#include "ReachableSampler.h" namespace GW3 { @@ -117,7 +119,7 @@ namespace GW3 { double p = 1; for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, end, params); + const double p1 = eval->getProbability(start, end, start.getDistance(end), params); p *= p1; } @@ -156,7 +158,7 @@ namespace GW3 { /** ctor */ WalkerWeightedRandom(Grid& grid) : - grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.49f, +gridSize_m*0.49f) { + grid(grid), gridSize_m(grid.getGridSize_cm() / 100.0f), dFinal(-gridSize_m*0.48f, +gridSize_m*0.48f) { ; } @@ -181,6 +183,7 @@ namespace GW3 { if (!startNode) {throw Exception("start node not found!");} const float maxDist = params.distance_m + gridSize_m; + const int depth = std::ceil(params.distance_m / gridSize_m) + 1; Point3 best; double bestP = 0; //DrawList drawer; @@ -206,15 +209,17 @@ namespace GW3 { int numVisitedNodes = 0; - #define MODE 1 + #define MODE 3 #if (MODE == 1) double bestNodeP = 0; const Node* bestNode = nullptr; - while(ri.hasNext()) { - const Node* dstNode = &ri.next(); + ReachableByDepthUnsorted reach(grid); + std::unordered_set nodes = reach.get(*startNode, depth); + + for (const Node* dstNode : nodes) { const Point3 nodeCenter = Helper::gpToP3(*dstNode); double p = 1.0; for (const WalkEvaluator* eval : evals) { @@ -227,6 +232,20 @@ namespace GW3 { } } +// while(ri.hasNext()) { +// const Node* dstNode = &ri.next(); +// const Point3 nodeCenter = Helper::gpToP3(*dstNode); +// double p = 1.0; +// for (const WalkEvaluator* eval : evals) { +// const double p1 = eval->getProbability(start, nodeCenter, params); +// p *= p1; +// } +// if (p > bestNodeP) { +// bestNodeP = p; +// bestNode = dstNode; +// } +// } + for (int i = 0; i < 10; ++i) { const Point3 nodeCenter = Helper::gpToP3(*bestNode); @@ -250,16 +269,21 @@ namespace GW3 { #elif (MODE == 2) - // all reachable nodes - while(ri.hasNext()) { + ReachableByDepthUnsorted reach(grid); + std::unordered_set nodes = reach.get(*startNode, depth); + + // all reachable nodes + //while(ri.hasNext()) { + //const Node* dstNode = &ri.next(); + + for (const Node* dstNode : nodes) { - const Node* dstNode = &ri.next(); ++numVisitedNodes; const Point3 nodeCenter = Helper::gpToP3(*dstNode); // try multiple locations within each reachable node - for (int i = 0; i < 1; ++i) { + for (int i = 0; i < 3; ++i) { // random position within destination-node const float ox = dFinal(rndGen); @@ -289,6 +313,31 @@ namespace GW3 { } + #elif (MODE == 3) + + using Reachable = ReachableByDepthWithDistanceSorted; + using ReachableNode = typename Reachable::Entry; + Reachable reach(grid); + std::vector reachableNodes = reach.get(*startNode, depth); + + using Sampler = ReachableSamplerByDepth; + using SamplerResult = typename Sampler::SampleResult; + Sampler sampler(grid, reachableNodes); + + for (int i = 0; i < 1500; ++i) { + + const SamplerResult sample = sampler.sample(); + + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(start, sample.pos, sample.walkDistToStart_m*0.94, params); + p *= p1; + } + + if (p > bestP) {bestP = p; best = sample.pos;} + + } + #endif //std::cout << numVisitedNodes << std::endl; From 55c061b344752aad2336cd8dd53602f2848b2dd8 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 22 Nov 2017 17:38:37 +0100 Subject: [PATCH 03/12] minor chaqnges --- grid/walk/v3/Walker.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index f15101d..c666042 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -299,10 +299,11 @@ namespace GW3 { // continue; // } //Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); + const float walkDist_m = end.getDistance(start);//*1.05; double p = 1; for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, end, params); + const double p1 = eval->getProbability(start, end, walkDist_m, params); p *= p1; } From 63bc2f30462fd102ed2bfa7ecee935d392daadd6 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 29 Nov 2017 16:35:29 +0100 Subject: [PATCH 04/12] worked on grid-walker and synthetic steps/turns --- grid/walk/v3/Helper.h | 16 +- grid/walk/v3/Reachable.h | 491 ++++++++++++++++++++++--------------- grid/walk/v3/Walker.h | 52 +++- synthetic/SyntheticSteps.h | 20 +- synthetic/SyntheticTurns.h | 23 +- 5 files changed, 370 insertions(+), 232 deletions(-) diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h index a47b3f6..c62e7b8 100644 --- a/grid/walk/v3/Helper.h +++ b/grid/walk/v3/Helper.h @@ -193,10 +193,22 @@ namespace GW3 { return bbox.contains(pt); } +// /** does one of the given grid-nodes contains the provided point-in-question? */ +// static const Node* contains(const Grid& grid, const Nodes& nodes, Point2 pt) { +// for (const Node* n : nodes) { +// if (contains(grid, n, pt)) { +// return n; +// } +// } +// return nullptr; +// } + /** does one of the given grid-nodes contains the provided point-in-question? */ - static const Node* contains(const Grid& grid, const Nodes& nodes, Point2 pt) { + static const Node* contains(const Grid& grid, const std::vector& nodes, Point2 pt) { for (const Node* n : nodes) { - if (contains(grid, n, pt)) {return n;} + if (contains(grid, n, pt)) { + return n; + } } return nullptr; } diff --git a/grid/walk/v3/Reachable.h b/grid/walk/v3/Reachable.h index e353bcf..fdd3c9b 100644 --- a/grid/walk/v3/Reachable.h +++ b/grid/walk/v3/Reachable.h @@ -7,250 +7,333 @@ namespace GW3 { -#define likely(x) __builtin_expect((x),1) -#define unlikely(x) __builtin_expect((x),0) + #define likely(x) __builtin_expect((x),1) + #define unlikely(x) __builtin_expect((x),0) -/** - * get all grid nodes that are reachable within x-edges (depth) - */ -template class ReachableByDepthUnsorted { + /** + * get all grid nodes that are reachable within x-edges (depth) + */ + template class ReachableByDepthUnsorted { - struct VisitEntry { - const Node* gn; - int depth; - VisitEntry() {;} - VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;} - }; + struct VisitEntry { + const Node* gn; + int depth; + VisitEntry() {;} + VisitEntry(const Node* gn, const int depth) : gn(gn), depth(depth) {;} + }; - struct Visits { - VisitEntry visits[512];// __attribute__((aligned(16))); - size_t head = 0; - size_t tail = 0; - VisitEntry& getNext() { - return visits[tail++]; - } - void add(const VisitEntry& e) { - visits[head++] = e; - assert(head < 512); - //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! - } - bool hasMore() const { - return head > tail; - } - }; + struct Visits { + VisitEntry visits[512];// __attribute__((aligned(16))); + size_t head = 0; + size_t tail = 0; + VisitEntry& getNext() { + return visits[tail++]; + } + void add(const VisitEntry& e) { + visits[head++] = e; + assert(head < 512); + //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! + } + bool hasMore() const { + return head > tail; + } + }; - const Grid& grid; + const Grid& grid; -public: + public: - ReachableByDepthUnsorted(const Grid& grid) : grid(grid) { - ; - } - - /** get all nodes reachable from start using maxDepth steps */ - std::unordered_set get(const Node& start, const int maxDepth) { - - std::unordered_set checked; - - // assuming max 8 neighbors per node, we need - // we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth) - // which is 1 + (1+2+3+4+5)*neighbors - // which is 1 + (n*n + n)/2*neighbors - // however this seems to be slow?! - //const int n = maxDepth + 1; - //const int maxEntries = (n * n + n) / 2 * 10 + 1; - //const int toAlloc = 4096 / sizeof(VisitEntry); - //if ( unlikely(toAlloc < maxEntries) ) {return checked;} - //if (maxDepth > 9) {throw Exception("will not fit!");} - - Visits toVisit; - - // directly start with the node itself and all its neighbors - checked.insert(&start); - for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { - const int nIdx = start.getNeighborIdx(i); - const Node& gnNext = grid[nIdx]; - checked.insert(&gnNext); - toVisit.add(VisitEntry(&gnNext, 1)); + ReachableByDepthUnsorted(const Grid& grid) : grid(grid) { + ; } - // check all to-be-visited nodes - while ( likely(toVisit.hasMore()) ) { + /** get all nodes reachable from start using maxDepth steps */ + std::unordered_set get(const Node& start, const int maxDepth) { - const VisitEntry& e = toVisit.getNext(); + std::unordered_set checked; - if ( likely(e.depth <= maxDepth) ) { + // assuming max 8 neighbors per node, we need + // we need 1 + 8 + 16 + 24 + 32 + ... entries (increments for each depth) + // which is 1 + (1+2+3+4+5)*neighbors + // which is 1 + (n*n + n)/2*neighbors + // however this seems to be slow?! + //const int n = maxDepth + 1; + //const int maxEntries = (n * n + n) / 2 * 10 + 1; + //const int toAlloc = 4096 / sizeof(VisitEntry); + //if ( unlikely(toAlloc < maxEntries) ) {return checked;} + //if (maxDepth > 9) {throw Exception("will not fit!");} - const Node* gnCur = e.gn; - for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { - const int nIdx = gnCur->getNeighborIdx(i); - const Node& gnNext = grid[nIdx]; - if ( unlikely(checked.find(&gnNext) == checked.end()) ) { - toVisit.add(VisitEntry(&gnNext, e.depth+1)); - checked.insert(&gnNext); + Visits toVisit; + + // directly start with the node itself and all its neighbors + checked.insert(&start); + for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { + const int nIdx = start.getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + checked.insert(&gnNext); + toVisit.add(VisitEntry(&gnNext, 1)); + } + + // check all to-be-visited nodes + while ( likely(toVisit.hasMore()) ) { + + const VisitEntry& e = toVisit.getNext(); + + if ( likely(e.depth <= maxDepth) ) { + + const Node* gnCur = e.gn; + for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { + const int nIdx = gnCur->getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + if ( unlikely(checked.find(&gnNext) == checked.end()) ) { + toVisit.add(VisitEntry(&gnNext, e.depth+1)); + checked.insert(&gnNext); + } } + } } - } + return checked; - return checked; - - } - -}; - - -/** - * get all grid nodes that are reachable within x-edges (depth) - * additionally returns the needed walking distance in meter - */ -template class ReachableByDepthWithDistanceSorted { - - struct VisitEntry { - const Node* gn; - int depth; - float dist_m; - int myIdx; - VisitEntry() {;} - VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) : - gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;} - }; - - struct Visits { - VisitEntry visits[1024];// __attribute__((aligned(16))); - size_t head = 0; - size_t tail = 0; - VisitEntry& getNext() { - return visits[tail++]; - } - void add(const VisitEntry& e) { - visits[head++] = e; - assert(head < 1024); - //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! - } - bool hasMore() const { - return head > tail; - } - void sort() { - const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { - return e1.dist_m < e2.dist_m; - }; - std::sort(&visits[tail], &visits[head], comp); - } - }; - - const Grid& grid; - -public: - - /** result */ - struct Entry { - - const Node* node; - const float walkDistToStart_m; - const int prevIdx; - - Entry(const Node* node, const float dist, const size_t prevIdx) : - node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;} - - bool hasPrev() const { - return prevIdx >= 0; } }; - ReachableByDepthWithDistanceSorted(const Grid& grid) : grid(grid) { - ; - } - /** get all nodes reachable from start using maxDepth steps */ - std::vector get(const Node& start, const int maxDepth) { + /** + * get all grid nodes that are reachable within x-edges (depth) + * additionally returns the needed walking distance in meter + */ + template class ReachableByDepthWithDistanceSorted { - std::unordered_set checked; - std::vector res; + struct VisitEntry { + const Node* gn; + int depth; + float dist_m; + int myIdx; + VisitEntry() {;} + VisitEntry(const Node* gn, const int depth, const float dist_m, const int myIdx) : + gn(gn), depth(depth), dist_m(dist_m), myIdx(myIdx) {;} + }; - Visits toVisit; - - // directly start with the node itself and all its neighbors - checked.insert(&start); - res.push_back(Entry(&start, 0, -1)); - for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { - const int nIdx = start.getNeighborIdx(i); - const Node& gnNext = grid[nIdx]; - const float dist_m = gnNext.getDistanceInMeter(start); - toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size())); - res.push_back(Entry(&gnNext, dist_m, 0)); - checked.insert(&gnNext); - } - toVisit.sort(); - - // check all to-be-visited nodes - while ( likely(toVisit.hasMore()) ) { - - const VisitEntry& e = toVisit.getNext(); - - if ( likely(e.depth <= maxDepth) ) { - - const Node* gnCur = e.gn; - -// for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { -// const int nIdx = gnCur->getNeighborIdx(i); -// const Node& gnNext = grid[nIdx]; -// if ( unlikely(checked.find(&gnNext) == checked.end()) ) { -// const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); -// const float dist_m = e.dist_m + nodeNodeDist_m; -// toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); -// res.push_back(Entry(&gnNext, dist_m, e.myIdx)); -// checked.insert(&gnNext); -// } -// } - -// const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01; - - std::vector sub; - - for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { - const int nIdx = gnCur->getNeighborIdx(i); - const Node& gnNext = grid[nIdx]; - if ( unlikely(checked.find(&gnNext) == checked.end()) ) { - const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); - const float dist_m = e.dist_m + nodeNodeDist_m; - //toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); - sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); - res.push_back(Entry(&gnNext, dist_m, e.myIdx)); - checked.insert(&gnNext); - - } - } - - // dijkstra.. sort the new nodes by destination to start - // only sorting the 8 new nodes seems enough due to the graph's layout + struct Visits { + VisitEntry visits[1024];// __attribute__((aligned(16))); + size_t head = 0; + size_t tail = 0; + VisitEntry& getNext() { + return visits[tail++]; + } + void add(const VisitEntry& e) { + visits[head++] = e; + assert(head < 1024); + //if (head >= 512) {throw std::runtime_error("too many visits");} / COSTLY AS HELL?! + } + bool hasMore() const { + return head > tail; + } + void sort() { const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { return e1.dist_m < e2.dist_m; }; + std::sort(&visits[tail], &visits[head], comp); + } + }; - std::sort(sub.begin(), sub.end(), comp); + const Grid& grid; + + public: + + /** result */ + struct Entry { + + const Node* node; + const float walkDistToStart_m; + const int prevIdx; + + Entry(const Node* node, const float dist, const size_t prevIdx) : + node(node), walkDistToStart_m(dist), prevIdx(prevIdx) {;} + + bool hasPrev() const { + return prevIdx >= 0; + } + + }; + + ReachableByDepthWithDistanceSorted(const Grid& grid) : grid(grid) { + ; + } + + /** get all nodes reachable from start using maxDepth steps */ + std::vector get(const Node& start, const int maxDepth) { + + std::unordered_set checked; + std::vector res; + + Visits toVisit; + + // directly start with the node itself and all its neighbors + checked.insert(&start); + res.push_back(Entry(&start, 0, -1)); + for (int i = 0; likely(i < start.getNumNeighbors()); ++i) { + const int nIdx = start.getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + const float dist_m = gnNext.getDistanceInMeter(start); + toVisit.add(VisitEntry(&gnNext, 1, dist_m, res.size())); + res.push_back(Entry(&gnNext, dist_m, 0)); + checked.insert(&gnNext); + } + toVisit.sort(); + + // check all to-be-visited nodes + while ( likely(toVisit.hasMore()) ) { + + const VisitEntry& e = toVisit.getNext(); + + if ( likely(e.depth <= maxDepth) ) { + + const Node* gnCur = e.gn; + + // for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { + // const int nIdx = gnCur->getNeighborIdx(i); + // const Node& gnNext = grid[nIdx]; + // if ( unlikely(checked.find(&gnNext) == checked.end()) ) { + // const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); + // const float dist_m = e.dist_m + nodeNodeDist_m; + // toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); + // res.push_back(Entry(&gnNext, dist_m, e.myIdx)); + // checked.insert(&gnNext); + // } + // } + + // const float gridSize_m = grid.getGridSize_cm() / 100 * 1.01; + + std::vector sub; + + for (int i = 0; likely(i < gnCur->getNumNeighbors()); ++i) { + const int nIdx = gnCur->getNeighborIdx(i); + const Node& gnNext = grid[nIdx]; + if ( unlikely(checked.find(&gnNext) == checked.end()) ) { + const float nodeNodeDist_m = gnCur->getDistanceInMeter(gnNext); + const float dist_m = e.dist_m + nodeNodeDist_m; + //toVisit.add(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); + sub.push_back(VisitEntry(&gnNext, e.depth+1, dist_m, res.size())); + res.push_back(Entry(&gnNext, dist_m, e.myIdx)); + checked.insert(&gnNext); + + } + } + + // dijkstra.. sort the new nodes by destination to start + // only sorting the 8 new nodes seems enough due to the graph's layout + const auto comp = [] (const VisitEntry& e1, const VisitEntry& e2) { + return e1.dist_m < e2.dist_m; + }; + + std::sort(sub.begin(), sub.end(), comp); + + for (const VisitEntry& e : sub) { + toVisit.add(e); + } + + } + + // slower with same result ;) + //toVisit.sort(); + + } + + return res; + + } + + }; + + + /** + * data-structure to track to-be-visited nodes + * push_back, pop_front + * as pop_front is costly, we omit the pop and use a head-index instead + * memory-consumption vs speed + */ + struct _ToVisit { + size_t nextIdx = 0; + std::vector vec; + _ToVisit() {vec.reserve(256);} + void add(const uint32_t nodeIdx) {vec.push_back(nodeIdx);} + uint32_t next() {return vec[nextIdx++];} + bool empty() const {return nextIdx >= vec.size();} + }; + + + + /** get a list of all nodes that are reachable after checking several conditions */ + template class ReachableByConditionUnsorted { + + + public: + + + static std::vector get(const Grid& grid, const Node& start, const Conditions cond) { + + //Node* curNode = nullptr; + std::unordered_set scheduled; + _ToVisit toVisit; + toVisit.add(start.getIdx()); + + std::vector res; + + while(!toVisit.empty()) { + + // get the next to-be-visited node + const uint32_t curIdx = toVisit.next(); //visit from inside out (needed for correct distance) + const Node& curNode = grid[curIdx]; + + // process current node + res.push_back(&curNode); + scheduled.insert(curIdx); + + // get all neighbors + const int numNeighbors = curNode.getNumNeighbors(); + for (int i = 0; i < numNeighbors; ++i) { + + const uint32_t neighborIdx = curNode.getNeighborIdx(i); + + // already visited? + if (scheduled.find(neighborIdx) != scheduled.end()) {continue;} + scheduled.insert(neighborIdx); + + // matches the used condition? + const Node& neighbor = grid[neighborIdx]; + if (!cond.visit(neighbor)) {continue;} + + // OK! + toVisit.add(neighborIdx); - for (const VisitEntry& e : sub) { - toVisit.add(e); } } - // slower with same result ;) - //toVisit.sort(); + // done + return res; } - return res; - } -}; + //const Node& next(const std::function& skip) { + //template const Node& next(const Skip skip) { + const Node& next() { + + + + } + + }; + } diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index c666042..c1a29f2 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -65,35 +65,63 @@ namespace GW3 { const GridPoint gpStart = Helper::p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); + // calculate a walk's probability + auto getP = [&] (const Point3 dst) { + double p = 1; + for (const WalkEvaluator* eval : evals) { + const double p1 = eval->getProbability(params.start, dst, params.start.getDistance(dst), params); + p *= p1; + } + return p; + }; + // include one additional grid-cell (increased distance) - const float secBuffer_m = (grid.getGridSize_cm() / 100.0f) + (params.distance_m * 0.1); - ReachableSettings set; - set.limitDistance = true; - set.dist_m = params.distance_m + secBuffer_m; - set.limitHeading = false; - set.heading = params.heading; - set.maxHeadingDiff_rad = M_PI/2; - const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); + //const float secBuffer_m = (grid.getGridSize_cm() * 2/ 100.0f);// + (params.distance_m * 0.1); + const float secBuffer_m = (grid.getGridSize_cm() * 1.15 / 100.0f);// + (params.distance_m * 0.15); + +// ReachableSettings set; +// set.limitDistance = true; +// set.dist_m = params.distance_m + secBuffer_m; +// set.limitHeading = false; +// set.heading = params.heading; +// set.maxHeadingDiff_rad = M_PI/2; + +// // get all nodes that satisfy above constraints +// const Nodes reachableNodes = Helper::getAllReachableNodes(grid, startNode, set); + + struct Cond { + const float maxDist_m; + const Node* startNode; + Cond(float maxDist_m, const Node* startNode) : maxDist_m(maxDist_m), startNode(startNode) {;} + bool visit(const Node& n) const { + return (startNode->getDistanceInMeter(n)) < maxDist_m; + } + }; + Cond cond(params.distance_m+secBuffer_m, startNode); + std::vector reachableNodes = ReachableByConditionUnsorted::get(grid, *startNode, cond); WalkResult res; res.heading = params.heading; res.position = params.start; - + // get the to-be-reached destination's position (using start+distance+heading) const Point2 dir = res.heading.asVector(); const Point2 dst = params.start.xy() + (dir * params.distance_m); - // is dst reachable? + // is above destination reachable? const Node* n = Helper::contains(grid, reachableNodes, dst); + //const Node* n = ri.contains(dst); if (n) { const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); const GridPoint gp = Helper::p3ToGp(p3); + + if (grid.hasNodeFor(gp)) { res.position = p3; // update position //res.heading; // keep as-is - //res.probability; // keep as-is + res.probability *= getP(p3); // keep as-is return res; // done } else { @@ -129,7 +157,7 @@ namespace GW3 { } res.heading = Heading(start.xy(), end.xy()); - res.probability = p; + res.probability *= getP(end); res.position = end; return res; diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index 849907f..7bfb885 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -37,14 +37,22 @@ private: Distribution::Normal dY = Distribution::Normal(0, 0.3); Distribution::Normal dZ = Distribution::Normal(0, 0.4); + int stepPatternPos = -1; std::vector listeners; + //float stepSize_m; + //float stepSizeSigma_m; + float noiseLevel; + Distribution::Normal dNextStep; + public: /** ctor with the walker to follow */ - SyntheticSteps(SyntheticWalker* walker) { + SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) : + //stepSize_m(stepSize_m), drift(drift), stepSizeSigma_m(stepSizeSigma_m), + noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m) { walker->addListener(this); dX.setSeed(1); @@ -82,15 +90,15 @@ protected: void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { (void) curPos; - const float nextStepAt = (lastStepAtDistance + stepSize_m); + const float nextStepAt = lastStepAtDistance + dNextStep.draw(); // 1st, start with random noise on the accelerometer const float x = dX.draw(); const float y = dY.draw(); const float z = dZ.draw(); - const AccelerometerData base(0, 4, 9.7); - const AccelerometerData noise(x, y, z); - AccelerometerData acc = base + noise; + const AccelerometerData aBase(0, 4, 9.7); + const AccelerometerData aNoise(x, y, z); + AccelerometerData acc = aBase + aNoise * noiseLevel; // is it time to inject a "step" into the accelerometer data? if (walkedDistance > nextStepAt) { @@ -105,7 +113,7 @@ protected: refStepPattern = Timestamp::fromMS(0); } else { const AccelerometerData step = stepPattern.get(curPatPos); - acc = base + noise*2.5f + step; + acc = aBase + (aNoise * noiseLevel) + step; } } diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h index f94db0c..d9d8ef8 100644 --- a/synthetic/SyntheticTurns.h +++ b/synthetic/SyntheticTurns.h @@ -39,17 +39,23 @@ private: Distribution::Normal dMaxChange = Distribution::Normal(0.011, 0.003); Distribution::Normal dChange = Distribution::Normal(1.0, 0.25); - Distribution::Normal dHeadErr = Distribution::Normal(0.15, 0.10); // heading error, slightly biased Distribution::Uniform dRadDiff = Distribution::Uniform(40,100); + //float headingDrift_rad; + //float headingSigma_rad; + float noiseLevel; + Distribution::Normal dHeadErr; std::vector listeners; public: /** ctor with the walker to follow */ - SyntheticTurns(SyntheticWalker* walker) { + SyntheticTurns(SyntheticWalker* walker, const float headingDrift_rad = 0, const float headingSigma_rad = 0, const float noiseLevel = 0) : + //headingDrift_rad(headingDrift_rad), headingSigma_rad(headingSigma_rad), + noiseLevel(noiseLevel + 0.00001f), dHeadErr(headingDrift_rad, headingSigma_rad) { + walker->addListener(this); dAccX.setSeed(1); dAccY.setSeed(3); @@ -57,6 +63,7 @@ public: dGyroX.setSeed(7); dGyroY.setSeed(9); dGyroZ.setSeed(11); + } /** attach a listener to this provider */ @@ -116,14 +123,14 @@ protected: // convert to gyro's radians-per-second const double radPerSec = change * 1000 / deltaTs.ms();; - const float accX = 0.00 + dAccX.draw(); - const float accY = 0.00 + dAccY.draw(); - const float accZ = 9.81 + dAccZ.draw(); + const float accX = 0.00 + dAccX.draw() * (noiseLevel); + const float accY = 0.00 + dAccY.draw() * (noiseLevel); + const float accZ = 9.81 + dAccZ.draw() * (noiseLevel); AccelerometerData acc(accX, accY, accZ); - const float gyroX = dGyroX.draw(); - const float gyroY = dGyroY.draw(); - const float gyroZ = dGyroZ.draw() + radPerSec; + const float gyroX = dGyroX.draw() * (noiseLevel); + const float gyroY = dGyroY.draw() * (noiseLevel); + const float gyroZ = dGyroZ.draw() * (noiseLevel) + radPerSec; GyroscopeData gyro(gyroX, gyroY, gyroZ); for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);} From ade2425fbd9818e80af6ec737d31717482d73cde Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 6 Dec 2017 17:09:54 +0100 Subject: [PATCH 05/12] minor code changes --- math/distribution/Normal.h | 5 ++++ sensors/offline/FilePlayer.h | 58 +++++++++++++++++++++++++----------- 2 files changed, 45 insertions(+), 18 deletions(-) diff --git a/math/distribution/Normal.h b/math/distribution/Normal.h index b40ce67..f822319 100644 --- a/math/distribution/Normal.h +++ b/math/distribution/Normal.h @@ -26,8 +26,13 @@ namespace Distribution { Normal(const T mu, const T sigma) : mu(mu), sigma(sigma), _a(1.0 / (sigma * std::sqrt(2.0 * M_PI))), gen(RANDOM_SEED), dist(mu,sigma) { +#warning "analyze issue when coping an existing distribution and using draw() afterwards. this seems to yield issues" + } + /** do not allow copy. this will not work as expected for std::normal_distribution when using draw() ?! */ + //Normal(const Normal& o) = delete; + /** get probability for the given value */ T getProbability(const T val) const { const T b = -0.5 * ((val-mu)/sigma) * ((val-mu)/sigma); diff --git a/sensors/offline/FilePlayer.h b/sensors/offline/FilePlayer.h index b257b42..b74ef22 100644 --- a/sensors/offline/FilePlayer.h +++ b/sensors/offline/FilePlayer.h @@ -75,13 +75,25 @@ namespace Offline { thread.join(); } + + /** manual ticking */ + int tickPos = 0; + void tick() { + const std::vector& events = reader->getEntries(); + const Entry& e = events[tickPos]; + trigger(Timestamp::fromMS(e.ts), e); + ++tickPos; + } + private: + + /** background loop */ void loop() { // get all sensor events from the offline file - const std::vector events = reader->getEntries(); + const std::vector& events = reader->getEntries(); // reference time (system vs. first-event) Timestamp tsRef1 = Timestamp::fromMS(events.front().ts); @@ -104,23 +116,7 @@ namespace Offline { if (diff.ms() > 0) {std::this_thread::sleep_for(std::chrono::milliseconds(diff.ms()));} } - // event index - const size_t idx = e.idx; - -#warning "some sensors todo:" - switch(e.type) { - case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break; - case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break; - case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break; - case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break; - case Sensor::MAGNETOMETER: listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break; - case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break; - case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break; - case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break; - case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break; - case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break; - default: throw Exception("code error. found not-yet-implemented sensor"); - } + trigger(ts, e); } @@ -129,8 +125,34 @@ namespace Offline { } + + + void trigger(const Timestamp ts, const Entry& e) { + + const int idx = e.idx; + + #warning "some sensors todo:" + switch(e.type) { + case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break; + case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break; + case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break; + case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break; + case Sensor::MAGNETOMETER: listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break; + case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break; + case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break; + case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break; + case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break; + case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break; + default: throw Exception("code error. found not-yet-implemented sensor"); + } + + } + + }; + + } #endif // FILEPLAYER_H From 1114331fd250c659520fc50b477c479a026364c7 Mon Sep 17 00:00:00 2001 From: frank Date: Wed, 13 Dec 2017 13:25:53 +0100 Subject: [PATCH 06/12] worked on synthetic sensor data --- synthetic/SyntheticPath.h | 17 +++++++++++++++++ synthetic/SyntheticSteps.h | 17 +++++++++++------ synthetic/SyntheticTurns.h | 2 +- synthetic/SyntheticWalker.h | 11 +++++++++-- 4 files changed, 38 insertions(+), 9 deletions(-) diff --git a/synthetic/SyntheticPath.h b/synthetic/SyntheticPath.h index fd50715..f194729 100644 --- a/synthetic/SyntheticPath.h +++ b/synthetic/SyntheticPath.h @@ -4,18 +4,23 @@ #include "../math/Interpolator.h" #include "../floorplan/v2/Floorplan.h" #include "../floorplan/v2/FloorplanHelper.h" +#include "../floorplan/v2/FloorplanHelper.h" /** allows interpolation along a synthetic path */ class SyntheticPath : private Interpolator { using Base = Interpolator; using Entry = Base::InterpolatorEntry; + const Floorplan::IndoorMap* map; + public: /** create path using the given ground-truth points from the map */ void create(const Floorplan::IndoorMap* map, std::vector ids) { + this->map = map; + // get all ground-truth points from the map auto gtps = FloorplanHelper::getGroundTruthPoints(map); float dist = 0; @@ -38,6 +43,8 @@ public: return Base::getEntries(); } + + /** smooth harsh angles */ void smooth(float delta = 1, int numRuns = 1) { @@ -104,6 +111,16 @@ public: return Base::get(distance); } + /** is the given position part of a floor (or in the air) */ + bool isOnFloor(const float distance) const { + const Point3 pos = getPosAfterDistance(distance); + for (const Floorplan::Floor* floor : map->floors) { + const float delta = std::abs(floor->atHeight - pos.z); + if (delta < 0.1) {return true;} + } + return false; + } + }; #endif // INDOOR_SYNTEHTICPATH_H diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index 7bfb885..ee0ad51 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -25,8 +25,11 @@ private: /** the walker to listen to */ SyntheticWalker* walker; - /** the pedestrian's step-size (in meter) */ - float stepSize_m = 0.7; + ///** the pedestrian's step-size (in meter) */ + //float stepSize_m = 0; + + ///** when walking stairs, the step size is much smaller */ + //float stepSizeStair_m = 0; float lastStepAtDistance = 0; @@ -46,13 +49,14 @@ private: //float stepSizeSigma_m; float noiseLevel; Distribution::Normal dNextStep; + Distribution::Normal dNextStepStair; public: /** ctor with the walker to follow */ - SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) : + SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeStair_m = 0.3, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) : //stepSize_m(stepSize_m), drift(drift), stepSizeSigma_m(stepSizeSigma_m), - noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m) { + noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m), dNextStepStair(stepSizeStair_m, stepSizeSigma_m) { walker->addListener(this); dX.setSeed(1); @@ -87,10 +91,11 @@ public: protected: - void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { + void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override { (void) curPos; - const float nextStepAt = lastStepAtDistance + dNextStep.draw(); + const float distAdd = (type == SyntheticWalker::Type::FLOOR) ? (dNextStep.draw()) : (dNextStepStair.draw()); + const float nextStepAt = lastStepAtDistance + distAdd; // 1st, start with random noise on the accelerometer const float x = dX.draw(); diff --git a/synthetic/SyntheticTurns.h b/synthetic/SyntheticTurns.h index d9d8ef8..ba8beb0 100644 --- a/synthetic/SyntheticTurns.h +++ b/synthetic/SyntheticTurns.h @@ -85,7 +85,7 @@ protected: return val; } - void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override { + void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos, const SyntheticWalker::Type type) override { // time sine last onWalk(); if (lastTs.isZero()) {lastTs = walkedTime; return;} diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h index 2bb5c1e..0e290c8 100644 --- a/synthetic/SyntheticWalker.h +++ b/synthetic/SyntheticWalker.h @@ -8,9 +8,14 @@ class SyntheticWalker { public: + enum class Type { + FLOOR, + NON_FLOOR, + }; + class Listener { public: - virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos) = 0; + virtual void onWalk(Timestamp walkedTime, float walkedDistance, const Point3 curPos, const Type type) = 0; }; private: @@ -55,11 +60,13 @@ public: // get the current position along the path const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance); + const bool isOnFloor = path.isOnFloor(this->walkedDistance); + const Type type = (isOnFloor) ? (Type::FLOOR) : (Type::NON_FLOOR); Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() ); // inform listener - for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath);} + for (Listener* l : listeners) {l->onWalk(walkedTime, walkedDistance, curPosOnPath, type);} return curPosOnPath; From d48b0b8fd4b967ed117df0d230b8c27b5831e1ac Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 13 Dec 2017 16:37:51 +0100 Subject: [PATCH 07/12] minor changes to grid walking --- grid/Grid.h | 12 ++++++++++-- grid/GridNode.h | 1 + grid/walk/v3/Walker.h | 12 +++++++----- sensors/imu/StepDetection.h | 4 ++-- synthetic/SyntheticPath.h | 14 ++++++-------- synthetic/SyntheticSteps.h | 18 +++++++++++++----- synthetic/SyntheticWalker.h | 4 ++-- 7 files changed, 41 insertions(+), 24 deletions(-) diff --git a/grid/Grid.h b/grid/Grid.h index aa4da03..259cb52 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -213,6 +213,14 @@ public: return GridNodeBBox(node, gridSize_cm); } + /** is this node part of a non-plain stair/escalator */ + bool isPlain(const T& n1) const { + for (const T& n2 : neighbors(n1)) { + if (n2.z_cm != n1.z_cm) {return false;} + } + return true; + } + /** * get an UID for the given point. * this works only for aligned points. @@ -469,11 +477,11 @@ public: - NeighborForEach neighbors(const int idx) { + NeighborForEach neighbors(const int idx) const { return neighbors(nodes[idx]); } - NeighborForEach neighbors(const T& node) { + NeighborForEach neighbors(const T& node) const { return NeighborForEach(*this, node._idx); } diff --git a/grid/GridNode.h b/grid/GridNode.h index 306105c..8b73500 100755 --- a/grid/GridNode.h +++ b/grid/GridNode.h @@ -87,6 +87,7 @@ public: /** set the node's semantic type */ void setType(const uint8_t type) {this->_type = type;} + // /** get the n-th neighbor for this node */ // template inline T& getNeighbor(const int nth, const Grid& grid) const { // return grid.getNeighbor(_idx, nth); diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index c1a29f2..bee1178 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -121,7 +121,7 @@ namespace GW3 { if (grid.hasNodeFor(gp)) { res.position = p3; // update position //res.heading; // keep as-is - res.probability *= getP(p3); // keep as-is + res.probability *= 1;//getP(p3); // keep as-is return res; // done } else { @@ -157,7 +157,7 @@ namespace GW3 { } res.heading = Heading(start.xy(), end.xy()); - res.probability *= getP(end); + res.probability *= 0.1;//getP(end); res.position = end; return res; @@ -237,7 +237,7 @@ namespace GW3 { int numVisitedNodes = 0; - #define MODE 3 + #define MODE 2 #if (MODE == 1) @@ -249,9 +249,10 @@ namespace GW3 { for (const Node* dstNode : nodes) { const Point3 nodeCenter = Helper::gpToP3(*dstNode); + const float walkDist_m = nodeCenter.getDistance(start);//*1.05; double p = 1.0; for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, nodeCenter, params); + const double p1 = eval->getProbability(start, nodeCenter, walkDist_m, params); p *= p1; } if (p > bestNodeP) { @@ -284,10 +285,11 @@ namespace GW3 { // destination = nodeCenter + offset (within the node's bbox, (x,y) only! keep z as-is) const Point3 end(nodeCenter.x + ox, nodeCenter.y + oy, nodeCenter.z); + const float walkDist_m = end.getDistance(start);//*1.05; double p = 1; for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, end, params); + const double p1 = eval->getProbability(start, end, walkDist_m, params); p *= p1; } diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index 68edfbb..e0e7b74 100644 --- a/sensors/imu/StepDetection.h +++ b/sensors/imu/StepDetection.h @@ -36,7 +36,7 @@ private: Timestamp blockUntil; bool waitForUp = false; - const Timestamp blockTime = Timestamp::fromMS(250); // 150-250 looks good + const Timestamp blockTime; // 150-250 looks good const float upperThreshold = +0.4*0.6f; // + is usually smaller than down (look at graphs) const float lowerThreshold = -1.5*0.6f; // the 0.8 is for testing! @@ -57,7 +57,7 @@ private: public: /** ctor */ - StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) { + StepDetection(const Timestamp blockTime = Timestamp::fromMS(200)) : blockTime(blockTime), avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) { #ifdef WITH_DEBUG_PLOT gp << "set autoscale xfix\n"; diff --git a/synthetic/SyntheticPath.h b/synthetic/SyntheticPath.h index f194729..f173852 100644 --- a/synthetic/SyntheticPath.h +++ b/synthetic/SyntheticPath.h @@ -111,14 +111,12 @@ public: return Base::get(distance); } - /** is the given position part of a floor (or in the air) */ - bool isOnFloor(const float distance) const { - const Point3 pos = getPosAfterDistance(distance); - for (const Floorplan::Floor* floor : map->floors) { - const float delta = std::abs(floor->atHeight - pos.z); - if (delta < 0.1) {return true;} - } - return false; + /** at the given distance: are we walking on a plain surface or up/down? */ + bool isPlain(const float distance) const { + const Point3 pos1 = getPosAfterDistance(distance); + const Point3 pos2 = getPosAfterDistance(distance + 0.1); + const float delta = std::abs(pos1.z - pos2.z); + return delta < 0.01; } }; diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index ee0ad51..a24efd7 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -34,7 +34,8 @@ private: float lastStepAtDistance = 0; Timestamp refStepPattern; - Interpolator stepPattern; + Interpolator stepPatternPlain; + Interpolator stepPatternStair; Distribution::Normal dX = Distribution::Normal(0, 0.2); Distribution::Normal dY = Distribution::Normal(0, 0.3); @@ -76,10 +77,16 @@ public: // AccelerometerData acc(x,y,z); // stepPattern.add(Timestamp::fromMS(i), acc); // } - stepPattern.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0)); - stepPattern.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3)); - stepPattern.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8)); - stepPattern.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0)); + + stepPatternPlain.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0)); + stepPatternPlain.add(Timestamp::fromMS(250), AccelerometerData(0, 0.6, 3)); + stepPatternPlain.add(Timestamp::fromMS(350), AccelerometerData(0.5, -0.6, -1.8)); + stepPatternPlain.add(Timestamp::fromMS(450), AccelerometerData(0, 0, 0)); + + stepPatternStair.add(Timestamp::fromMS(0), AccelerometerData(0, 0, 0)); + stepPatternStair.add(Timestamp::fromMS(200), AccelerometerData(0, 0.6, 4)); + stepPatternStair.add(Timestamp::fromMS(300), AccelerometerData(0.5, -0.6, -3.5)); + stepPatternStair.add(Timestamp::fromMS(350), AccelerometerData(0, 0, 0)); } @@ -95,6 +102,7 @@ protected: (void) curPos; const float distAdd = (type == SyntheticWalker::Type::FLOOR) ? (dNextStep.draw()) : (dNextStepStair.draw()); + const auto stepPattern = (type == SyntheticWalker::Type::FLOOR) ? (stepPatternPlain) : (stepPatternStair); const float nextStepAt = lastStepAtDistance + distAdd; // 1st, start with random noise on the accelerometer diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h index 0e290c8..b4fadfb 100644 --- a/synthetic/SyntheticWalker.h +++ b/synthetic/SyntheticWalker.h @@ -60,8 +60,8 @@ public: // get the current position along the path const Point3 curPosOnPath = path.getPosAfterDistance(this->walkedDistance); - const bool isOnFloor = path.isOnFloor(this->walkedDistance); - const Type type = (isOnFloor) ? (Type::FLOOR) : (Type::NON_FLOOR); + const bool isPlainPart = path.isPlain(this->walkedDistance); + const Type type = (isPlainPart) ? (Type::FLOOR) : (Type::NON_FLOOR); Log::add(name, "walkTime: " + std::to_string(walkedTime.sec()) + " walkDistance: " + std::to_string(walkedDistance) + " -> " + curPosOnPath.asString() ); From c346b7f2229ef7401b3bb3b52ef355ac53d0f7cd Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 20 Dec 2017 17:12:30 +0100 Subject: [PATCH 08/12] started testing a new grid-builder minor fixes worked on walkers --- grid/Grid.h | 18 +- grid/factory/v3/GridFactory3.h | 473 +++++++++++++++++++++++++++++++++ grid/factory/v3/HelperPoly3.h | 101 +++++++ grid/walk/v3/Helper.h | 9 +- grid/walk/v3/Structs.h | 39 ++- grid/walk/v3/WalkEvaluator.h | 52 ++-- grid/walk/v3/Walker.h | 57 +++- 7 files changed, 709 insertions(+), 40 deletions(-) create mode 100644 grid/factory/v3/GridFactory3.h create mode 100644 grid/factory/v3/HelperPoly3.h diff --git a/grid/Grid.h b/grid/Grid.h index 259cb52..4340e8b 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -177,10 +177,11 @@ public: } /** get the center-node the given Point belongs to */ - const T& getNodeFor(const GridPoint& p) { - const UID uid = getUID(p); - Assert::isTrue(hashes.find(uid) != hashes.end(), "element not found!"); - return nodes[hashes[uid]]; + const T& getNodeFor(const GridPoint& p) const { + //const UID uid = getUID(p); + auto it = hashes.find(getUID(p)); + Assert::isTrue(it != hashes.end(), "element not found!"); + return nodes[it->second]; } /** get the center-node the given Point belongs to. or nullptr if not present */ @@ -249,11 +250,11 @@ public: inline int idxX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm);} inline int idxY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm);} - inline int idxZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm);} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes + inline int idxZ(const int z_cm) const {return std::round(z_cm / 20.0f);} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes inline int snapX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm) * gridSize_cm;} inline int snapY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm) * gridSize_cm;} - inline int snapZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm) * gridSize_cm;} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes + inline int snapZ(const int z_cm) const {return std::round(z_cm / 20.0f) * 20;} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes /** array access */ @@ -295,6 +296,11 @@ public: } } + /** convert to a GridPoint coordinate (in cm) */ + GridPoint toGridPoint(const Point3 pos_m) const { + return GridPoint( snapX(pos_m.x*100), snapY(pos_m.y*100), snapZ(pos_m.z*100) ); + } + /** remove the given array-index by moving all follwing elements down by one */ template void arrayRemove(X* arr, const int idxToRemove, const int arrayLen) { for (int i = idxToRemove+1; i < arrayLen; ++i) { diff --git a/grid/factory/v3/GridFactory3.h b/grid/factory/v3/GridFactory3.h new file mode 100644 index 0000000..cd68c86 --- /dev/null +++ b/grid/factory/v3/GridFactory3.h @@ -0,0 +1,473 @@ +#ifndef GRIDFACTORY3_H +#define GRIDFACTORY3_H + +#include "../../Grid.h" +#include "../../../floorplan/v2/Floorplan.h" +#include "HelperPoly3.h" + + +template class GridFactory3 { + +private: + + + Grid& grid; + const int gs_cm; + + struct NewNode { + GridPoint pos; + int type; + NewNode(const GridPoint pos, const int type) : pos(pos), type(type) {;} + bool operator == (const NewNode& o) const {return o.pos == pos;} + }; + +public: + + GridFactory3(Grid& grid) : grid(grid), gs_cm(grid.getGridSize_cm()) { + + } + + void build(const Floorplan::IndoorMap* map) { + + std::vector add; + std::vector rem; + + for (const Floorplan::Floor* floor : map->floors) { + +// for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) { + +// const std::vector pts = getPointsOn(floor, *poly); +// if (poly->method == Floorplan::OutlineMethod::ADD) { +// add.insert(add.end(), pts.begin(), pts.end()); +// } else { +// rem.insert(rem.end(), pts.begin(), pts.end()); +// } + +// } + + const std::vector pts = getPointsOn(floor); + add.insert(add.end(), pts.begin(), pts.end()); + + for (const Floorplan::Stair* stair : floor->stairs) { + std::vector quads = Floorplan::getQuads(stair->getParts(), floor); + const std::vector pts = getPointsOn(floor, quads); + add.insert(add.end(), pts.begin(), pts.end()); + } + + + } + + for (const NewNode& nn : add) { + auto it = std::find(rem.begin(), rem.end(), nn); + if (it == rem.end()) { + if (!grid.hasNodeFor(nn.pos)) { + Node n(nn.pos.x_cm, nn.pos.y_cm, nn.pos.z_cm); + n.setType(nn.type); + grid.add(n); + } + } + } + + connect(map); + removeIsolatedNodes(); + + } + + bool isBlocked(const Floorplan::IndoorMap* map, const Node& n1, const Node& n2) { + + Line2 lNodes(n1.inMeter().xy(), n2.inMeter().xy()); + + for (Floorplan::Floor* floor : map->floors) { + + if (n1.inMeter().z != floor->atHeight) {continue;} + if (n2.inMeter().z != floor->atHeight) {continue;} + + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + if (line) { + const std::vector lines = getThickLines(line); + for (const Line2& lObs : lines) { + if (lObs.getSegmentIntersection(lNodes)) { + return true; + } + } + } + } + } + + return false; + + } + + /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ + static std::vector getThickLines(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 + return { + Line2(p1, p2), + Line2(p3, p4), + Line2(p2, p4), + Line2(p1, p3), + }; + } + + void connect(const Floorplan::IndoorMap* map) { + + for (Node& n1 : grid) { + for (Node& n2 : grid) { + + if (n1 == n2) {continue;} + + + if ( + (n1.getType() == GridNode::TYPE_STAIR && n2.getType() == GridNode::TYPE_FLOOR) || + (n2.getType() == GridNode::TYPE_STAIR && n1.getType() == GridNode::TYPE_FLOOR) + ) { + + 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 (n1.fullyConnected()) {continue;} + if (n2.fullyConnected()) {continue;} + grid.connectUniDir(n1, n2); + } + + } 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.fullyConnected()) {continue;} + if (n2.fullyConnected()) {continue;} + grid.connectUniDir(n1, n2); + } + } 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 (n1.fullyConnected()) {continue;} + if (n2.fullyConnected()) {continue;} + grid.connectUniDir(n1, n2); + } + + } + + +// if (n1.getDistanceInCM(n2) < gs_cm * 1.7 && !isBlocked(map, n1, n2)) { +// if (n1.fullyConnected()) {continue;} +// if (n2.fullyConnected()) {continue;} +// grid.connectUniDir(n1, n2); +// } + + } + } + + } + + + + + + + + + + + + + + + + + /** recursively get all connected nodes and add them to the set */ + void getConnected(Node& n1, std::unordered_set& visited) { + + std::unordered_set toVisit; + toVisit.insert(n1.getIdx()); + + // run while there are new nodes to visit + while(!toVisit.empty()) { + + // get the next node + int nextIdx = *toVisit.begin(); + toVisit.erase(nextIdx); + visited.insert(nextIdx); + Node& next = grid[nextIdx]; + + // get all his (unprocessed) neighbors and add them to the region + for (const Node& n2 : grid.neighbors(next)) { + if (visited.find(n2.getIdx()) == visited.end()) { + toVisit.insert(n2.getIdx()); + } + } + + } + + } + + void removeIsolatedNodes() { + + //std::cout << "todo: remove" << std::endl; + //return; + + // try to start at the first stair + for (Node& n : grid) { + if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;} + } + + // no stair found? try to start at the first node + removeIsolatedNodes(grid[0]); + + } + + /** remove all nodes not connected to n1 */ + void removeIsolatedNodes(Node& n1) { + + // get the connected region around n1 + //Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false); + //Log::tick(); + std::unordered_set set; + getConnected(n1, set); + //Log::tock(); + + //const int numToRemove = grid.getNumNodes() - set.size(); + //int numRemoved = 0; + + // remove all other + //Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false); + //Log::tick(); + for (Node& n2 : grid) { + if (set.find(n2.getIdx()) == set.end()) { + + // sanity check + // wouldn't make sense that a stair-node is removed.. + // maybe something went wrong elsewhere??? + Assert::notEqual(n2.getType(), GridNode::TYPE_STAIR, "detected an isolated stair?!"); + Assert::notEqual(n2.getType(), GridNode::TYPE_ELEVATOR, "detected an isolated elevator?!"); + //Assert::notEqual(n2.getType(), GridNode::TYPE_DOOR, "detected an isolated door?!"); + + // proceed ;) + grid.remove(n2); + + //++numRemoved; + //std::cout << numRemoved << ":" << numToRemove << std::endl; + + } + } + //Log::tock(); + + // clean the grid (physically delete the removed nodes) + grid.cleanup(); + + } + + +// std::vector getPointsOn(const Floorplan::Floor* floor, const Floorplan::FloorOutlinePolygon& poly) { + +// std::vector res; + +// BBox2 bbox; +// for (Point2 pt : poly.poly.points) {bbox.add(pt);} + +// int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm; +// int x2 = std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm; + +// int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm; +// int y2 = std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm; + +// int z = floor->atHeight * 100; + +// for (int y = y1; y <= y2; y += gs_cm) { +// for (int x = x1; x <= x2; x += gs_cm) { + +// GridPoint gp(x, y, z); +// int type = poly.outdoor ? GridNode::TYPE_OUTDOOR : GridNode::TYPE_FLOOR; +// res.push_back(NewNode(gp, type)); + +// } +// } + +// return res; + +// } + + + + std::vector getPointsOn(const Floorplan::Floor* floor) { + + std::vector res; + + BBox2 bbox; + for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) { + for (Point2 pt : poly->poly.points) {bbox.add(pt);} + } + + int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm; + int x2 = std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm; + + int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm; + int y2 = std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm; + + int z = floor->atHeight * 100; + + struct Combo { + HelperPoly3 poly; + const Floorplan::FloorOutlinePolygon* orig; + Combo(HelperPoly3 poly, const Floorplan::FloorOutlinePolygon* orig) : poly(poly), orig(orig) {;} + }; + + std::vector polygons; + for (const Floorplan::FloorOutlinePolygon* poly : floor->outline) { + HelperPoly3 pol(*poly); + polygons.push_back(Combo(pol, poly)); + } + + for (int y = y1; y <= y2; y += gs_cm) { + for (int x = x1; x <= x2; x += gs_cm) { + + int type = GridNode::TYPE_FLOOR; + bool remove = false; + bool add = false; + + for (const Combo& c : polygons) { + if (c.poly.contains(Point2(x,y))) { + if (c.orig->method == Floorplan::OutlineMethod::ADD) {add = true;} + if (c.orig->method == Floorplan::OutlineMethod::REMOVE) {remove = true; break;} + if (c.orig->outdoor) {type = GridNode::TYPE_OUTDOOR;} + } + } + + if (add && !remove) { + GridPoint gp(x, y, z); + res.push_back(NewNode(gp, type)); + } + + } + } + + return res; + + } + + + + // + + // const std::vector pts = getPointsOn(floor, *poly); + // if (poly->method == Floorplan::OutlineMethod::ADD) { + // add.insert(add.end(), pts.begin(), pts.end()); + // } else { + // rem.insert(rem.end(), pts.begin(), pts.end()); + // } + + // } + + static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) { + const Point2 v0 = b - a, v1 = c - a, v2 = p - a; + double d00 = dot(v0, v0); + double d01 = dot(v0, v1); + double d11 = dot(v1, v1); + double d20 = dot(v2, v0); + double d21 = dot(v2, v1); + double denom = d00 * d11 - d01 * d01; + v = (d11 * d20 - d01 * d21) / denom; + w = (d00 * d21 - d01 * d20) / denom; + u = 1.0f - v - w; + return (u <= 1 && v <= 1 && w <= 1) && (u >= 0 && v >= 0 && w >= 0); + } + +// void isBlocked(const GridPoint& gp) { +// for (Floorplan::Floor* floor : map->floors) { +// for (Floorplan::FloorObstacle* obs : floor->obstacles) { +// Floorplan::FloorObstacleLine* line = dynamic_cast(obs); +// if (line) { +// line-> +// } +// } +// } +// } + + std::vector getPointsOn(const Floorplan::Floor* floor, const std::vector& quads) { + + std::vector res; + + // whole stair + BBox3 bboxStair; + for (const Floorplan::Quad3& quad : quads) { + bboxStair.add(quad.p1); + bboxStair.add(quad.p2); + bboxStair.add(quad.p3); + bboxStair.add(quad.p4); + } + + // stair's starting and ending z (must be connected to a floor) + //int z1 = grid.snapZ( (floor->atHeight) * 100 ); + // + int z2 = grid.snapZ( (floor->atHeight + bboxStair.getMax().z) * 100 ); + + // one quad + for (const Floorplan::Quad3& quad : quads) { + + BBox3 bbox; + bbox.add(quad.p1); + bbox.add(quad.p2); + bbox.add(quad.p3); + bbox.add(quad.p4); + + + int x1 = std::floor(bbox.getMin().x * 100 / gs_cm) * gs_cm; + int x2 = std::ceil(bbox.getMax().x * 100 / gs_cm) * gs_cm; + + int y1 = std::floor(bbox.getMin().y * 100 / gs_cm) * gs_cm; + int y2 = std::ceil(bbox.getMax().y * 100 / gs_cm) * gs_cm; + + //int zFloor = floor->atHeight * 100; + + for (int y = y1; y <= y2; y += gs_cm) { + for (int x = x1; x <= x2; x += gs_cm) { + + int z = 0; + Point2 p(x/100.0f, y/100.0f); + + float u,v,w; + if (bary(p, quad.p1.xy(), quad.p2.xy(), quad.p3.xy(), u, v, w)) { + z = (quad.p1.z*u + quad.p2.z*v + quad.p3.z*w) * 100; + } else if (bary(p, quad.p1.xy(), quad.p3.xy(), quad.p4.xy(), u, v, w)) { + z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100; + } else { + // outside of the quad -> skip + //z = (quad.p1.z*u + quad.p3.z*v + quad.p4.z*w) * 100; + continue; + + //z = zFloor + ( + // (quad.p1.z*u + quad.p2.z*v + quad.p3.z*w) + // ) * 100; + + } + + //z = grid.snapZ(z); + + const GridPoint gp(x, y, z); + const int type = GridNode::TYPE_STAIR; + res.push_back(NewNode(gp, type)); + + } + } + + } + + // scale to ensure starting at floor, and ending at floor + + return res; + + } + +}; + +#endif // GRIDFACTORY3_H diff --git a/grid/factory/v3/HelperPoly3.h b/grid/factory/v3/HelperPoly3.h new file mode 100644 index 0000000..652159d --- /dev/null +++ b/grid/factory/v3/HelperPoly3.h @@ -0,0 +1,101 @@ +#ifndef HELPERPOLY3_H +#define HELPERPOLY3_H + +#include "../../../geo/Point2.h" +#include "../../../geo/Point3.h" +#include "../../../geo/BBox2.h" +#include "../../../geo/BBox3.h" +#include "../../../floorplan/v2/Floorplan.h" +#include "../../../grid/Grid.h" + +/** helper class for polygon methods */ +struct HelperPoly3 { + + BBox2 bbox_cm; + std::vector points_cm; + + /** empty ctor */ + HelperPoly3() { + ; + } + + /** ctor from floorplan-polygon */ + HelperPoly3(const Floorplan::FloorOutlinePolygon& poly) { + for (Point2 p : poly.poly.points) { add(p * 100); } + } + + /** ctor from floorplan-quad */ + HelperPoly3(const Floorplan::Quad3& quad) { + add(quad.p1*100); add(quad.p2*100); add(quad.p3*100); add(quad.p4*100); + } + + /** ctor from floorplan-polygon */ + HelperPoly3(const Floorplan::Polygon2& poly) { + for (Point2 p : poly.points) { add(p * 100); } + } + + void add(const Point2 p) { + points_cm.push_back(p); + bbox_cm.add(p); + } + + void add(const Point3& p) { + points_cm.push_back(p.xy()); + bbox_cm.add(p.xy()); + } + + /** does the polygon contain the given point (in cm)? */ + bool contains(const Point2 p_cm) const { + + // not within bbox? -> not within polygon + if (!bbox_cm.contains(p_cm)) {return false;} + + // ensure the point is at least a bit outside of the polygon + const float x1_cm = bbox_cm.getMin().x - 17.71920; + const float y1_cm = bbox_cm.getMin().y - 23.10923891; + + // construct line between point outside of the polygon and the point in question + const Line2 l(x1_cm, y1_cm, p_cm.x, p_cm.y); + + // determine the number of intersections + int hits = 0; + const int cnt = points_cm.size(); + for (int i = 0; i < cnt; ++i) { + const Point2 p1 = points_cm[(i+0)%cnt]; + const Point2 p2 = points_cm[(i+1)%cnt]; + const Line2 l12(p1, p2); + if (l12.getSegmentIntersection(l)) {++hits;} + } + + // inside or outside? + return ((hits % 2) == 1); + + } + + /** call a user-function for each GRID-ALIGNED point within the polygon */ + void forEachGridPoint(const int gridSize_cm, std::function callback) const { + + int x1 = std::floor(bbox_cm.getMin().x / gridSize_cm) * gridSize_cm; + int x2 = std::ceil(bbox_cm.getMax().x / gridSize_cm) * gridSize_cm; + + int y1 = std::floor(bbox_cm.getMin().y / gridSize_cm) * gridSize_cm; + int y2 = std::ceil(bbox_cm.getMax().y / gridSize_cm) * gridSize_cm; + + // process each point within the (aligned) bbox + for (int y = y1; y <= y2; y += gridSize_cm) { + for (int x = x1; x <= x2; x += gridSize_cm) { + + // does this point belong to the polygon? + if (!contains(Point2(x,y))) {continue;} + + // call the callback + callback(x,y); + + } + } + + } + +}; + +#endif // HELPERPOLY3_H diff --git a/grid/walk/v3/Helper.h b/grid/walk/v3/Helper.h index c62e7b8..57b6c13 100644 --- a/grid/walk/v3/Helper.h +++ b/grid/walk/v3/Helper.h @@ -174,10 +174,11 @@ namespace GW3 { public: - static GridPoint p3ToGp(const Point3 p) { - const Point3 p100 = p*100; - return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) ); - } +// static GridPoint p3ToGp(const Grid& grid, const Point3 p) { +// const Point3 p100 = p*100; +// //return GridPoint( std::round(p100.x), std::round(p100.y), std::round(p100.z) ); +// return GridPoint( grid.snapX(p100.x), grid.snapY(p100.y), grid.snapZ(p100.z) ); +// } static Point3 gpToP3(const GridPoint gp) { return Point3(gp.x_cm / 100.0f, gp.y_cm / 100.0f, gp.z_cm / 100.0f); diff --git a/grid/walk/v3/Structs.h b/grid/walk/v3/Structs.h index 8d46389..2c92da6 100644 --- a/grid/walk/v3/Structs.h +++ b/grid/walk/v3/Structs.h @@ -4,18 +4,55 @@ #include "../../../geo/Heading.h" #include "../../../geo/Point3.h" #include +#include "../../../math/Distributions.h" +#include "../../../grid/Grid.h" namespace GW3 { + struct StepSizes { + + float stepSizeFloor_m = NAN; + float stepSizeStair_m = NAN; + + bool isValid() const { + return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m); + } + + template float inMeter(const int steps, const Point3 start, const Grid& grid) const { + + Assert::isTrue(isValid(), "invalid step-sizes given"); + + const GridPoint gp = grid.toGridPoint(start); + const Node& n = grid.getNodeFor(gp); + if (grid.isPlain(n)) { + return stepSizeFloor_m * steps; + } else { + return stepSizeStair_m * steps; + } + + } + + }; + /** paremters for the walk */ struct WalkParams { + //Distribution::Normal dDistFloor; + //Distribution::Normal dDistStair; + Point3 start; - float distance_m; + //float distance_m; + int numSteps; Heading heading = Heading(0); float lookFurther_m = 1.5; + StepSizes stepSizes; + + template float getDistanceInMeter(const Grid& grid) const { + return stepSizes.inMeter(numSteps, start, grid); + } + }; /** result of the random walk */ diff --git a/grid/walk/v3/WalkEvaluator.h b/grid/walk/v3/WalkEvaluator.h index c34080a..340bbb8 100644 --- a/grid/walk/v3/WalkEvaluator.h +++ b/grid/walk/v3/WalkEvaluator.h @@ -9,6 +9,29 @@ namespace GW3 { + /** describes a potential walk, which can be evaluated */ + struct PotentialWalk { + + /** initial parameters (requested walk) */ + const WalkParams& params; + + /** walk started here */ + Point3 pStart; + + /** walk ended here */ + Point3 pEnd; + + /** usually the euclidean distance start<->end but not necessarily! */ + float walkDist_m; + + /** ctor */ + PotentialWalk(const WalkParams& params, const Point3 pStart, const Point3 pEnd, const float walkedDistance_m) : + params(params), pStart(pStart), pEnd(pEnd), walkDist_m(walkedDistance_m) { + ; + } + + }; + /** interface for all evaluators that return a probability for a given walk */ template class WalkEvaluator { @@ -17,7 +40,7 @@ namespace GW3 { /** get the probability for the given walk */ //virtual double getProbability(const Walk& walk) const = 0; - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const = 0; + virtual double getProbability(const PotentialWalk& walk) const = 0; }; @@ -31,12 +54,9 @@ namespace GW3 { WalkEvalEndNodeProbability(Grid* grid) : grid(grid) {;} - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkedDist_m, const WalkParams& params) const override { + virtual double getProbability(const PotentialWalk& walk) const override { - (void) params; - (void) pStart; - - const GridPoint gp = Helper::p3ToGp(pEnd); + const GridPoint gp = Helper::p3ToGp(walk.pEnd); const Node& node = grid->getNodeFor(gp); const double p = node.getWalkImportance(); return p; @@ -65,17 +85,15 @@ namespace GW3 { ; } - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override { + virtual double getProbability(const PotentialWalk& walk) const override { - (void) params; - - if (pStart == pEnd) { + if (walk.pStart == walk.pEnd) { std::cout << "warn! start-position == end-positon" << std::endl; return 0; } - const Heading head(pStart.xy(), pEnd.xy()); - const float diff = head.getDiffHalfRAD(params.heading); + const Heading head(walk.pStart.xy(), walk.pEnd.xy()); + const float diff = head.getDiffHalfRAD(walk.params.heading); //const float diff = Heading::getSignedDiff(params.heading, head); //return Distribution::Normal::getProbability(0, sigma, diff); return dist.getProbability(diff); @@ -87,18 +105,20 @@ namespace GW3 { /** evaluate the difference between distance(start, end) and the requested distance */ template class WalkEvalDistance : public WalkEvaluator { + const Grid& grid; + const double sigma; const Distribution::Normal dist; public: - WalkEvalDistance(const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;} + WalkEvalDistance(const Grid& grid, const double sigma = 0.1) : grid(grid), sigma(sigma), dist(0, sigma) {;} - virtual double getProbability(const Point3 pStart, const Point3 pEnd, const float walkDist_m, const WalkParams& params) const override { + virtual double getProbability(const PotentialWalk& walk) const override { - const float requestedDistance_m = params.distance_m; - const float walkedDistance_m = walkDist_m;//pStart.getDistance(pEnd); + const float requestedDistance_m = walk.params.getDistanceInMeter(grid); + const float walkedDistance_m = walk.walkDist_m;//pStart.getDistance(pEnd); const float diff = walkedDistance_m - requestedDistance_m; return dist.getProbability(diff); //return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); diff --git a/grid/walk/v3/Walker.h b/grid/walk/v3/Walker.h index bee1178..48a3c2e 100644 --- a/grid/walk/v3/Walker.h +++ b/grid/walk/v3/Walker.h @@ -58,18 +58,22 @@ namespace GW3 { /** perform the walk based on the configured setup */ const WalkResult getDestination(const WalkParams& params) const override { - Assert::isNot0(params.distance_m, "walking distance must be > 0"); + Assert::isNot0(params.getDistanceInMeter(grid), "walking distance must be > 0"); + + Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(params.start)), "start-point not found on grid"); + static std::mt19937 rndGen; - const GridPoint gpStart = Helper::p3ToGp(params.start); + const GridPoint gpStart = grid.toGridPoint(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); // calculate a walk's probability auto getP = [&] (const Point3 dst) { double p = 1; + const PotentialWalk pWalk(params, params.start, dst, params.start.getDistance(dst)); for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(params.start, dst, params.start.getDistance(dst), params); + const double p1 = eval->getProbability(pWalk); p *= p1; } return p; @@ -97,7 +101,7 @@ namespace GW3 { return (startNode->getDistanceInMeter(n)) < maxDist_m; } }; - Cond cond(params.distance_m+secBuffer_m, startNode); + Cond cond(params.getDistanceInMeter(grid) + secBuffer_m, startNode); std::vector reachableNodes = ReachableByConditionUnsorted::get(grid, *startNode, cond); WalkResult res; @@ -106,7 +110,7 @@ namespace GW3 { // get the to-be-reached destination's position (using start+distance+heading) const Point2 dir = res.heading.asVector(); - const Point2 dst = params.start.xy() + (dir * params.distance_m); + const Point2 dst = params.start.xy() + (dir * params.getDistanceInMeter(grid)); // is above destination reachable? const Node* n = Helper::contains(grid, reachableNodes, dst); @@ -114,14 +118,14 @@ namespace GW3 { if (n) { const Point3 p3(dst.x, dst.y, n->z_cm / 100.0f); - const GridPoint gp = Helper::p3ToGp(p3); + const GridPoint gp = grid.toGridPoint(p3); if (grid.hasNodeFor(gp)) { res.position = p3; // update position //res.heading; // keep as-is - res.probability *= 1;//getP(p3); // keep as-is + res.probability *= getP(p3); // keep as-is return res; // done } else { @@ -145,9 +149,11 @@ namespace GW3 { const Point3 start = params.start; const Point3 end = Helper::gpToP3(*dstNode) + dstOffset; + const PotentialWalk pWalk(params, start, end, start.getDistance(end)); + double p = 1; for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, end, start.getDistance(end), params); + const double p1 = eval->getProbability(pWalk); p *= p1; } @@ -157,8 +163,29 @@ namespace GW3 { } res.heading = Heading(start.xy(), end.xy()); - res.probability *= 0.1;//getP(end); + res.probability *= getP(end); res.position = end; + + if (!grid.hasNodeFor(grid.toGridPoint(res.position))) { + + std::cout << "issue:" << grid.toGridPoint(res.position).asString() << std::endl; + + std::cout << "issue:" << res.position.asString() << std::endl; + + for (int i = -80; i <= +80; i+=10) { + Point3 pos = res.position + Point3(0,0,i/100.0f); + std::cout << pos.asString() << " ----- " << res.position.asString() << std::endl; + std::cout << (grid.toGridPoint(pos)).asString() << std::endl; + std::cout << grid.hasNodeFor(grid.toGridPoint(pos)) << std::endl; + std::cout << std::endl; + } + + int i = 0; (void) i; + + } + + Assert::isTrue(grid.hasNodeFor(grid.toGridPoint(res.position)), "end-point not found on grid"); + return res; } @@ -204,14 +231,16 @@ namespace GW3 { /** perform the walk based on the configured setup */ const WalkResult getDestination(const WalkParams& params) const override { - Assert::isNot0(params.distance_m, "walking distance must be > 0"); + const float walkDist_m = params.getDistanceInMeter(grid); + + Assert::isNot0(walkDist_m, "walking distance must be > 0"); const GridPoint gpStart = Helper::p3ToGp(params.start); const Node* startNode = grid.getNodePtrFor(gpStart); if (!startNode) {throw Exception("start node not found!");} - const float maxDist = params.distance_m + gridSize_m; - const int depth = std::ceil(params.distance_m / gridSize_m) + 1; + const float maxDist = walkDist_m + gridSize_m; + const int depth = std::ceil(walkDist_m / gridSize_m) + 1; Point3 best; double bestP = 0; //DrawList drawer; @@ -331,9 +360,11 @@ namespace GW3 { //Assert::isTrue(grid.hasNodeFor(Helper::p3ToGp(end)), "random destination is not part of the grid"); const float walkDist_m = end.getDistance(start);//*1.05; + const PotentialWalk pWalk(params, start, end, walkDist_m); + double p = 1; for (const WalkEvaluator* eval : evals) { - const double p1 = eval->getProbability(start, end, walkDist_m, params); + const double p1 = eval->getProbability(pWalk); p *= p1; } From ca6fed5371a0250d1a62c384f23ff24b57ad1758 Mon Sep 17 00:00:00 2001 From: frank Date: Mon, 8 Jan 2018 20:55:50 +0100 Subject: [PATCH 09/12] 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); } From 3fc9688825e959a3a5befec89ea20e4be0559727 Mon Sep 17 00:00:00 2001 From: frank Date: Wed, 10 Jan 2018 08:31:14 +0100 Subject: [PATCH 10/12] worked on nav-meshes --- navMesh/NavMesh.h | 58 ++++++++++++++++----------- navMesh/NavMeshLocation.h | 19 +++++++++ navMesh/NavMeshRandom.h | 13 +++--- navMesh/NavMeshTriangle.h | 6 ++- navMesh/walk/NavMeshSub.h | 56 ++++++++++++++++++++++++++ navMesh/walk/NavMeshWalkHelper.h | 10 +++++ tests/navMesh/TestNavMeshFactory.cpp | 4 +- tests/navMesh/TestNavMeshSub.cpp | 29 ++++++++++++++ tests/navMesh/TestNavMeshTriangle.cpp | 6 +-- 9 files changed, 165 insertions(+), 36 deletions(-) create mode 100644 navMesh/NavMeshLocation.h create mode 100644 navMesh/walk/NavMeshSub.h create mode 100644 navMesh/walk/NavMeshWalkHelper.h create mode 100644 tests/navMesh/TestNavMeshSub.cpp diff --git a/navMesh/NavMesh.h b/navMesh/NavMesh.h index 1740bc0..362ee39 100644 --- a/navMesh/NavMesh.h +++ b/navMesh/NavMesh.h @@ -7,11 +7,12 @@ #include #include "../math/DrawList.h" #include "NavMeshRandom.h" +#include "NavMeshLocation.h" template class NavMesh { /** all triangles within the mesh */ - std::vector triangles; + std::vector triangles; BBox3 bbox; @@ -28,12 +29,21 @@ public: /** 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)); + triangles.push_back(new Tria(p1,p2,p3,type)); bbox.add(p1); bbox.add(p2); bbox.add(p3); } + NavMeshLocation getLocation(const Point3 pos) { + for (const Tria* tria : triangles) { + if (tria->contains(pos)) { + return NavMeshLocation(pos, tria); + } + } + throw Exception("location not found"); + } + /** connect both triangles */ void connectBiDir(int idx1, int idx2) { connectUniDir(idx1,idx2); @@ -42,8 +52,8 @@ public: /** connect both triangles */ void connectUniDir(int idxFrom, int idxTo) { - NavMeshTriangle& tria = triangles[idxFrom]; - tria._neighbors[tria._numNeighbors] = idxTo; + NavMeshTriangle* tria = triangles[idxFrom]; + tria->_neighbors[tria->_numNeighbors] = triangles[idxTo]; } /** allows for-each iteration over all included triangles */ @@ -53,7 +63,7 @@ public: decltype(triangles.end()) end() {return triangles.end();} /** array access */ - Tria& operator [] (const size_t idx) { + Tria* operator [] (const size_t idx) { Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds"); return triangles[idx]; } @@ -70,29 +80,29 @@ public: return NavMeshRandom(triangles); } - /** ---------------- NEIGHBORS ---------------- */ +// /** ---------------- 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 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 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 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; - } +// /** 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; +// } }; diff --git a/navMesh/NavMeshLocation.h b/navMesh/NavMeshLocation.h new file mode 100644 index 0000000..51d8fb9 --- /dev/null +++ b/navMesh/NavMeshLocation.h @@ -0,0 +1,19 @@ +#ifndef NAVMESHLOCATION_H +#define NAVMESHLOCATION_H + +#include "../geo/Point3.h" + +template struct NavMeshLocation { + + const Tria* tria; + + Point3 pos; + + /** ctor */ + NavMeshLocation(Point3 pos, const Tria* tria) : pos(pos), tria(tria) { + ; + } + +}; + +#endif // NAVMESHLOCATION_H diff --git a/navMesh/NavMeshRandom.h b/navMesh/NavMeshRandom.h index 2fcd98a..32cc38e 100644 --- a/navMesh/NavMeshRandom.h +++ b/navMesh/NavMeshRandom.h @@ -5,12 +5,13 @@ #include #include "../math/DrawList.h" #include "../geo/Point3.h" +#include "NavMeshLocation.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; + const std::vector& triangles; DrawList lst; public: @@ -22,24 +23,24 @@ public: }; /** ctor */ - NavMeshRandom(const std::vector& triangles) : triangles(triangles) { + NavMeshRandom(const std::vector& triangles) : triangles(triangles) { for (size_t idx = 0; idx < triangles.size(); ++idx) { - lst.add(idx, triangles[idx].getArea()); + lst.add(idx, triangles[idx]->getArea()); } } /** draw a random point within the map */ - Result draw() { + NavMeshLocation draw() { const size_t idx = lst.get(); - const Tria& tria = triangles[idx]; + 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); + return NavMeshLocation(pos, tria); } } diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h index 6397ac3..a91ccb0 100644 --- a/navMesh/NavMeshTriangle.h +++ b/navMesh/NavMeshTriangle.h @@ -17,7 +17,7 @@ private: template friend class NavMesh; - int _neighbors[3]; + NavMeshTriangle* _neighbors[3]; int _numNeighbors; /** precalculated stuff */ @@ -51,6 +51,10 @@ public: } + decltype(std::begin(_neighbors)) begin() {return std::begin(_neighbors);} + + decltype(std::end(_neighbors)) end() {return std::end(_neighbors);} + Point3 getA() const { return p1; } diff --git a/navMesh/walk/NavMeshSub.h b/navMesh/walk/NavMeshSub.h new file mode 100644 index 0000000..90fc789 --- /dev/null +++ b/navMesh/walk/NavMeshSub.h @@ -0,0 +1,56 @@ +#ifndef NAVMESHSUB_H +#define NAVMESHSUB_H + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" + +#include +#include + + +template class NavMeshSub { + + std::vector toVisit; + +public: + + NavMeshSub(const NavMesh& nm, const NavMeshLocation& loc, float radius_m) { + build(nm,loc,radius_m); + } + +private: + + void build(const NavMesh& nm, const NavMeshLocation& loc, float radius_m) { + + // center to start searching + const Point3 center = loc.pos; + + toVisit.push_back(loc.tria); + + std::unordered_set visited; + + size_t next = 0; + while (next < toVisit.size()) { + + // next triangle + const Tria* cur = toVisit[next]; ++next; + + // neighbors + for (const Tria* n : cur) { + const float dist = loc.pos.getDistance(n.getCenter()); + if (dist > radius_m) {continue;} + if (visited.find(n) != visited.end()) {continue;} + toVisit.push_back(n); + visited.push_back(n); + } + + } + + return toVisit; + + } + + +}; + +#endif // NAVMESHSUB_H diff --git a/navMesh/walk/NavMeshWalkHelper.h b/navMesh/walk/NavMeshWalkHelper.h new file mode 100644 index 0000000..65cc4fb --- /dev/null +++ b/navMesh/walk/NavMeshWalkHelper.h @@ -0,0 +1,10 @@ +#ifndef NAVMESHWALKHELPER_H +#define NAVMESHWALKHELPER_H + +template class NavMeshWalkHelper { + + + +} + +#endif // NAVMESHWALKHELPER_H diff --git a/tests/navMesh/TestNavMeshFactory.cpp b/tests/navMesh/TestNavMeshFactory.cpp index 805bba1..cf8928a 100644 --- a/tests/navMesh/TestNavMeshFactory.cpp +++ b/tests/navMesh/TestNavMeshFactory.cpp @@ -30,8 +30,8 @@ TEST(NavMeshFactory, build1) { ASSERT_EQ(2, nm.getNumTriangles()); - ASSERT_EQ(nm.getNeighbor(0,0), nm[1]); - ASSERT_EQ(nm.getNeighbor(1,0), nm[0]); +// ASSERT_EQ(nm.getNeighbor(0,0), nm[1]); +// ASSERT_EQ(nm.getNeighbor(1,0), nm[0]); } diff --git a/tests/navMesh/TestNavMeshSub.cpp b/tests/navMesh/TestNavMeshSub.cpp new file mode 100644 index 0000000..9b3f4ab --- /dev/null +++ b/tests/navMesh/TestNavMeshSub.cpp @@ -0,0 +1,29 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../navMesh/NavMeshFactory.h" +#include "../../navMesh/walk/NavMeshSub.h" + +TEST(NavMeshSub, 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); + + NavMeshLocation loc = nm.getLocation(Point3(1,1,1)); + + +} + +#endif diff --git a/tests/navMesh/TestNavMeshTriangle.cpp b/tests/navMesh/TestNavMeshTriangle.cpp index 1234161..10027c3 100644 --- a/tests/navMesh/TestNavMeshTriangle.cpp +++ b/tests/navMesh/TestNavMeshTriangle.cpp @@ -6,7 +6,7 @@ TEST(NavMeshTriangle, contains) { - NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0)); + NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1); ASSERT_TRUE(t1.contains(Point3(0,0,0))); ASSERT_TRUE(t1.contains(Point3(1,0,0))); @@ -21,10 +21,10 @@ TEST(NavMeshTriangle, contains) { TEST(NavMeshTriangle, area) { - NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0)); + NavMeshTriangle t1(Point3(0,0,0), Point3(1,0,0), Point3(0,1,0), 1); ASSERT_NEAR(0.5, t1.getArea(), 0.0001); - NavMeshTriangle t2(Point3(0,0,9), Point3(1,0,9), Point3(0,1,9)); + NavMeshTriangle t2(Point3(0,0,9), Point3(1,0,9), Point3(0,1,9), 1); ASSERT_NEAR(0.5, t2.getArea(), 0.0001); } From fee6cd349642cef4e4b8b50f9ed4b1aff913dba6 Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Wed, 10 Jan 2018 16:57:19 +0100 Subject: [PATCH 11/12] worked on navMesh stuff - creation - walking - helper --- navMesh/NavMesh.h | 160 ++-- navMesh/NavMeshDebug.h | 105 ++- navMesh/NavMeshFactory.h | 1116 +++++++++++++------------ navMesh/NavMeshLocation.h | 34 +- navMesh/NavMeshPoly.h | 123 --- navMesh/NavMeshRandom.h | 88 +- navMesh/NavMeshTriangle.h | 222 +++-- navMesh/walk/NavMeshSub.h | 80 +- navMesh/walk/NavMeshWalkEval.h | 103 +++ navMesh/walk/NavMeshWalkHelper.h | 10 - navMesh/walk/NavMeshWalkParams.h | 64 ++ navMesh/walk/NavMeshWalkSimple.h | 121 ++- tests/navMesh/TestNavMeshFactory.cpp | 5 +- tests/navMesh/TestNavMeshSub.cpp | 7 +- tests/navMesh/TestNavMeshTriangle.cpp | 1 + 15 files changed, 1282 insertions(+), 957 deletions(-) delete mode 100644 navMesh/NavMeshPoly.h create mode 100644 navMesh/walk/NavMeshWalkEval.h delete mode 100644 navMesh/walk/NavMeshWalkHelper.h create mode 100644 navMesh/walk/NavMeshWalkParams.h diff --git a/navMesh/NavMesh.h b/navMesh/NavMesh.h index 362ee39..6077049 100644 --- a/navMesh/NavMesh.h +++ b/navMesh/NavMesh.h @@ -9,101 +9,113 @@ #include "NavMeshRandom.h" #include "NavMeshLocation.h" -template class NavMesh { +namespace NM { - /** all triangles within the mesh */ - std::vector triangles; + template class NavMesh { - BBox3 bbox; + /** all triangles within the mesh */ + std::vector triangles; -public: + BBox3 bbox; - NavMesh() { + public: - } + /** ctor */ + 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(new Tria(p1,p2,p3,type)); - bbox.add(p1); - bbox.add(p2); - bbox.add(p3); - } - - NavMeshLocation getLocation(const Point3 pos) { - for (const Tria* tria : triangles) { - if (tria->contains(pos)) { - return NavMeshLocation(pos, tria); - } } - throw Exception("location not found"); - } - /** connect both triangles */ - void connectBiDir(int idx1, int idx2) { - connectUniDir(idx1,idx2); - connectUniDir(idx2,idx1); - } + /** dtor */ + ~NavMesh() { + for (const Tria* t : triangles) {delete t;} + triangles.clear(); + } - /** connect both triangles */ - void connectUniDir(int idxFrom, int idxTo) { - NavMeshTriangle* tria = triangles[idxFrom]; - tria->_neighbors[tria->_numNeighbors] = triangles[idxTo]; - } + /** the overall bounding-box */ + const BBox3 getBBox() const { + return bbox; + } - /** allows for-each iteration over all included triangles */ - decltype(triangles.begin()) begin() {return triangles.begin();} + /** add a new triangle */ + void add(const Point3 p1, const Point3 p2, const Point3 p3, const uint8_t type) { + triangles.push_back(new Tria(p1,p2,p3,type)); + bbox.add(p1); + bbox.add(p2); + bbox.add(p3); + } - /** allows for-each iteration over all included triangles */ - decltype(triangles.end()) end() {return triangles.end();} + /** get the triangle this point belongs to (if any) */ + NavMeshLocation getLocation(const Point3 pos) { + for (const Tria* tria : triangles) { + if (tria->contains(pos)) { + return NavMeshLocation(pos, tria); + } + } + throw Exception("location not found within NavMesh: " + pos.asString()); + } - /** array access */ - Tria* operator [] (const size_t idx) { - Assert::isBetween(idx, (size_t)0, getNumTriangles()-1, "index out of bounds"); - return triangles[idx]; - } + /** connect both triangles */ + void connectBiDir(int idx1, int idx2) { + connectUniDir(idx1,idx2); + connectUniDir(idx2,idx1); + } - /** get the number of triangles used */ - size_t getNumTriangles() const { - return triangles.size(); - } + /** connect both triangles */ + void connectUniDir(int idxFrom, int idxTo) { + Tria* tria = triangles[idxFrom]; + tria->addNeighbor(triangles[idxTo]); + } - /** ---------------- MISC ---------------- */ + /** 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); - } + NavMeshRandom getRandom() { + return NavMeshRandom(triangles); + } -// /** ---------------- NEIGHBORS ---------------- */ + // /** ---------------- 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 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 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 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; -// } + // /** 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 index 85a4578..682add0 100644 --- a/navMesh/NavMeshDebug.h +++ b/navMesh/NavMeshDebug.h @@ -9,67 +9,80 @@ #include #include -class NavMeshDebug { +namespace NM { -public: + /** + * debug plot NavMeshes + */ + class NavMeshDebug { - template static void show(NavMesh& nm) { + public: - 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) - }; + template static void show(NavMesh& nm) { - K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600")); + 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::Gnuplot gp; - gp << "set view equal xy\n"; + K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600")); - K::GnuplotSplot plot; - K::GnuplotSplotElementLines lines; plot.add(&lines); lines.setShowPoints(true); - K::GnuplotSplotElementPoints points; plot.add(&points); + K::Gnuplot gp; + gp << "set view equal xy\n"; - const BBox3 bbox = nm.getBBox(); + K::GnuplotSplot plot; + K::GnuplotSplotElementLines lines; plot.add(&lines); lines.setShowPoints(true); + K::GnuplotSplotElementPoints points; plot.add(&points); - 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)); + const BBox3 bbox = nm.getBBox(); - //stairs in eigene group? vlt gehen dann auch die dellen weg? + 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) { + const uint8_t type = tria->getType(); + 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->getP1().x, tria->getP1().y, tria->getP1().z, K::GnuplotCoordinateSystem::FIRST)); + pol->add(K::GnuplotCoordinate3(tria->getP2().x, tria->getP2().y, tria->getP2().z, K::GnuplotCoordinateSystem::FIRST)); + pol->add(K::GnuplotCoordinate3(tria->getP3().x, tria->getP3().y, tria->getP3().z, K::GnuplotCoordinateSystem::FIRST)); + pol->close(); + pol->setZIndex(tria->getP3().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)); + //} + + for (const NavMeshTriangle* o : *tria) { + 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)); + } - 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); } - plot.getObjects().reOrderByZIndex(); + }; - gp.draw(plot); - gp.flush(); - sleep(1); - - } - -}; +} #endif // NAVMESHDEBUG_H diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index 1899a33..7a5e282 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -5,569 +5,647 @@ #include "../floorplan/v2/FloorplanHelper.h" #include "NavMesh.h" -#include "NavMeshPoly.h" #include "NavMeshTriangle.h" +#include "../lib/gpc/gpc.cpp.h" #include "../lib/Recast/Recast.h" -enum SamplePartitionType { - SAMPLE_PARTITION_WATERSHED, - SAMPLE_PARTITION_MONOTONE, - SAMPLE_PARTITION_LAYERS, -}; +namespace NM { -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 { + class NavMeshPoly { - 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); + struct GPCPolygon : gpc_polygon { + GPCPolygon() { + 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) { + ; } - for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { - if (poly->method == Floorplan::OutlineMethod::REMOVE) { - nmPoly.remove(poly->poly); - } + void add(const Floorplan::Polygon2& poly) { + GPCPolygon cur = toGPC(poly); + gpc_polygon_clip(GPC_UNION, &state, &cur, &state); } - for (Floorplan::FloorObstacle* obs : floor->obstacles) { - Floorplan::FloorObstacleLine* line = dynamic_cast(obs); - if (line != nullptr) { - nmPoly.remove(getPolygon(line)); - } + void remove(const Floorplan::Polygon2& poly) { + GPCPolygon cur = toGPC(poly); + gpc_polygon_clip(GPC_DIFF, &state, &cur, &state); } - 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); - } + std::vector> get() { - // 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); - } - } + gpc_tristrip res; + res.num_strips = 0; + res.strip = nullptr; - } + //res.strip = (gpc_vertex_list*) malloc(1024); + gpc_polygon_to_tristrip(&state, &res); - bool fire(BBox3 bbox) { + std::vector> trias; - 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; + for (int i = 0; i < res.num_strips; ++i) { + gpc_vertex_list lst = res.strip[i]; + 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); } - // 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; - } + + 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); } - 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; + + GPCPolygon gpol; + gpc_vertex_list list; + list.num_vertices = verts.size(); + list.vertex = verts.data(); + gpc_add_contour(&gpol, &list, 0); + + return gpol; + + } + + }; + + + + + 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: + + float maxQuality_m = 0.20f; // 25cm elements are the smallest to-be-detected + + 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); + } + + /** get the smallest obstacle size that can be detected */ + float getMaxQuality_m() const { + return maxQuality_m; + } + + 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); } } - // - // 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; + for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + if (poly->method == Floorplan::OutlineMethod::REMOVE) { + nmPoly.remove(poly->poly); + } } - // - // 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; + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + if (line != nullptr) { + nmPoly.remove(getPolygon(line)); + } } - // - // 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; + 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); } - 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); + // 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); + } } } - return true; + 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; -// void dump() { + //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(); -// 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)); + float m_cellSize = maxQuality_m/2.0f; //0.3f; // ensure quality is enough to fit maxQuality_m + float m_cellHeight = maxQuality_m/2.0f; //0.2f; + float m_agentHeight = 2.0f; + float m_agentRadius = 0.2f;//0.6f; + float m_agentMaxClimb = maxQuality_m; // 0.9f; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail! + float m_agentMaxSlope = 45.0f; // elevator??? + 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.1f; //1.3f; // higher values allow joining some small triangles + float m_vertsPerPoly = 3;//6.0f; + float m_detailSampleDist = 6.0f; + float m_detailSampleMaxError = 1.0f;//1.0f; + int m_partitionType = SAMPLE_PARTITION_WATERSHED; // SAMPLE_PARTITION_WATERSHED SAMPLE_PARTITION_MONOTONE SAMPLE_PARTITION_LAYERS -// 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); -// } + // 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; -// gp.draw(plot); -// gp.flush(); -// sleep(1000); + 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); - /** 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; - } + // 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; + + } + + + /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ + Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) const { + //const Line2 base(line->from*100, line->to*100); + const float thickness_m = std::max(line->thickness_m, maxQuality_m); // wall's thickness (make thin walls big enough to be detected) + 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/NavMeshLocation.h b/navMesh/NavMeshLocation.h index 51d8fb9..1ed7c60 100644 --- a/navMesh/NavMeshLocation.h +++ b/navMesh/NavMeshLocation.h @@ -3,17 +3,35 @@ #include "../geo/Point3.h" -template struct NavMeshLocation { +class NavMeshTriangle; - const Tria* tria; +namespace NM { - Point3 pos; + /** + * as Point3 -> Triangle (on Mesh) lookups are expensive, + * we try to combine both information (point -> triangle) + * most of the time using this structure + */ + template struct NavMeshLocation { - /** ctor */ - NavMeshLocation(Point3 pos, const Tria* tria) : pos(pos), tria(tria) { - ; - } + /** point within the world (in meter) */ + Point3 pos; -}; + /** NavMeshTriangle the point belongs to */ + const Tria* tria; + + /** empty ctor */ + NavMeshLocation() : pos(0,0,0), tria(nullptr) { + ; + } + + /** ctor */ + NavMeshLocation(const Point3 pos, const Tria* tria) : pos(pos), tria(tria) { + ; + } + + }; + +} #endif // NAVMESHLOCATION_H diff --git a/navMesh/NavMeshPoly.h b/navMesh/NavMeshPoly.h deleted file mode 100644 index 014338a..0000000 --- a/navMesh/NavMeshPoly.h +++ /dev/null @@ -1,123 +0,0 @@ -#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 index 32cc38e..b448cf2 100644 --- a/navMesh/NavMeshRandom.h +++ b/navMesh/NavMeshRandom.h @@ -5,46 +5,66 @@ #include #include "../math/DrawList.h" #include "../geo/Point3.h" + #include "NavMeshLocation.h" -template class NavMeshRandom { +namespace NM { - std::minstd_rand gen; - std::uniform_real_distribution dOnTriangle = std::uniform_real_distribution(0.0f, 1.0f); - const std::vector& triangles; - DrawList lst; + /** + * randomly pick points within the area of the nav-mesh. + * points are picked evenly: + * bigger triangles are used more often + * + */ + template class NavMeshRandom { -public: + DrawList lst; + std::minstd_rand gen; + std::uniform_real_distribution dOnTriangle = std::uniform_real_distribution(0.0f, 1.0f); + std::vector triangles; + + + uint32_t nextSeed() { + static uint32_t seed = 0; + return ++seed; + } + + public: + + /** ctor (const/non-const using T) */ + template NavMeshRandom(const std::vector& srcTriangles) : lst(nextSeed()), gen(nextSeed()) { + + // almost always the same number?! + gen(); + + // construct a DrawList (probability = size[area] of the triangle + // bigger triangles must be choosen more often + for (size_t idx = 0; idx < srcTriangles.size(); ++idx) { + this->triangles.push_back(srcTriangles[idx]); + this->lst.add(idx, srcTriangles[idx]->getArea()); + } + + } + + /** draw a random point */ + NavMeshLocation draw() { + + // pick a random triangle to draw from + 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->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v); + return NavMeshLocation(pos, tria); + } + + } - 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 */ - NavMeshLocation 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 NavMeshLocation(pos, tria); - } - - } - -}; +} #endif // NAVMESHRANDOM_H diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h index a91ccb0..ce5d129 100644 --- a/navMesh/NavMeshTriangle.h +++ b/navMesh/NavMeshTriangle.h @@ -4,127 +4,185 @@ #include "../geo/Point3.h" #include "../geo/Point2.h" -class NavMeshTriangle { +namespace NM { -public: + /** + * represents one triangle within the NavMesh + * each Triangle has up to 3 neighbors (one per edge) + * + * for performance enhancements, + * some memeber attributes are pre-calculated once + */ + class NavMeshTriangle { - Point3 p1; - Point3 p2; - Point3 p3; - uint8_t type; + private: -private: + template friend class NavMesh; - template friend class NavMesh; + const Point3 p1; + const Point3 p2; + const Point3 p3; + const uint8_t type; - NavMeshTriangle* _neighbors[3]; - int _numNeighbors; + NavMeshTriangle* _neighbors[3]; + int _numNeighbors; - /** precalculated stuff */ + private: // precalculated stuff -private: + Point2 v0; + Point2 v1; + float dot00; + float dot01; + float dot11; + float invDenom; + float area; - Point2 v0; - Point2 v1; - float dot00; - float dot01; - float dot11; - float invDenom; - float area; + float minZ; + float maxZ; - const Point3 center; - const Point3 v12; - const Point3 v13; + const Point3 center; + const Point3 v12; + const Point3 v13; -public: + 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(); - } + /** 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) { - bool operator == (const NavMeshTriangle& o) const { - return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3); - } + precompute(); + + } + + /** get the triangle's type */ + uint8_t getType() const {return type;} + + Point3 getP1() const {return p1;} + + Point3 getP2() const {return p2;} + + Point3 getP3() const {return p3;} - decltype(std::begin(_neighbors)) begin() {return std::begin(_neighbors);} + bool operator == (const NavMeshTriangle& o) const { + return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3); + } - decltype(std::end(_neighbors)) end() {return std::end(_neighbors);} + /** is the triangle plain? (same Z for all points) */ + bool isPlain() const { + const float d1 = std::abs(p1.z - p2.z); + const float d2 = std::abs(p2.z - p3.z); + return (d1 < 0.1) && (d2 < 0.1); + } - Point3 getA() const { - return p1; - } + const NavMeshTriangle* const* begin() const {return &_neighbors[0];} - Point3 getAB() const { - return v12; - } + const NavMeshTriangle* const* end() const {return &_neighbors[_numNeighbors];} - Point3 getAC() const { - return v13; - } + Point3 getPoint(const float u, const float v) const { + return p1 + (v12*u) + (v13*v); + } - bool contains(const Point3 p) const { + /** does the triangle contain the given 3D point? */ + bool contains(const Point3 p) const { + return (minZ <= p.z) && (maxZ >= p.z) && contains(p.xy()); + } - const Point2 v2 = p.xy() - p1.xy(); + /** does the triangle contain the given 2D point? */ + bool contains(const Point2 p) const { - // Compute dot products - float dot02 = dot(v0, v2); - float dot12 = dot(v1, v2); + const Point2 v2 = p - p1.xy(); - // Compute barycentric coordinates - float u = (dot11 * dot02 - dot01 * dot12) * invDenom; - float v = (dot00 * dot12 - dot01 * dot02) * invDenom; + // Compute dot products + float dot02 = dot(v0, v2); + float dot12 = dot(v1, v2); - // Check if point is in triangle - return (u >= 0) && (v >= 0) && (u + v <= 1); + // 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; - } + /** estimate the correct z-value for the given 2D point */ + Point3 toPoint3(const Point2 p) const { + + const Point2 v2 = p - 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; + + const Point3 res = getPoint(v,u); + return res; + + } -private: + /** get the triangle's size */ + float getArea() const { + return area; + } - /** perform some pre-calculations to speed things up */ - void precompute() { + /** get the triangle's center-point */ + Point3 getCenter() const { + return center; + } - // 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); + private: + + /** perform some pre-calculations to speed things up */ + void precompute() { + + #warning "TODO, z buffer" + minZ = std::min(p1.z, std::min(p2.z, p3.z)) - 0.15; // TODO the builder does not align on the same height as we did + maxZ = std::max(p1.z, std::max(p2.z, p3.z)) + 0.15; + + // 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) ); + 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) ); - } + } + + protected: + + void addNeighbor(NavMeshTriangle* o) { + Assert::isBetween(_numNeighbors, 0, 3, "number of neighbors out of bounds"); + _neighbors[_numNeighbors] = o; + ++_numNeighbors; + } -}; + }; + +} #endif // NAVMESHTRIANGLE_H diff --git a/navMesh/walk/NavMeshSub.h b/navMesh/walk/NavMeshSub.h index 90fc789..57aa0e5 100644 --- a/navMesh/walk/NavMeshSub.h +++ b/navMesh/walk/NavMeshSub.h @@ -3,54 +3,80 @@ #include "../NavMesh.h" #include "../NavMeshLocation.h" +#include "../NavMeshRandom.h" #include #include +namespace NM { -template class NavMeshSub { + template class NavMeshSub { - std::vector toVisit; + std::vector toVisit; -public: + public: - NavMeshSub(const NavMesh& nm, const NavMeshLocation& loc, float radius_m) { - build(nm,loc,radius_m); - } + NavMeshSub(const NavMeshLocation& loc, float radius_m) { + build(loc,radius_m); + } -private: + /** does this submesh contain the given point? */ + bool contains(const Point2 p2) const { + for (const Tria* t : toVisit) { + if (t->contains(p2)) {return true;} + } + return false; + } - void build(const NavMesh& nm, const NavMeshLocation& loc, float radius_m) { + /** get the triangle that contains the given point (if any) */ + const Tria* getContainingTriangle(const Point2 p2) const { + for (const Tria* t : toVisit) { + if (t->contains(p2)) {return t;} + } + return nullptr; + } - // center to start searching - const Point3 center = loc.pos; + /** perform random operations on the submesh */ + NavMeshRandom getRandom() { + return NavMeshRandom(toVisit); + } - toVisit.push_back(loc.tria); + private: - std::unordered_set visited; + void build(const NavMeshLocation& loc, float radius_m) { - size_t next = 0; - while (next < toVisit.size()) { + std::unordered_set visited; - // next triangle - const Tria* cur = toVisit[next]; ++next; + // starting-triangle + all its (max 3) neighbors + toVisit.push_back(loc.tria); + visited.insert(loc.tria); + for (const auto* n : *loc.tria) { + toVisit.push_back( (const Tria*)n ); + } + + size_t next = 1; // start with the first neighbor (skip starting triangle itself) + while (next < toVisit.size()) { + + // next triangle + const NavMeshTriangle* cur = toVisit[next]; ++next; + + // neighbors + for (const auto* n : *cur) { + const Tria* t = (const Tria*) n; + const float dist = loc.pos.getDistance(n->getCenter()); + if (dist > radius_m) {continue;} + if (visited.find(t) != visited.end()) {continue;} + toVisit.push_back(t); + visited.insert(t); + } - // neighbors - for (const Tria* n : cur) { - const float dist = loc.pos.getDistance(n.getCenter()); - if (dist > radius_m) {continue;} - if (visited.find(n) != visited.end()) {continue;} - toVisit.push_back(n); - visited.push_back(n); } } - return toVisit; - } + }; - -}; +} #endif // NAVMESHSUB_H diff --git a/navMesh/walk/NavMeshWalkEval.h b/navMesh/walk/NavMeshWalkEval.h new file mode 100644 index 0000000..e9e79ef --- /dev/null +++ b/navMesh/walk/NavMeshWalkEval.h @@ -0,0 +1,103 @@ +#ifndef NAVMESHWALKEVAL_H +#define NAVMESHWALKEVAL_H + +#include "NavMeshWalkParams.h" +#include "../NavMeshLocation.h" +#include "../../math/Distributions.h" + +namespace NM { + + template struct NavMeshPotentialWalk { + + NavMeshWalkParams requested; + + NavMeshLocation end; + + NavMeshPotentialWalk(const NavMeshWalkParams& requested, const NavMeshLocation& end) : requested(requested), end(end) { + ; + } + + }; + + /** + * evaluate a NavMeshWalk from -> to = probability + */ + template class NavMeshWalkEval { + + public: + + virtual double getProbability(const NavMeshPotentialWalk& walk) const = 0; + + }; + + + + + + + + /** + * evaluate the difference between head(start,end) and the requested heading + */ + template class WalkEvalHeadingStartEnd : public NavMeshWalkEval { + + const double sigma_rad; + const double kappa; + Distribution::VonMises _dist; + Distribution::LUT dist; + + public: + + // kappa = 1/var = 1/sigma^2 + // https://en.wikipedia.org/wiki/Von_Mises_distribution + WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) : + sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) { + ; + } + + virtual double getProbability(const NavMeshPotentialWalk& walk) const override { + + if (walk.requested.start.pos == walk.end.pos) { + std::cout << "warn! start-position == end-positon" << std::endl; + return 0; + } + + const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy()); + const float diff = head.getDiffHalfRAD(walk.requested.heading); + //const float diff = Heading::getSignedDiff(params.heading, head); + //return Distribution::Normal::getProbability(0, sigma, diff); + return dist.getProbability(diff); + + } + + }; + + /** + * evaluate the difference between distance(start, end) and the requested distance + */ + template class WalkEvalDistance : public NavMeshWalkEval { + + const double sigma; + + const Distribution::Normal dist; + + public: + + WalkEvalDistance( const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;} + + virtual double getProbability(const NavMeshPotentialWalk& walk) const override { + + const float requestedDistance_m = walk.requested.getToBeWalkedDistance(); + const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos); + const float diff = walkedDistance_m - requestedDistance_m; + return dist.getProbability(diff); + //return Distribution::Normal::getProbability(params.distance_m, sigma, walkedDistance_m); + + } + + }; + + +} + +#endif // NAVMESHWALKEVAL_H diff --git a/navMesh/walk/NavMeshWalkHelper.h b/navMesh/walk/NavMeshWalkHelper.h deleted file mode 100644 index 65cc4fb..0000000 --- a/navMesh/walk/NavMeshWalkHelper.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef NAVMESHWALKHELPER_H -#define NAVMESHWALKHELPER_H - -template class NavMeshWalkHelper { - - - -} - -#endif // NAVMESHWALKHELPER_H diff --git a/navMesh/walk/NavMeshWalkParams.h b/navMesh/walk/NavMeshWalkParams.h new file mode 100644 index 0000000..1e8d8f0 --- /dev/null +++ b/navMesh/walk/NavMeshWalkParams.h @@ -0,0 +1,64 @@ +#ifndef NAVMESHWALKPARAMS_H +#define NAVMESHWALKPARAMS_H + +#include "../../geo/Heading.h" +#include "../NavMeshLocation.h" + +namespace NM { + + /** configure pedestrian StepSizes */ + struct StepSizes { + + float stepSizeFloor_m = NAN; + float stepSizeStair_m = NAN; + + bool isValid() const { + return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m); + } + + template float inMeter(const int steps, const NavMeshLocation& start) const { + + Assert::isTrue(isValid(), "invalid step-sizes given"); + + if (start.tria->isPlain()) { + return stepSizeFloor_m * steps; + } else { + return stepSizeStair_m * steps; + } + + } + + }; + + /** configure walking from -> to */ + template struct NavMeshWalkParams { + + /** walk starts here (pos/tria) */ + NavMeshLocation start; + +// /** to-be-walked distance */ +// float distance_m; + + /** direction to walk to */ + Heading heading; + + /** number of steps to walk */ + int numSteps; + + /** configuration for pedestrian's step-sizes */ + StepSizes stepSizes; + + + /** empty ctor */ + NavMeshWalkParams() : heading(0) {;} + + /** get the to-be-walked distance (steps vs. current location [stair/floor/..]) */ + float getToBeWalkedDistance() const { + return stepSizes.inMeter(numSteps, start); + } + + }; + +} + +#endif // NAVMESHWALKPARAMS_H diff --git a/navMesh/walk/NavMeshWalkSimple.h b/navMesh/walk/NavMeshWalkSimple.h index 2f197ec..86f49a9 100644 --- a/navMesh/walk/NavMeshWalkSimple.h +++ b/navMesh/walk/NavMeshWalkSimple.h @@ -2,42 +2,105 @@ #define NAVMESHWALKSIMPLE_H #include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" -template class NavMeshWalkSimpel { +#include "NavMeshSub.h" +#include "NavMeshWalkParams.h" +#include "NavMeshWalkEval.h" -private: +namespace NM { - const NavMesh& mesh; + template class NavMeshWalkSimple { + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + int hits = 0; + int misses = 0; + + public: + + + struct Result { + + NavMeshLocation location; + Heading heading; + double probability; + + Result() : heading(0) {;} + + }; + + public: + + /** ctor */ + NavMeshWalkSimple(const NavMesh& mesh) : mesh(mesh) { + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* eval) { + this->evals.push_back(eval); + } + + Result getDestination(const NavMeshWalkParams& params) { + + Result res; + res.heading = params.heading; + + // to-be-walked distance; + const float toBeWalkedDist = params.getToBeWalkedDistance(); + const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1; + + // construct reachable region + NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + // get the to-be-reached destination's position (using start+distance+heading) + const Point2 dir = res.heading.asVector(); + const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist); + + const Tria* dstTria = reachable.getContainingTriangle(dst); + + // is above destination reachable? + if (dstTria) { + + res.location.pos = dstTria->toPoint3(dst); + res.location.tria = dstTria; + ++hits; + + } else { + + NavMeshRandom rnd = reachable.getRandom(); + NavMeshLocation rndLoc = rnd.draw(); + res.location = rndLoc; + res.heading = Heading(params.start.pos.xy(), rndLoc.pos.xy()); // update the heading + ++misses; + + } + + const int total = (hits + misses); + if (total % 10000 == 0) { + std::cout << "hits: " << (hits*100/total) << "%" << std::endl; + } + + const NavMeshPotentialWalk pwalk(params, res.location); + res.probability = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + res.probability *= p1; + } + + return res; + + } -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/tests/navMesh/TestNavMeshFactory.cpp b/tests/navMesh/TestNavMeshFactory.cpp index cf8928a..4362f60 100644 --- a/tests/navMesh/TestNavMeshFactory.cpp +++ b/tests/navMesh/TestNavMeshFactory.cpp @@ -3,6 +3,7 @@ #include "../Tests.h" #include "../../navMesh/NavMeshFactory.h" +using namespace NM; TEST(NavMeshFactory, build1) { @@ -16,8 +17,8 @@ TEST(NavMeshFactory, build1) { outline.outdoor = false; outline.method = Floorplan::OutlineMethod::ADD; - NavMesh nm; - NavMeshFactory fac(&nm); + NavMesh nm; + NavMeshFactory fac(&nm); fac.build(&map); ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5); diff --git a/tests/navMesh/TestNavMeshSub.cpp b/tests/navMesh/TestNavMeshSub.cpp index 9b3f4ab..2309cb5 100644 --- a/tests/navMesh/TestNavMeshSub.cpp +++ b/tests/navMesh/TestNavMeshSub.cpp @@ -4,6 +4,7 @@ #include "../../navMesh/NavMeshFactory.h" #include "../../navMesh/walk/NavMeshSub.h" +using namespace NM; TEST(NavMeshSub, build1) { @@ -17,11 +18,11 @@ TEST(NavMeshSub, build1) { outline.outdoor = false; outline.method = Floorplan::OutlineMethod::ADD; - NavMesh nm; - NavMeshFactory fac(&nm); + NavMesh nm; + NavMeshFactory fac(&nm); fac.build(&map); - NavMeshLocation loc = nm.getLocation(Point3(1,1,1)); + NavMeshLocation loc = nm.getLocation(Point3(1,1,1)); } diff --git a/tests/navMesh/TestNavMeshTriangle.cpp b/tests/navMesh/TestNavMeshTriangle.cpp index 10027c3..2051bfe 100644 --- a/tests/navMesh/TestNavMeshTriangle.cpp +++ b/tests/navMesh/TestNavMeshTriangle.cpp @@ -3,6 +3,7 @@ #include "../Tests.h" #include "../../navMesh/NavMeshTriangle.h" +using namespace NM; TEST(NavMeshTriangle, contains) { From 55061ef0da1f2bd28569bfbb8876dce4fc248469 Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 16 Jan 2018 12:41:05 +0100 Subject: [PATCH 12/12] minor changes to floorplan fixed some compile issues worked on nav-meshes added some tests --- floorplan/v2/Floorplan.h | 5 + geo/Angle.h | 1 + lib/gpc/gpc.cpp.h | 16 +- math/speed.h | 63 +++++ misc/PerfCheck.h | 130 +++++++++ navMesh/NavMeshDebug.h | 78 ++++-- navMesh/NavMeshFactory.h | 348 ++++++++++++++++--------- navMesh/NavMeshFactoryListener.h | 20 ++ navMesh/NavMeshRandom.h | 58 ++++- navMesh/NavMeshSettings.h | 63 +++++ navMesh/NavMeshTriangle.h | 48 +++- navMesh/NavMeshType.h | 22 ++ navMesh/walk/NavMeshSub.h | 33 ++- navMesh/walk/NavMeshWalkEval.h | 46 +++- navMesh/walk/NavMeshWalkParams.h | 26 +- navMesh/walk/NavMeshWalkRandom.h | 146 +++++++++++ navMesh/walk/NavMeshWalkSemiRandom.h | 169 ++++++++++++ navMesh/walk/NavMeshWalkSimple.h | 46 ++-- synthetic/SyntheticPath.h | 4 + synthetic/SyntheticSteps.h | 2 +- synthetic/SyntheticWalker.h | 4 + tests/navMesh/TestNavMeshBenchmark.cpp | 101 +++++++ tests/navMesh/TestNavMeshFactory.cpp | 3 +- tests/navMesh/TestNavMeshSub.cpp | 61 ++++- 24 files changed, 1288 insertions(+), 205 deletions(-) create mode 100644 math/speed.h create mode 100644 misc/PerfCheck.h create mode 100644 navMesh/NavMeshFactoryListener.h create mode 100644 navMesh/NavMeshSettings.h create mode 100644 navMesh/NavMeshType.h create mode 100644 navMesh/walk/NavMeshWalkRandom.h create mode 100644 navMesh/walk/NavMeshWalkSemiRandom.h create mode 100644 tests/navMesh/TestNavMeshBenchmark.cpp diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index d03f30d..8844d85 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -130,6 +130,10 @@ namespace Floorplan { default: throw Exception("out of bounds"); } } + /** same z-value for all points? */ + bool isLeveled() const { + return (p1.z == p2.z) && (p2.z == p3.z) && (p3.z == p4.z); + } }; /** additional type-info for obstacles */ @@ -202,6 +206,7 @@ namespace Floorplan { /** describes one floor within the map, starting at a given height */ struct Floor { + bool enabled = true; float atHeight; // the floor's starting height float height; // the floor's total height (from start) std::string name; // the floor's name diff --git a/geo/Angle.h b/geo/Angle.h index eabe999..a568450 100755 --- a/geo/Angle.h +++ b/geo/Angle.h @@ -4,6 +4,7 @@ #include #include "../Assertions.h" #include "Point2.h" +#include "../math/speed.h" #define PI ((float) M_PI) diff --git a/lib/gpc/gpc.cpp.h b/lib/gpc/gpc.cpp.h index 1f8371f..1dc913e 100755 --- a/lib/gpc/gpc.cpp.h +++ b/lib/gpc/gpc.cpp.h @@ -1017,7 +1017,7 @@ static void minimax_test(gpc_polygon *subj, gpc_polygon *clip, gpc_op op) =========================================================================== */ -void gpc_free_polygon(gpc_polygon *p) +inline void gpc_free_polygon(gpc_polygon *p) { int c; @@ -1029,7 +1029,7 @@ void gpc_free_polygon(gpc_polygon *p) } -void gpc_read_polygon(FILE *fp, int read_hole_flags, gpc_polygon *p) +inline void gpc_read_polygon(FILE *fp, int read_hole_flags, gpc_polygon *p) { int c, v; @@ -1056,7 +1056,7 @@ void gpc_read_polygon(FILE *fp, int read_hole_flags, gpc_polygon *p) } -void gpc_write_polygon(FILE *fp, int write_hole_flags, gpc_polygon *p) +inline void gpc_write_polygon(FILE *fp, int write_hole_flags, gpc_polygon *p) { int c, v; @@ -1076,7 +1076,7 @@ void gpc_write_polygon(FILE *fp, int write_hole_flags, gpc_polygon *p) } -void gpc_add_contour(gpc_polygon *p, gpc_vertex_list *new_contour, int hole) +inline void gpc_add_contour(gpc_polygon *p, gpc_vertex_list *new_contour, int hole) { int *extended_hole, c, v; gpc_vertex_list *extended_contour; @@ -1116,7 +1116,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, +inline void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, gpc_polygon *result) { sb_tree *sbtree= NULL; @@ -1754,7 +1754,7 @@ void gpc_polygon_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, } -void gpc_free_tristrip(gpc_tristrip *t) +inline void gpc_free_tristrip(gpc_tristrip *t) { int s; @@ -1765,7 +1765,7 @@ void gpc_free_tristrip(gpc_tristrip *t) } -void gpc_polygon_to_tristrip(gpc_polygon *s, gpc_tristrip *t) +inline void gpc_polygon_to_tristrip(gpc_polygon *s, gpc_tristrip *t) { gpc_polygon c; @@ -1776,7 +1776,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, +inline void gpc_tristrip_clip(gpc_op op, gpc_polygon *subj, gpc_polygon *clip, gpc_tristrip *result) { sb_tree *sbtree= NULL; diff --git a/math/speed.h b/math/speed.h new file mode 100644 index 0000000..9b32577 --- /dev/null +++ b/math/speed.h @@ -0,0 +1,63 @@ +#ifndef SPEED_H +#define SPEED_H + +#include + +class Speed { + +public: + +#define PI_FLOAT 3.14159265f +#define PIBY2_FLOAT 1.5707963f + + + static inline float atan2(float y, float x) { + + //http://pubs.opengroup.org/onlinepubs/009695399/functions/atan2.html + //Volkan SALMA + + const float ONEQTR_PI = M_PI / 4.0; + const float THRQTR_PI = 3.0 * M_PI / 4.0; + float r, angle; + float abs_y = fabs(y) + 1e-10f; // kludge to prevent 0/0 condition + if ( x < 0.0f ) { + r = (x + abs_y) / (abs_y - x); + angle = THRQTR_PI; + } else { + r = (x - abs_y) / (x + abs_y); + angle = ONEQTR_PI; + } + angle += (0.1963f * r * r - 0.9817f) * r; + if ( y < 0.0f ) + return( -angle ); // negate if in quad III or IV + else + return( angle ); + + } + +// // https://gist.github.com/volkansalma/2972237 +// static inline float atan2(const float y, const float x) { +// if ( x == 0.0f ) { +// if ( y > 0.0f ) return PIBY2_FLOAT; +// if ( y == 0.0f ) return 0.0f; +// return -PIBY2_FLOAT; +// } +// float atan; +// float z = y/x; +// if ( fabs( z ) < 1.0f ) { +// atan = z/(1.0f + 0.28f*z*z); +// if ( x < 0.0f ) { +// if ( y < 0.0f ) return atan - PI_FLOAT; +// return atan + PI_FLOAT; +// } +// } else { +// atan = PIBY2_FLOAT - z/(z*z + 0.28f); +// if ( y < 0.0f ) return atan - PI_FLOAT; +// } +// return atan; +// } + + +}; + +#endif // SPEED_H diff --git a/misc/PerfCheck.h b/misc/PerfCheck.h new file mode 100644 index 0000000..a72e455 --- /dev/null +++ b/misc/PerfCheck.h @@ -0,0 +1,130 @@ +#ifndef PERFCHECK_H +#define PERFCHECK_H + + +//#define WITH_PERF_CHECK + +#ifdef WITH_PERF_CHECK + + #include + #include + #include + + class PerfCheck { + + struct Stats { + size_t calls = 0; + clock_t time = 0; + }; + + uint32_t name; + clock_t in; + + static Stats stats[1024]; + + public: + + /** ctor */ + explicit PerfCheck(const uint32_t name) : name(name), in(clock()) { + ; + } + + /** dtor */ + ~PerfCheck() { + stats[name].calls += 1; + stats[name].time += clock() - in; + } + + PerfCheck(PerfCheck const&) = delete; + + PerfCheck& operator = (PerfCheck const&) = delete; + + static void dump() { + for (int i = 0; i < 1024; ++i) { + const Stats& s = stats[i]; + if (s.calls != 0) { + std::cout << i << ":\t"; + std::cout << "\tcalls: " << s.calls; + std::cout << "\ttime: " << s.time; + std::cout << "\ttime/call: " << ((double)s.time / (double)s.calls); + std::cout << std::endl; + } + } + } + + + + // static inline Stats* map() { + // static Stats stats[1024];// = new Stats[1024]; + // return stats; + // } + + }; + + PerfCheck::Stats PerfCheck::stats[1024]; + + #define PERF_REGION(idx, name) PerfCheck pcr_idx(idx) + #define PERF_DUMP() PerfCheck::dump(); + +#else + + #define PERF_REGION(idx, name) + #define PERF_DUMP() + +#endif + + + +/* +class PerfCheck { + + std::string name; + clock_t in; + +public: + + explicit PerfCheck(const std::string& name) : name(name), in(clock()) { + ; + } + + ~PerfCheck() { + const clock_t diff = (clock() - in); + add(name, diff); + } + + PerfCheck(PerfCheck const&) = delete; + + PerfCheck& operator = (PerfCheck const&) = delete; + + static void dump() { + for (const auto& it : map()) { + std::cout << it.first << ":\t"; + std::cout << "\tcalls: " << it.second.calls; + std::cout << "\ttime: " << it.second.time; + std::cout << "\ttime/call: " << ((double)it.second.time / (double)it.second.calls); + std::cout << std::endl; + } + } + +private: + + struct Stats { + size_t calls = 0; + clock_t time = 0; + }; + + static std::unordered_map& map() { + static std::unordered_map stats; + return stats; + } + + void add(const std::string& name, const clock_t diff) { + std::unordered_map& m = map(); + m[name].calls += 1; + m[name].time += diff; + } + +}; +*/ + +#endif // PERFCHECK_H diff --git a/navMesh/NavMeshDebug.h b/navMesh/NavMeshDebug.h index 682add0..deedec0 100644 --- a/navMesh/NavMeshDebug.h +++ b/navMesh/NavMeshDebug.h @@ -7,6 +7,7 @@ #include #include #include +#include #include namespace NM { @@ -18,27 +19,61 @@ namespace NM { public: - template static void show(NavMesh& nm) { + K::Gnuplot gp; + K::GnuplotSplot plot; + K::GnuplotSplotElementLines lines; + K::GnuplotSplotElementPoints border; + K::GnuplotSplotElementColorPoints particles; + K::GnuplotSplotElementLines pathEstimated; - 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) - }; + private: + + K::GnuplotFill gFill[6] = { + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#0000ff"), 1), // unknown + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"), 1), // indoor + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#44ffee"), 1), // outdoor + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666699"), 1), // door + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#444444"), 1), // stairs_level + K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"), 1) // stairs_skewed + }; + + public: + + NavMeshDebug() { + gp << "set view equal xy\n"; + plot.add(&lines); lines.setShowPoints(true); + plot.add(&border); + plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2); + plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00"); + + } + + void draw() { + gp.draw(plot); + gp.flush(); + } + + template void showParticles(const std::vector& particles) { + this->particles.clear(); + double min = +999; + double max = -999; + for (const T& p : particles) { + const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z); + const double prob = std::pow(p.weight, 0.25); + this->particles.add(p3, prob); + if (prob > max) {max = prob;} + if (prob < min) {min = prob;} + } + plot.getAxisCB().setRange(min, max + 0.000001); + } + template void addMesh(NavMesh& nm) { 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)); + border.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z)); + border.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)); @@ -47,7 +82,7 @@ namespace NM { for (const Tria* tria : nm) { const uint8_t type = tria->getType(); - if (type < 0 || type > 2) { + if (type < 0 || type > 5) { throw std::runtime_error("out of type-bounds"); } K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke); @@ -75,10 +110,15 @@ namespace NM { plot.getObjects().reOrderByZIndex(); - gp.draw(plot); - gp.flush(); - sleep(1); + } + + void setGT(const Point3 pt) { + gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.4) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n"; + } + + void setCurPos(const Point3 pt) { + gp << "set arrow 31338 from " << pt.x << "," << pt.y << "," << (pt.z+0.9) << " to " << pt.x << "," << pt.y << "," << pt.z << " lw 2 lc 'green' front \n"; } }; diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index 7a5e282..3a03ee3 100644 --- a/navMesh/NavMeshFactory.h +++ b/navMesh/NavMeshFactory.h @@ -6,10 +6,14 @@ #include "NavMesh.h" #include "NavMeshTriangle.h" +#include "NavMeshFactoryListener.h" +#include "NavMeshType.h" +#include "NavMeshSettings.h" #include "../lib/gpc/gpc.cpp.h" #include "../lib/Recast/Recast.h" + namespace NM { @@ -119,11 +123,7 @@ namespace NM { - enum SamplePartitionType { - SAMPLE_PARTITION_WATERSHED, - SAMPLE_PARTITION_MONOTONE, - SAMPLE_PARTITION_LAYERS, - }; + struct TriangleIn { Point3 p1; @@ -150,86 +150,172 @@ namespace NM { }; +#define NMF_STEPS 8 + template class NavMeshFactory { private: - float maxQuality_m = 0.20f; // 25cm elements are the smallest to-be-detected - NavMesh* dst = nullptr; + const NavMeshSettings& settings; + std::vector triangles; public: - NavMeshFactory(NavMesh* dst) : dst(dst) { + NavMeshFactory(NavMesh* dst, const NavMeshSettings& settings) : dst(dst), settings(settings) { } - void build(Floorplan::IndoorMap* map) { + void build(Floorplan::IndoorMap* map, NavMeshFactoryListener* listener = nullptr) { + if (listener) {listener->onNavMeshBuildUpdateMajor("preparing");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 0);} const BBox3 bbox = FloorplanHelper::getBBox(map); for (const Floorplan::Floor* floor : map->floors) { add(floor); } - fire(bbox); + fire(bbox, listener); } - /** get the smallest obstacle size that can be detected */ - float getMaxQuality_m() const { - return maxQuality_m; - } +// /** get the smallest obstacle size that can be detected */ +// float getMaxQuality_m() const { +// return maxQuality_m; +// } private: /** add one floor */ void add(const Floorplan::Floor* floor) { - NavMeshPoly nmPoly(floor->atHeight); + if (!floor->enabled) {return;} +// 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); +// } + + + + // we need this strange loop, as we need to distinguish between indoor and outdoor regions/polygons + // adding all "add" polygons first and removing "remove" polygons / obstacles afterwards is more performant + // but does not allow for tagging the "add" polygons (indoor/outdoor/...) + // thats why we have to tread each "add" polygon on its own (and remove all potential elements from it) for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { + + // if this is a to-be-added polygon, add it if (poly->method == Floorplan::OutlineMethod::ADD) { + + NavMeshPoly nmPoly(floor->atHeight); nmPoly.add(poly->poly); - } - } - for (Floorplan::FloorOutlinePolygon* poly : floor->outline) { - if (poly->method == Floorplan::OutlineMethod::REMOVE) { - nmPoly.remove(poly->poly); - } - } + // get all other polygons of this floor, that are tagged as "remove" and remove them (many will be outside of the added polygon) + 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)); - } - } + // get all obstacles of this floor and remove them from the polygon as well (many will be outside of the added polygon) + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleLine* line = dynamic_cast(obs); + if (line != nullptr) { + nmPoly.remove(getPolygon(line)); + } + } + + // construct and add + std::vector> tmp = nmPoly.get(); + int type = poly->outdoor ? (int) NavMeshType::FLOOR_OUTDOOR : (int) NavMeshType::FLOOR_INDOOR; + for (const std::vector& tria : tmp) { + const TriangleIn t(tria[0], tria[1], tria[2], type); + triangles.push_back(t); + } + + } - 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 + // those must be DIRECTLY connected to the ending floor (stair's ending edge connected to an edge of the floor) + // otherwise the stair ends UNDER a floor polygon and is thus not added (higher polygons always win) for (const Floorplan::Stair* stair : floor->stairs) { - const std::vector quads = Floorplan::getQuads(stair->getParts(), floor); + const std::vector quads = Floorplan::getQuads(stair->getParts(), floor); // slightly grow to ensure connection?! 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); + + // stair has two options: either leveled parts (no steps) and skewed parts (steps) + // as those affect the pedestrian's step-length, we tag them differently + const int type = quad.isLeveled() ? (int) NavMeshType::STAIR_LEVELED : (int) NavMeshType::STAIR_SKEWED; + const TriangleIn t1(quad.p1, quad.p2, quad.p3, type); + const TriangleIn t2(quad.p1, quad.p3, quad.p4, type); triangles.push_back(t1); triangles.push_back(t2); + + // sanity check. should never happen. just to be ultra sure + const Point3 norm1 = cross((t1.p2-t1.p1), (t1.p3-t1.p1)); + const Point3 norm2 = cross((t2.p2-t2.p1), (t2.p3-t2.p1)); + Assert::isTrue(norm1.z > 0, "detected invalid culling for stair-quad. normal points downwards"); + Assert::isTrue(norm2.z > 0, "detected invalid culling for stair-quad. normal points downwards"); + } } + // finally create additional triangles for the doors to tag doors differently (tagging also seems to improve the triangulation result) + // note: door-regions are already walkable as doors are NOT removed from the outline + // however: adding them again here seems to work.. triangles at the end of the list seem to overwrite (tagging) previous ones -> fine + { + + // add (overlay) all doors for tagging them within the plan + NavMeshPoly nmDoors(floor->atHeight); + for (Floorplan::FloorObstacle* obs : floor->obstacles) { + Floorplan::FloorObstacleDoor* door = dynamic_cast(obs); + if (door != nullptr) { + nmDoors.add(getPolygon(door)); + } + } + + // construct and add triangles + std::vector> tmp = nmDoors.get(); + for (const std::vector& tria : tmp) { + const TriangleIn t(tria[0], tria[1], tria[2], (int) NavMeshType::DOOR); + triangles.push_back(t); + } + + } + } - bool fire(BBox3 bbox) { + bool fire(BBox3 bbox, NavMeshFactoryListener* listener) { std::vector tData; std::vector vData; std::vector typeData; + if (listener) {listener->onNavMeshBuildUpdateMajor("building polygons");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 1);} + // floor outlines for (const TriangleIn& t : triangles) { @@ -282,37 +368,37 @@ namespace NM { rcPolyMeshDetail* m_dmesh; rcContext* m_ctx = new rcContext(); - float m_cellSize = maxQuality_m/2.0f; //0.3f; // ensure quality is enough to fit maxQuality_m - float m_cellHeight = maxQuality_m/2.0f; //0.2f; - float m_agentHeight = 2.0f; - float m_agentRadius = 0.2f;//0.6f; - float m_agentMaxClimb = maxQuality_m; // 0.9f; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail! - float m_agentMaxSlope = 45.0f; // elevator??? - 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.1f; //1.3f; // higher values allow joining some small triangles - float m_vertsPerPoly = 3;//6.0f; - float m_detailSampleDist = 6.0f; - float m_detailSampleMaxError = 1.0f;//1.0f; - int m_partitionType = SAMPLE_PARTITION_WATERSHED; // SAMPLE_PARTITION_WATERSHED SAMPLE_PARTITION_MONOTONE SAMPLE_PARTITION_LAYERS +// float m_cellSize = maxQuality_m/2.0f; //0.3f; // ensure quality is enough to fit maxQuality_m +// float m_cellHeight = maxQuality_m/2.0f; //0.2f; +// float m_agentHeight = 1.8f; +// float m_agentRadius = 0.2f;//0.6f; +// float m_agentMaxClimb = maxQuality_m; // 0.9f; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail! +// float m_agentMaxSlope = 45.0f; // elevator??? +// 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.1f; //1.3f; // higher values allow joining some small triangles +// float m_vertsPerPoly = 3;//6.0f; +// float m_detailSampleDist = 6.0f; +// float m_detailSampleMaxError = 1.0f;//1.0f; +// int m_partitionType = SAMPLE_PARTITION_WATERSHED; // SAMPLE_PARTITION_WATERSHED SAMPLE_PARTITION_MONOTONE SAMPLE_PARTITION_LAYERS // 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; + m_cfg.cs = settings.getCellSizeXY(); + m_cfg.ch = settings.getCellSizeZ(); + m_cfg.walkableSlopeAngle = settings.agentMaxSlope; + m_cfg.walkableHeight = (int)ceilf(settings.agentHeight / m_cfg.ch); + m_cfg.walkableClimb = (int)floorf(settings.getMaxClimb() / m_cfg.ch); + m_cfg.walkableRadius = (int)ceilf(settings.agentRadius / m_cfg.cs); + m_cfg.maxEdgeLen = (int)(settings.edgeMaxLen / settings.getCellSizeXY()); + m_cfg.maxSimplificationError = settings.edgeMaxError; + m_cfg.minRegionArea = (int)rcSqr(settings.regionMinSize); // Note: area = size*size + m_cfg.mergeRegionArea = (int)rcSqr(settings.regionMergeSize); // Note: area = size*size + m_cfg.maxVertsPerPoly = settings.vertsPerPoly; + m_cfg.detailSampleDist = settings.detailSampleDist < 0.9f ? 0 : settings.getCellSizeXY() * settings.detailSampleDist; + m_cfg.detailSampleMaxError = settings.getCellSizeZ() * settings.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? @@ -338,15 +424,16 @@ namespace NM { // Step 2. Rasterize input polygon soup. // + if (listener) {listener->onNavMeshBuildUpdateMajor("rasterizing polygons");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 2);} + // Allocate voxel heightfield where we rasterize our input data to. m_solid = rcAllocHeightfield(); - if (!m_solid) - { + 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)) - { + 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; } @@ -366,8 +453,7 @@ namespace NM { // 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)) - { + 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; } @@ -377,18 +463,9 @@ namespace NM { bool m_filterLedgeSpans = false; bool m_filterWalkableLowHeightSpans = false; - // std::vector! - // if (!m_keepInterResults) - // { - // delete [] m_triareas; - // m_triareas = 0; - // } - - // // Step 3. Filter walkables surfaces. - // - - + if (listener) {listener->onNavMeshBuildUpdateMajor("filtering");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 3);} // Once all geoemtry is rasterized, we do initial pass of filtering to // remove unwanted overhangs caused by the conservative rasterization @@ -401,30 +478,27 @@ namespace NM { rcFilterWalkableLowHeightSpans(m_ctx, m_cfg.walkableHeight, *m_solid); - // // Step 4. Partition walkable surface to simple regions. - // + if (listener) {listener->onNavMeshBuildUpdateMajor("partitioning");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 4);} // 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) - { + 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)) - { + 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) - { + //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)) @@ -465,103 +539,107 @@ namespace NM { // 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) - { + + switch (settings.partitionType) { + + case SamplePartitionType::SAMPLE_PARTITION_WATERSHED: + // Prepare for region partitioning, by calculating distance field along the walkable surface. - if (!rcBuildDistanceField(m_ctx, *m_chf)) - { + 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)) - { + 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) - { + break; + + case SamplePartitionType::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)) - { + 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 - { + + break; + + case SamplePartitionType::SAMPLE_PARTITION_LAYERS: + // Partition the walkable surface into simple regions without holes. - if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea)) - { + if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea)) { m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions."); return false; } + + break; + + default: + + throw Exception("unsupported SamplePartitionType"); + } - // // Step 5. Trace and simplify region contours. - // + if (listener) {listener->onNavMeshBuildUpdateMajor("tracing");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 5);} // Create contours. m_cset = rcAllocContourSet(); - if (!m_cset) - { + 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)) - { + 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. - // + if (listener) {listener->onNavMeshBuildUpdateMajor("building triangles");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 6);} // Build polygon navmesh from the contours. m_pmesh = rcAllocPolyMesh(); - if (!m_pmesh) - { + 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)) - { + 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. - // + if (listener) {listener->onNavMeshBuildUpdateMajor("building details");} + if (listener) {listener->onNavMeshBuildUpdateMajor(NMF_STEPS, 7);} m_dmesh = rcAllocPolyMeshDetail(); - if (!m_dmesh) - { + 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)) - { + 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) - { + //if (!m_keepInterResults) { rcFreeCompactHeightfield(m_chf); m_chf = 0; rcFreeContourSet(m_cset); m_cset = 0; - } + //} - std::vector res; + // std::vector res; const float* orig = m_pmesh->bmin; @@ -628,8 +706,7 @@ namespace NM { /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleLine* line) const { - //const Line2 base(line->from*100, line->to*100); - const float thickness_m = std::max(line->thickness_m, maxQuality_m); // wall's thickness (make thin walls big enough to be detected) + const float thickness_m = std::max(line->thickness_m, settings.maxQuality_m); // wall's thickness (make thin walls big enough to be detected) 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 @@ -644,6 +721,23 @@ namespace NM { return res; } + /** as line-obstacles have a thickness, we need 4 lines for the intersection test! */ + Floorplan::Polygon2 getPolygon(const Floorplan::FloorObstacleDoor* door) const { + const float thickness_m = std::max(0.3f, settings.maxQuality_m); // wall's thickness (make thin walls big enough to be detected) + const Point2 dir = (door->to - door->from); // obstacle's direction + const Point2 perp = dir.perpendicular().normalized(); // perpendicular direction (90 degree) + const Point2 p1 = door->from + perp * thickness_m/2; // start-up + const Point2 p2 = door->from - perp * thickness_m/2; // start-down + const Point2 p3 = door->to + perp * thickness_m/2; // end-up + const Point2 p4 = door->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; + } + }; } diff --git a/navMesh/NavMeshFactoryListener.h b/navMesh/NavMeshFactoryListener.h new file mode 100644 index 0000000..986ea45 --- /dev/null +++ b/navMesh/NavMeshFactoryListener.h @@ -0,0 +1,20 @@ +#ifndef NAVMESHFACTORYLISTENER_H +#define NAVMESHFACTORYLISTENER_H + +#include + +namespace NM { + + /** listen for events during the build process */ + class NavMeshFactoryListener { + + public: + + virtual void onNavMeshBuildUpdateMajor(const std::string& what) = 0; + virtual void onNavMeshBuildUpdateMajor(const int cnt, const int cur) = 0; + + }; + +} + +#endif // NAVMESHFACTORYLISTENER_H diff --git a/navMesh/NavMeshRandom.h b/navMesh/NavMeshRandom.h index b448cf2..391ed5c 100644 --- a/navMesh/NavMeshRandom.h +++ b/navMesh/NavMeshRandom.h @@ -5,6 +5,7 @@ #include #include "../math/DrawList.h" #include "../geo/Point3.h" +#include "../misc/PerfCheck.h" #include "NavMeshLocation.h" @@ -21,6 +22,7 @@ namespace NM { DrawList lst; std::minstd_rand gen; std::uniform_real_distribution dOnTriangle = std::uniform_real_distribution(0.0f, 1.0f); + std::uniform_real_distribution dHeading = std::uniform_real_distribution(0, M_PI*2); std::vector triangles; @@ -34,8 +36,8 @@ namespace NM { /** ctor (const/non-const using T) */ template NavMeshRandom(const std::vector& srcTriangles) : lst(nextSeed()), gen(nextSeed()) { - // almost always the same number?! - gen(); + // 1st = almost always the same number?! + gen(); gen(); // construct a DrawList (probability = size[area] of the triangle // bigger triangles must be choosen more often @@ -49,16 +51,56 @@ namespace NM { /** draw a random point */ NavMeshLocation draw() { + PERF_REGION(3, "NavMeshRandom::draw()"); + // pick a random triangle to draw from 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->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v); - return NavMeshLocation(pos, tria); + // get random (u,v) on triangle + float u = dOnTriangle(gen); + float v = dOnTriangle(gen); + + // if the (u,v) is outside of the triangle, mirror it so its inside the triangle again + if ((u+v) > 1) { + u = 1.0f - u; + v = 1.0f - v; + } + + // done + const Point3 pos = tria->getPoint(u,v); //tria->getA() + (tria.getAB() * u) + (tria.getAC() * v); + return NavMeshLocation(pos, tria); + + } + + /** draw a random location within the given radius */ + NavMeshLocation drawWithin(const Point3 center, const float radius) { + + std::uniform_real_distribution dDistance(0.001, radius); + + while(true) { + const float head = dHeading(gen); + const float dist = dDistance(gen); + + const float ox = std::cos(head) * dist; + const float oy = std::sin(head) * dist; + + // 2D destination (ignore z) + const Point2 dst(center.x + ox, center.y + oy); + + for (const Tria* t : triangles) { + + // if triangle contains 2D position + if (t->contains(dst)) { + + // convert it to a 3D position + const Point3 p3 = t->toPoint3(dst); + const NavMeshLocation loc(p3, t); + return loc; + + } + } + } } diff --git a/navMesh/NavMeshSettings.h b/navMesh/NavMeshSettings.h new file mode 100644 index 0000000..1ef3de7 --- /dev/null +++ b/navMesh/NavMeshSettings.h @@ -0,0 +1,63 @@ +#ifndef NAVMESHSETTINGS_H +#define NAVMESHSETTINGS_H + +namespace NM { + + enum class SamplePartitionType { + SAMPLE_PARTITION_WATERSHED, + SAMPLE_PARTITION_MONOTONE, + SAMPLE_PARTITION_LAYERS, + }; + + struct NavMeshSettings { + + + /** maximum resolution for outputs. nothing below this size will be detected (walls, doors, ..) */ + float maxQuality_m = 0.20f; + + + /** height of the walking person (used to delete regions below other regions) */ + float agentHeight = 1.8f; + + /** radius of the walking person (used to shrink the walkable area) */ + float agentRadius = 0.2f; + + /** the max angle (degree) the pedestrian is able to walk */ + float agentMaxSlope = 45.0f; // elevator??? + + + /** maximal size for one triangle. too high = too many samples when walking! */ + float edgeMaxLen = 10.0f; + + + /** higher values allow joining some small triangles */ + float edgeMaxError = 1.1f; //1.3f; + + /** algorithm choice */ + SamplePartitionType partitionType = SamplePartitionType::SAMPLE_PARTITION_WATERSHED; + + const float regionMinSize = 2;//8; // (isolated) regions smaller than this will not be rendered?! + const float regionMergeSize = 20; //?? + const int vertsPerPoly = 3;//6.0f; + const float detailSampleDist = 6.0f; + const float detailSampleMaxError = 1.0f;//1.0f; + + + float getCellSizeXY() const { + return maxQuality_m / 2.0f; + } + + float getCellSizeZ() const { + return maxQuality_m / 2.0f; + } + + /** allow jumping onto stairs from the side. usually we do not want this -> set it as low as possible */ + float getMaxClimb() const { + return maxQuality_m; // prevent jumping onto stairs from the side of the stair. setting this below 2xgrid-size will fail! + } + + }; + +} + +#endif // NAVMESHSETTINGS_H diff --git a/navMesh/NavMeshTriangle.h b/navMesh/NavMeshTriangle.h index ce5d129..60f8358 100644 --- a/navMesh/NavMeshTriangle.h +++ b/navMesh/NavMeshTriangle.h @@ -34,7 +34,7 @@ namespace NM { float dot00; float dot01; float dot11; - float invDenom; + double invDenom; float area; float minZ; @@ -66,6 +66,44 @@ namespace NM { Point3 getP3() const {return p3;} + /** get the distance between the given point and the triangle using approximate tests */ + float getDistanceApx(const Point3 pt) const { + +// const float d1 = pt.getDistance(p1); +// const float d2 = pt.getDistance(p2); +// const float d3 = pt.getDistance(p3); +// const float d4 = pt.getDistance(center); +// const float d5 = pt.getDistance((p1-p2)/2); +// const float d6 = pt.getDistance((p2-p3)/2); +// const float d7 = pt.getDistance((p3-p1)/2); +// return std::min(d1, std::min(d2, std::min(d3, std::min(d4, std::min(d5, std::min(d6,d7)))))); + +// const float d1 = pt.getDistance(p1); +// const float d2 = pt.getDistance(p2); +// const float d3 = pt.getDistance(p3); +// const float d4 = pt.getDistance(center); +// return std::min(d1, std::min(d2, std::min(d3,d4))); + + float bestD = 99999; + Point3 bestP; + Point3 dir12 = p2-p1; + Point3 dir13 = p3-p1; + Point3 dir23 = p3-p2; + for (float f = 0; f < 1; f += 0.05f) { + const Point3 pos1 = p1 + dir12 * f; const float dist1 = pos1.getDistance(pt); + const Point3 pos2 = p1 + dir13 * f; const float dist2 = pos2.getDistance(pt); + const Point3 pos3 = p2 + dir23 * f; const float dist3 = pos3.getDistance(pt); + if (dist1 < bestD) {bestP = pos1; bestD = dist1;} + if (dist2 < bestD) {bestP = pos2; bestD = dist2;} + if (dist3 < bestD) {bestP = pos3; bestD = dist3;} + } + + return bestD; + + + } + + bool operator == (const NavMeshTriangle& o) const { return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3); } @@ -122,7 +160,11 @@ namespace NM { float v = (dot00 * dot12 - dot01 * dot02) * invDenom; const Point3 res = getPoint(v,u); - return res; + Assert::isNear(res.x, p.x, 1.0f, "TODO: high difference while mapping from 2D to 3D"); + Assert::isNear(res.y, p.y, 1.0f, "TODO: high difference while mapping from 2D to 3D"); + + //return res; + return Point3(p.x, p.y, res.z); // only use the new z, keep input as-is } @@ -159,7 +201,7 @@ namespace NM { dot11 = dot(v1, v1); // Compute barycentric coordinates - invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); + invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01); diff --git a/navMesh/NavMeshType.h b/navMesh/NavMeshType.h new file mode 100644 index 0000000..19f1352 --- /dev/null +++ b/navMesh/NavMeshType.h @@ -0,0 +1,22 @@ +#ifndef NAVMESHTYPE_H +#define NAVMESHTYPE_H + +namespace NM { + enum class NavMeshType { + + UNWALKABLE, // needed by Recast + + FLOOR_INDOOR, + FLOOR_OUTDOOR, + + DOOR, + + STAIR_LEVELED, // eben + STAIR_SKEWED, // schraeg + + ELEVATOR, + + }; +} + +#endif // NAVMESHTYPE_H diff --git a/navMesh/walk/NavMeshSub.h b/navMesh/walk/NavMeshSub.h index 57aa0e5..5b084f3 100644 --- a/navMesh/walk/NavMeshSub.h +++ b/navMesh/walk/NavMeshSub.h @@ -28,6 +28,14 @@ namespace NM { return false; } + /** does this submesh contain the given point? */ + bool contains(const Point3 p3) const { + for (const Tria* t : toVisit) { + if (t->contains(p3)) {return true;} + } + return false; + } + /** get the triangle that contains the given point (if any) */ const Tria* getContainingTriangle(const Point2 p2) const { for (const Tria* t : toVisit) { @@ -37,24 +45,36 @@ namespace NM { } /** perform random operations on the submesh */ - NavMeshRandom getRandom() { + NavMeshRandom getRandom() const { return NavMeshRandom(toVisit); } + + /** allows for-each iteration over all included triangles */ + decltype(toVisit.begin()) begin() {return toVisit.begin();} + + /** allows for-each iteration over all included triangles */ + decltype(toVisit.end()) end() {return toVisit.end();} + + + private: void build(const NavMeshLocation& loc, float radius_m) { + PERF_REGION(6, "NavMeshSub::build()"); + std::unordered_set visited; // starting-triangle + all its (max 3) neighbors toVisit.push_back(loc.tria); visited.insert(loc.tria); - for (const auto* n : *loc.tria) { - toVisit.push_back( (const Tria*)n ); - } +// for (const auto* n : *loc.tria) { +// toVisit.push_back( (const Tria*)n ); +// } +// size_t next = 1; // start with the first neighbor (skip starting triangle itself) - size_t next = 1; // start with the first neighbor (skip starting triangle itself) + size_t next = 0; while (next < toVisit.size()) { // next triangle @@ -63,7 +83,8 @@ namespace NM { // neighbors for (const auto* n : *cur) { const Tria* t = (const Tria*) n; - const float dist = loc.pos.getDistance(n->getCenter()); + //const float dist = loc.pos.getDistance(n->getCenter()); + const float dist = n->getDistanceApx(loc.pos); if (dist > radius_m) {continue;} if (visited.find(t) != visited.end()) {continue;} toVisit.push_back(t); diff --git a/navMesh/walk/NavMeshWalkEval.h b/navMesh/walk/NavMeshWalkEval.h index e9e79ef..bae5e10 100644 --- a/navMesh/walk/NavMeshWalkEval.h +++ b/navMesh/walk/NavMeshWalkEval.h @@ -4,6 +4,7 @@ #include "NavMeshWalkParams.h" #include "../NavMeshLocation.h" #include "../../math/Distributions.h" +#include "../../misc/PerfCheck.h" namespace NM { @@ -13,6 +14,10 @@ namespace NM { NavMeshLocation end; + NavMeshPotentialWalk(const NavMeshWalkParams& requested) : requested(requested) { + ; + } + NavMeshPotentialWalk(const NavMeshWalkParams& requested, const NavMeshLocation& end) : requested(requested), end(end) { ; } @@ -57,10 +62,9 @@ namespace NM { virtual double getProbability(const NavMeshPotentialWalk& walk) const override { - if (walk.requested.start.pos == walk.end.pos) { - std::cout << "warn! start-position == end-positon" << std::endl; - return 0; - } + PERF_REGION(4, "WalkEvalHeadingStartEnd"); + + Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position"); const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy()); const float diff = head.getDiffHalfRAD(walk.requested.heading); @@ -72,6 +76,38 @@ namespace NM { }; + /** + * evaluate the difference between head(start,end) and the requested heading + */ + template class WalkEvalHeadingStartEndNormal : public NavMeshWalkEval { + + const double sigma_rad; + Distribution::Normal dist; + + public: + + WalkEvalHeadingStartEndNormal(const double sigma_rad = 0.04) : + sigma_rad(sigma_rad), dist(0, sigma_rad) { + ; + } + + virtual double getProbability(const NavMeshPotentialWalk& walk) const override { + + PERF_REGION(4, "WalkEvalHeadingStartEnd"); + + Assert::notEqual(walk.requested.start.pos, walk.end.pos, "start equals end position"); + + const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy()); + const float diff = head.getDiffHalfRAD(walk.requested.heading); + //const float diff = Heading::getSignedDiff(params.heading, head); + //return Distribution::Normal::getProbability(0, sigma, diff); + return dist.getProbability(diff); + + } + + }; + + /** * evaluate the difference between distance(start, end) and the requested distance */ @@ -87,6 +123,8 @@ namespace NM { virtual double getProbability(const NavMeshPotentialWalk& walk) const override { + PERF_REGION(5, "WalkEvalDistance"); + const float requestedDistance_m = walk.requested.getToBeWalkedDistance(); const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos); const float diff = walkedDistance_m - requestedDistance_m; diff --git a/navMesh/walk/NavMeshWalkParams.h b/navMesh/walk/NavMeshWalkParams.h index 1e8d8f0..b569fbc 100644 --- a/navMesh/walk/NavMeshWalkParams.h +++ b/navMesh/walk/NavMeshWalkParams.h @@ -3,6 +3,7 @@ #include "../../geo/Heading.h" #include "../NavMeshLocation.h" +#include "../NavMeshType.h" namespace NM { @@ -20,12 +21,18 @@ namespace NM { Assert::isTrue(isValid(), "invalid step-sizes given"); - if (start.tria->isPlain()) { - return stepSizeFloor_m * steps; - } else { + if (start.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) { return stepSizeStair_m * steps; + } else { + return stepSizeFloor_m * steps; } +// if (start.tria->isPlain()) { +// return stepSizeFloor_m * steps; +// } else { +// return stepSizeStair_m * steps; +// } + } }; @@ -36,9 +43,6 @@ namespace NM { /** walk starts here (pos/tria) */ NavMeshLocation start; -// /** to-be-walked distance */ -// float distance_m; - /** direction to walk to */ Heading heading; @@ -54,9 +58,17 @@ namespace NM { /** get the to-be-walked distance (steps vs. current location [stair/floor/..]) */ float getToBeWalkedDistance() const { - return stepSizes.inMeter(numSteps, start); + if (_toBeWalkedDistance != _toBeWalkedDistance) { + _toBeWalkedDistance = stepSizes.inMeter(numSteps, start); + } + return _toBeWalkedDistance; } + private: + + // precalc + mutable float _toBeWalkedDistance = NAN; + }; } diff --git a/navMesh/walk/NavMeshWalkRandom.h b/navMesh/walk/NavMeshWalkRandom.h new file mode 100644 index 0000000..db625c4 --- /dev/null +++ b/navMesh/walk/NavMeshWalkRandom.h @@ -0,0 +1,146 @@ +#ifndef NAVMESHWALKRANDOM_H +#define NAVMESHWALKRANDOM_H + + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" + +#include "NavMeshSub.h" +#include "NavMeshWalkParams.h" +#include "NavMeshWalkEval.h" + +namespace NM { + + /** + * pick a truely random destination within the reachable area + * weight this area (evaluators) + * repeat this several times to find a robus destination + */ + template class NavMeshWalkRandom { + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + public: + + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + struct ResultList : std::vector {}; + + public: + + /** ctor */ + NavMeshWalkRandom(const NavMesh& mesh) : mesh(mesh) { + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* eval) { + this->evals.push_back(eval); + } + + ResultEntry getOne(const NavMeshWalkParams& params) const { + + ResultEntry res; + res.probability = 0; + + // to-be-walked distance; + const float toBeWalkedDist = params.getToBeWalkedDistance(); + const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1; + + // construct reachable region + const NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + NavMeshRandom rnd = reachable.getRandom(); + NavMeshPotentialWalk pwalk(params); + + // improve quality (the higher, the better) + for (int i = 0; i < 25; ++i) { + + PERF_REGION(1, "NavMeshWalkRandom::SampleLoop"); + + // draw a random destination + // is this destination within the reachable area? (triangles might be larger!) + pwalk.end = rnd.draw(); + if (pwalk.end.pos.getDistance(params.start.pos) > toBeWalkedDistSafe) { + --i; continue; + } + + // calculate the probability for this destination + const double p = eval(pwalk); + + // better? + if (p > res.probability) { + res.location = pwalk.end; + res.probability = p; + } + + } + + // destination is known. update the heading + res.heading = Heading(params.start.pos.xy(), res.location.pos.xy()); + return res; + + } + + ResultList getMany(const NavMeshWalkParams& params) const { + + ResultList res; + + // to-be-walked distance; + const float toBeWalkedDist = params.getToBeWalkedDistance(); + const float toBeWalkedDistSafe = 1.0 + toBeWalkedDist * 1.1; + + // construct reachable region + const NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + NavMeshRandom rnd = reachable.getRandom(); + NavMeshPotentialWalk pwalk(params); + + // improve quality (the higher, the better) + for (int i = 0; i < 25; ++i) { + + PERF_REGION(1, "NavMeshWalkRandom::SampleLoop"); + + pwalk.end = rnd.drawWithin(params.start.pos, toBeWalkedDistSafe); + + // calculate the probability for this destination + const double p = eval(pwalk); + + ResultEntry re; + re.heading = Heading(params.start.pos.xy(), pwalk.end.pos.xy()); + re.location = pwalk.end; + re.probability = p; + res.push_back(re); + + } + + return res; + + } + + double eval(const NM::NavMeshPotentialWalk& pwalk) const { + PERF_REGION(2, "NavMeshWalkRandom::EvalLoop"); + double p = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + p *= p1; + } + return p; + } + + + }; + +} + +#endif // NAVMESHWALKRANDOM_H diff --git a/navMesh/walk/NavMeshWalkSemiRandom.h b/navMesh/walk/NavMeshWalkSemiRandom.h new file mode 100644 index 0000000..b3c4866 --- /dev/null +++ b/navMesh/walk/NavMeshWalkSemiRandom.h @@ -0,0 +1,169 @@ +#ifndef NAVMESHWALKSEMIRANDOM_H +#define NAVMESHWALKSEMIRANDOM_H + +#include "../NavMesh.h" +#include "../NavMeshLocation.h" +#include "../../geo/Heading.h" + +#include "NavMeshSub.h" +#include "NavMeshWalkParams.h" +#include "NavMeshWalkEval.h" + + +namespace NM { + + /** + * similar to NavMeshWalkRandom but: + * pick a semi random destination within the reachable area (requested distance/heading + strong deviation) + * if this destination is reachable: + * weight this area (evaluators) + * repeat this some times to find a robus destination + */ + template class NavMeshWalkSemiRandom { + + private: + + const NavMesh& mesh; + + std::vector*> evals; + + public: + + + struct ResultEntry { + NavMeshLocation location; + Heading heading; + double probability; + ResultEntry() : heading(0) {;} + }; + + struct ResultList : public std::vector {}; + + public: + + /** ctor */ + NavMeshWalkSemiRandom(const NavMesh& mesh) : mesh(mesh) { + + } + + /** add a new evaluator to the walker */ + void addEvaluator(NavMeshWalkEval* eval) { + this->evals.push_back(eval); + } + + ResultEntry getOne(const NavMeshWalkParams& params) const { + + static Distribution::Normal dDist(1.0, 0.4); + static Distribution::Normal dHead(0.0, 1.0); + + // construct reachable region + const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1; + const NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + ResultEntry re; + + NavMeshPotentialWalk pwalk(params); + pwalk.end = reachable.getRandom().draw(); // to have at least a non-start solution + re.probability = eval(pwalk); + re.location = pwalk.end; + + + for (int i = 0; i < 25; ++i) { + + const float distance = params.getToBeWalkedDistance() * dDist.draw(); + const Heading head = params.heading + dHead.draw(); + + // only forward! + if (distance < 0.01) {continue;} + + // get the to-be-reached destination's position (using start+distance+heading) + const Point2 dir = head.asVector(); + const Point2 dst = params.start.pos.xy() + (dir * distance); + + const Tria* dstTria = reachable.getContainingTriangle(dst); + + // is above destination reachable? + if (dstTria) { + + pwalk.end.pos = dstTria->toPoint3(dst); + pwalk.end.tria = dstTria; + const double p = eval(pwalk); + + // better? + if (p > re.probability) { + re.location = pwalk.end; + re.probability = p; + re.heading = head; + } + + } + + } + + return re; + + } + + ResultList getMany(const NavMeshWalkParams& params) const { + + static Distribution::Normal dDist(1.0, 0.4); + static Distribution::Normal dHead(0.0, 1.0); + + ResultList res; + + // construct reachable region + const float toBeWalkedDistSafe = 1.0 + params.getToBeWalkedDistance() * 1.1; + const NavMeshSub reachable(params.start, toBeWalkedDistSafe); + + NavMeshPotentialWalk pwalk(params); + + for (int i = 0; i < 25; ++i) { + + const float distance = params.getToBeWalkedDistance() * dDist.draw(); + const Heading head = params.heading + dHead.draw(); + + // only forward! + if (distance < 0.01) {continue;} + + // get the to-be-reached destination's position (using start+distance+heading) + const Point2 dir = head.asVector(); + const Point2 dst = params.start.pos.xy() + (dir * distance); + + const Tria* dstTria = reachable.getContainingTriangle(dst); + + // is above destination reachable? + if (dstTria) { + + pwalk.end.pos = dstTria->toPoint3(dst); + pwalk.end.tria = dstTria; + const double p = eval(pwalk); + + ResultEntry re; + re.location = pwalk.end; + re.probability = p; + re.heading = head; + res.push_back(re); + + } + + } + + return res; + + } + + double eval(const NM::NavMeshPotentialWalk& pwalk) const { + double p = 1.0; + for (const NavMeshWalkEval* eval : evals) { + const double p1 = eval->getProbability(pwalk); + p *= p1; + } + return p; + } + + + }; + +} + +#endif // NAVMESHWALKSEMIRANDOM_H diff --git a/navMesh/walk/NavMeshWalkSimple.h b/navMesh/walk/NavMeshWalkSimple.h index 86f49a9..4184ba0 100644 --- a/navMesh/walk/NavMeshWalkSimple.h +++ b/navMesh/walk/NavMeshWalkSimple.h @@ -25,16 +25,17 @@ namespace NM { public: - struct Result { - + /** single result */ + struct ResultEntry { NavMeshLocation location; Heading heading; double probability; - - Result() : heading(0) {;} - + ResultEntry() : heading(0) {;} }; + /** list of results */ + using ResultList = std::vector; + public: /** ctor */ @@ -47,10 +48,11 @@ namespace NM { this->evals.push_back(eval); } - Result getDestination(const NavMeshWalkParams& params) { - Result res; - res.heading = params.heading; + + ResultEntry getOne(const NavMeshWalkParams& params) { + + ResultEntry re; // to-be-walked distance; const float toBeWalkedDist = params.getToBeWalkedDistance(); @@ -60,7 +62,7 @@ namespace NM { NavMeshSub reachable(params.start, toBeWalkedDistSafe); // get the to-be-reached destination's position (using start+distance+heading) - const Point2 dir = res.heading.asVector(); + const Point2 dir = params.heading.asVector(); const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist); const Tria* dstTria = reachable.getContainingTriangle(dst); @@ -68,16 +70,16 @@ namespace NM { // is above destination reachable? if (dstTria) { - res.location.pos = dstTria->toPoint3(dst); - res.location.tria = dstTria; + re.heading = params.heading; // heading was OK -> keep + re.location.pos = dstTria->toPoint3(dst); // new destination position + re.location.tria = dstTria; // new destination triangle ++hits; } else { - NavMeshRandom rnd = reachable.getRandom(); - NavMeshLocation rndLoc = rnd.draw(); - res.location = rndLoc; - res.heading = Heading(params.start.pos.xy(), rndLoc.pos.xy()); // update the heading + NavMeshRandom rnd = reachable.getRandom(); // random-helper + re.location = rnd.draw(); // get a random destianation + re.heading = Heading(params.start.pos.xy(), re.location.pos.xy()); // update the heading ++misses; } @@ -87,17 +89,23 @@ namespace NM { std::cout << "hits: " << (hits*100/total) << "%" << std::endl; } - const NavMeshPotentialWalk pwalk(params, res.location); - res.probability = 1.0; + // calculate probability + const NavMeshPotentialWalk pwalk(params, re.location); + re.probability = 1.0; for (const NavMeshWalkEval* eval : evals) { const double p1 = eval->getProbability(pwalk); - res.probability *= p1; + re.probability *= p1; } - return res; + // done + return re; } + ResultList getMany(const NavMeshWalkParams& params) { + return {getOne(params)}; + } + }; diff --git a/synthetic/SyntheticPath.h b/synthetic/SyntheticPath.h index f173852..0fbf933 100644 --- a/synthetic/SyntheticPath.h +++ b/synthetic/SyntheticPath.h @@ -111,6 +111,10 @@ public: return Base::get(distance); } + bool doneAtDistance(const float distance) const { + return Base::getMaxKey() < distance; + } + /** at the given distance: are we walking on a plain surface or up/down? */ bool isPlain(const float distance) const { const Point3 pos1 = getPosAfterDistance(distance); diff --git a/synthetic/SyntheticSteps.h b/synthetic/SyntheticSteps.h index a24efd7..b0adf50 100644 --- a/synthetic/SyntheticSteps.h +++ b/synthetic/SyntheticSteps.h @@ -55,7 +55,7 @@ private: public: /** ctor with the walker to follow */ - SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeStair_m = 0.3, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) : + SyntheticSteps(SyntheticWalker* walker, const float stepSize_m = 0.7, const float stepSizeStair_m = 0.35, const float stepSizeSigma_m = 0.1, const float noiseLevel = 0.33) : //stepSize_m(stepSize_m), drift(drift), stepSizeSigma_m(stepSizeSigma_m), noiseLevel(noiseLevel), dNextStep(stepSize_m, stepSizeSigma_m), dNextStepStair(stepSizeStair_m, stepSizeSigma_m) { diff --git a/synthetic/SyntheticWalker.h b/synthetic/SyntheticWalker.h index b4fadfb..17dcc7a 100644 --- a/synthetic/SyntheticWalker.h +++ b/synthetic/SyntheticWalker.h @@ -49,6 +49,10 @@ public: this->listeners.push_back(l); } + bool done() { + return path.doneAtDistance(this->walkedDistance); + } + /** increment the walk */ Point3 tick(const Timestamp timePassed) { diff --git a/tests/navMesh/TestNavMeshBenchmark.cpp b/tests/navMesh/TestNavMeshBenchmark.cpp new file mode 100644 index 0000000..5ca8482 --- /dev/null +++ b/tests/navMesh/TestNavMeshBenchmark.cpp @@ -0,0 +1,101 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../navMesh/NavMeshFactory.h" +#include "../../navMesh/walk/NavMeshSub.h" +using namespace NM; + +TEST(NavMeshBenchmark, benchDraw) { + + 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); + + // circle (many triangles) + int i = 0; + for (float f = 0; f < M_PI*2; f += 0.1) { + const float x = std::cos(f) * 10; + const float y = std::sin(f) * 10; + outline.poly.points.push_back(Point2(x,y)); + ++i; + } + + outline.outdoor = false; + outline.method = Floorplan::OutlineMethod::ADD; + + NavMeshSettings set; + NavMesh nm; + NavMeshFactory fac(&nm, set); + fac.build(&map); + + ASSERT_NEAR(-10, nm.getBBox().getMin().x, 0.5); + ASSERT_NEAR(-10, 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(45, nm.getNumTriangles()); + + + NavMeshRandom rnd = nm.getRandom(); + + for (int i = 0; i < 5000*1000; ++i) { + NavMeshLocation loc = rnd.draw(); + } + +} + + + +TEST(NavMeshBenchmark, benchSubRegion) { + + 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); + + // circle (many triangles) + int i = 0; + for (float f = 0; f < M_PI*2; f += 0.1) { + const float x = std::cos(f) * 10; + const float y = std::sin(f) * 10; + outline.poly.points.push_back(Point2(x,y)); + ++i; + } + + outline.outdoor = false; + outline.method = Floorplan::OutlineMethod::ADD; + + NavMeshSettings set; + NavMesh nm; + NavMeshFactory fac(&nm, set); + fac.build(&map); + + ASSERT_NEAR(-10, nm.getBBox().getMin().x, 0.5); + ASSERT_NEAR(-10, 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(45, nm.getNumTriangles()); + + std::minstd_rand gen(1337); + std::uniform_real_distribution dist(0, M_PI*2); + + for (int i = 0; i < 50000; ++i) { + const float f = dist(gen); + const float x = std::cos(f) * 9; + const float y = std::sin(f) * 9; + NavMeshLocation loc = nm.getLocation(Point3(x,y,0)); + NavMeshSub(loc, 5); + } + +} + + + +#endif diff --git a/tests/navMesh/TestNavMeshFactory.cpp b/tests/navMesh/TestNavMeshFactory.cpp index 4362f60..c5dba81 100644 --- a/tests/navMesh/TestNavMeshFactory.cpp +++ b/tests/navMesh/TestNavMeshFactory.cpp @@ -17,8 +17,9 @@ TEST(NavMeshFactory, build1) { outline.outdoor = false; outline.method = Floorplan::OutlineMethod::ADD; + NavMeshSettings set; NavMesh nm; - NavMeshFactory fac(&nm); + NavMeshFactory fac(&nm,set); fac.build(&map); ASSERT_NEAR(0, nm.getBBox().getMin().x, 0.5); diff --git a/tests/navMesh/TestNavMeshSub.cpp b/tests/navMesh/TestNavMeshSub.cpp index 2309cb5..27c7cbe 100644 --- a/tests/navMesh/TestNavMeshSub.cpp +++ b/tests/navMesh/TestNavMeshSub.cpp @@ -18,11 +18,68 @@ TEST(NavMeshSub, build1) { outline.outdoor = false; outline.method = Floorplan::OutlineMethod::ADD; + NavMeshSettings set; NavMesh nm; - NavMeshFactory fac(&nm); + NavMeshFactory fac(&nm, set); fac.build(&map); - NavMeshLocation loc = nm.getLocation(Point3(1,1,1)); + nm.getLocation(Point3(1,1,0)); + nm.getLocation(Point3(8,0.2,0)); + nm.getLocation(Point3(0.2,8,0)); + nm.getLocation(Point3(4.5,4.5,0)); + + + + +} + +TEST(NavMeshSub, draw) { + + 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.outdoor = false; + outline.method = Floorplan::OutlineMethod::ADD; + + // circle (many triangles) + int i = 0; + for (float f = 0; f < M_PI*2; f += 0.1) { + const float x = std::cos(f) * 10; + const float y = std::sin(f) * 10; + outline.poly.points.push_back(Point2(x,y)); + ++i; + } + + + Floorplan::FloorOutlinePolygon remove; floor.outline.push_back(&remove); + remove.outdoor = false; + remove.method = Floorplan::OutlineMethod::REMOVE; + remove.poly.points.push_back(Point2(-2,-2)); + remove.poly.points.push_back(Point2(+2,-2)); + remove.poly.points.push_back(Point2(+2,+2)); + remove.poly.points.push_back(Point2(-2,+2)); + + NavMeshSettings set; + NavMesh nm; + NavMeshFactory fac(&nm, set); + fac.build(&map); + + NavMeshRandom rnd = nm.getRandom(); + + for (int i = 0; i < 1000; ++i) { + NavMeshLocation loc = rnd.draw(); + ASSERT_TRUE(loc.tria->contains(loc.pos)); + + NavMeshSub sub2(loc, 5); + NavMeshRandom rnd2 = sub2.getRandom(); + for (int j = 0; j < 100; ++j) { + NavMeshLocation loc2 = rnd2.draw(); + ASSERT_TRUE(loc2.tria->contains(loc2.pos)); + ASSERT_TRUE(sub2.contains(loc2.pos)); + } + + } + }