FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

worked on grid-walking

Browse Source 
worked on grid-generation
added helper library for nav-meshes
started working on nav meshes
This commit is contained in:
FrankE
2018-01-08 20:55:50 +01:00
parent c346b7f222
commit ca6fed5371
33 changed files with 12991 additions and 1913 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
grid/Grid.h
View File

@@ -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
}
};

33
grid/factory/v3/GridFactory3.h
View File

@@ -4,7 +4,20 @@
#include "../../Grid.h"
#include "../../../floorplan/v2/Floorplan.h"
#include "HelperPoly3.h"
#include <unordered_set>
#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 <typename Node> 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);

2
grid/walk/v3/Walker.h
View File

@@ -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;

511
lib/Recast/Recast.cpp Normal file
View File

@@ -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 <float.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdarg.h>
#include <new>
#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;
}
*/

1206
lib/Recast/Recast.h Normal file
View File

File diff suppressed because it is too large Load Diff

86
lib/Recast/RecastAlloc.cpp Normal file
View File

@@ -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 <stdlib.h>
#include <string.h>
#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;
}

146
lib/Recast/RecastAlloc.h Normal file
View File

@@ -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 <stddef.h>
/// 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 T> 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

591
lib/Recast/RecastArea.cpp Normal file
View File

@@ -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 <float.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#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;
}
}
}
}
}
}

35
lib/Recast/RecastAssert.cpp Normal file
View File

@@ -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

56
lib/Recast/RecastAssert.h Normal file
View File

@@ -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 <assert.h>
# define rcAssert(expression) \
{ \
rcAssertFailFunc* failFunc = rcAssertFailGetCustom(); \
if(failFunc == NULL) { assert(expression); } \
else if(!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \
}
#endif
#endif // RECASTASSERT_H

1105
lib/Recast/RecastContour.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

202
lib/Recast/RecastFilter.cpp Normal file
View File

@@ -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 <math.h>
#include <stdio.h>
#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: <tt>rcAbs(currentSpan.smax - neighborSpan.smax) < waklableClimb</tt>
///
/// @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: <tt>rcAbs(currentSpan.smax - neighborSpan.smax) > walkableClimb</tt>
///
/// @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;
}
}
}
}

644
lib/Recast/RecastLayers.cpp Normal file
View File

@@ -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 <float.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#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<unsigned char> 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<rcLayerSweepSpan> 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<rcLayerRegion> 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<<dir);
// Update height so that it matches on both sides of the portal.
const rcCompactSpan& as = chf.spans[ai];
if (as.y > 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<<dir);
}
}
}
layer->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;
}

1552
lib/Recast/RecastMesh.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1462
lib/Recast/RecastMeshDetail.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

454
lib/Recast/RecastRasterization.cpp Normal file
View File

@@ -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 <math.h>
#include <stdio.h>
#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<nvrow; ++i)
{
if (minX > 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;
}

1824
lib/Recast/RecastRegion.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

3080
lib/gpc/gpc.cpp.h
View File

File diff suppressed because it is too large Load Diff

4
main.cpp
View File

@@ -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*";

99
navMesh/NavMesh.h Normal file
View File

@@ -0,0 +1,99 @@
#ifndef NAV_MESH_H
#define NAV_MESH_H
#include "NavMeshTriangle.h"
#include <vector>
#include "../geo/BBox3.h"
#include <random>
#include "../math/DrawList.h"
#include "NavMeshRandom.h"
template <typename Tria> class NavMesh {
/** all triangles within the mesh */
std::vector<Tria> 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<Tria> getRandomizer() {
return NavMeshRandom<Tria>(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

75
navMesh/NavMeshDebug.h Normal file
View File

@@ -0,0 +1,75 @@
#ifndef NAVMESHDEBUG_H
#define NAVMESHDEBUG_H
#include "NavMesh.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
class NavMeshDebug {
public:
template <typename Tria> static void show(NavMesh<Tria>& 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

573
navMesh/NavMeshFactory.h Normal file
View File

@@ -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 <typename Tria> class NavMeshFactory {
private:
NavMesh<Tria>* dst = nullptr;
std::vector<TriangleIn> triangles;
public:
NavMeshFactory(NavMesh<Tria>* 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<Floorplan::FloorObstacleLine*>(obs);
if (line != nullptr) {
nmPoly.remove(getPolygon(line));
}
}
std::vector<std::vector<Point3>> tmp = nmPoly.get();
for (const std::vector<Point3>& 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<Floorplan::Quad3> 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<int> tData;
std::vector<float> vData;
std::vector<uint8_t> 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<TriangleOut> 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 <tt>2 * #nvp</tt> 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<Point3> 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<Point3> 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

123
navMesh/NavMeshPoly.h Normal file
View File

@@ -0,0 +1,123 @@
#ifndef POLYGON_H
#define POLYGON_H
#include <Indoor/floorplan/v2/Floorplan.h>
#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<std::vector<Point3>> 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<std::vector<Point3>> 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<Point3> 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<gpc_vertex> 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

49
navMesh/NavMeshRandom.h Normal file
View File

@@ -0,0 +1,49 @@
#ifndef NAVMESHRANDOM_H
#define NAVMESHRANDOM_H
#include <random>
#include <vector>
#include "../math/DrawList.h"
#include "../geo/Point3.h"
template <typename Tria> class NavMeshRandom {
std::minstd_rand gen;
std::uniform_real_distribution<float> dOnTriangle = std::uniform_real_distribution<float>(0.0f, 1.0f);
const std::vector<Tria>& triangles;
DrawList<size_t> 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<Tria>& 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

126
navMesh/NavMeshTriangle.h Normal file
View File

@@ -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<typename> 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

43
navMesh/walk/NavMeshWalkSimple.h Normal file
View File

@@ -0,0 +1,43 @@
#ifndef NAVMESHWALKSIMPLE_H
#define NAVMESHWALKSIMPLE_H
#include "../NavMesh.h"
template <typename Tria> class NavMeshWalkSimpel {
private:
const NavMesh<Tria>& 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<Tria>& mesh) : mesh(mesh) {
}
Result walk(const Params& params) {
}
}
#endif // NAVMESHWALKSIMPLE_H

2
sensors/imu/MagnetometerData.h
View File

@@ -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));
}
}

298
tests/math/filter/TestButter.cpp
View File

@@ -5,7 +5,7 @@
#include "../../../misc/Time.h"
#include "../../../math/Interpolator.h"
#include"../../../sensors/pressure/ActivityButterPressure.h"
#include"../../../sensors/activity/ActivityButterPressure.h"
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
@@ -23,234 +23,234 @@
TEST(Butterworth, offlineSinus) {
//input data
std::minstd_rand gen;
std::uniform_real_distribution<double> noise (-0.1, +0.1);
//input data
std::minstd_rand gen;
std::uniform_real_distribution<double> 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<double> butter(size,20,5);
butter.stepInitialization(0);
butter.filter(input, output, size, 0, true);
//butterworth
Filter::ButterworthLP<double> 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<double> butter(size,20,5);
butter.stepInitialization(0);
Filter::ButterworthLP<double> butter(size,20,5);
butter.stepInitialization(0);
//input data
std::minstd_rand gen;
std::uniform_real_distribution<double> noise (-0.1, +0.1);
//input data
std::minstd_rand gen;
std::uniform_real_distribution<double> 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<int, double> interp;
Interpolator<int, double> 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<double> butter(50,0.2,2);
butter.filter(input, output, counter, 938.15, true);
Filter::ButterworthLP<double> 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<double> input;
std::vector<double> output;
std::vector<double> input;
std::vector<double> output;
Interpolator<int, double> interp;
Interpolator<int, double> interp;
Filter::ButterworthLP<double> butter(50,0.02,2);
butter.stepInitialization(938.15);
Filter::ButterworthLP<double> 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);
}

38
tests/navMesh/TestNavMeshFactory.cpp Normal file
View File

@@ -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<NavMeshTriangle> nm;
NavMeshFactory<NavMeshTriangle> 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

35
tests/navMesh/TestNavMeshTriangle.cpp Normal file
View File

@@ -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

226
tests/sensors/imu/TestMotionDetection.cpp
View File

@@ -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<float> delta_motionAngles;
std::vector<float> delta_turnAngles;
//save for later plotting
std::vector<float> delta_motionAngles;
std::vector<float> 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<AccelerometerData>& _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<GyroscopeData>& _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);
}

38
tests/sensors/imu/TestTurnDetection.cpp
View File

@@ -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;
}

168
tests/sensors/pressure/TestBarometer.cpp
View File

@@ -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 <random>
@@ -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<ActivityButterPressure::History> actHist;
std::vector<ActivityButterPressure::History> rawHist;
std::vector<ActivityButterPressure::History> actHist;
std::vector<ActivityButterPressure::History> 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<ActivityButterPressurePercent::ActivityProbabilities> actHist;
std::vector<double> rawHist;
std::vector<ActivityButterPressurePercent::ActivityProbabilities> actHist;
std::vector<double> 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);
}