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

worked on nav-mesh

Browse Source 
added dijkstra support for nav mesh
some minor changes to distributions
minor fixes
This commit is contained in:
k-a-z-u
2018-01-17 16:36:37 +01:00
parent e81e8c112d
commit 3c72bc814c
15 changed files with 696 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

147
navMesh/NavMeshTriangle.h
View File

@@ -4,6 +4,11 @@
#include "../geo/Point3.h"
#include "../geo/Point2.h"
// fast barycentric code
// https://stackoverflow.com/questions/25385361/point-within-a-triangle-barycentric-co-ordinates#25386102
// https://i.stack.imgur.com/8VODS.png
// https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
namespace NM {
/**
@@ -29,12 +34,13 @@ namespace NM {
private: // precalculated stuff
Point2 v0;
Point2 v1;
float dot00;
float dot01;
float dot11;
double invDenom;
// Point2 v0;
// Point2 v1;
// float dot00;
// float dot01;
// float dot11;
// double invDenom;
float area;
float minZ;
@@ -44,13 +50,16 @@ namespace NM {
const Point3 v12;
const Point3 v13;
const double _det;
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) {
center((p1+p2+p3)/3), v12(p2-p1), v13(p3-p1),
_det(1.0*(p2.y - p3.y)*(p1.x - p3.x) + (p3.x - p2.x)*(p1.y - p3.y)) {
precompute();
@@ -66,6 +75,13 @@ namespace NM {
Point3 getP3() const {return p3;}
/** get the number of known neighbors for this triangle */
int getNumNeighbors() const {return _numNeighbors;}
/** get the idx-th neighbor */
const NavMeshTriangle* getNeighbor(const int idx) const {return _neighbors[idx];}
/** get the distance between the given point and the triangle using approximate tests */
float getDistanceApx(const Point3 pt) const {
@@ -123,6 +139,61 @@ namespace NM {
return p1 + (v12*u) + (v13*v);
}
/** 2D UV */
void getUV(const Point2 p, float& u, float& v) const {
// https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
const Point2 v0 = p2.xy() - p1.xy();
const Point2 v1 = p3.xy() - p1.xy();
const Point2 v2 = p - p1.xy();
const float den = v0.x * v1.y - v1.x * v0.y;
u = (v2.x * v1.y - v1.x * v2.y) / den;
v = (v0.x * v2.y - v2.x * v0.y) / den;
}
/** 2D UVW */
void getUVW(const Point2 p, float& u, float& v, float& w) const {
getUV(p,u,v);
w = 1-u-v;
}
/** 3D UV */
void getUV(const Point3 p, float& u, float& v) const {
const Point3 v0 = p2 - p1;
const Point3 v1 = p3 - p1;
const Point3 v2 = p - p1;
const float d00 = dot(v0, v0);
const float d01 = dot(v0, v1);
const float d11 = dot(v1, v1);
const float d20 = dot(v2, v0);
const float d21 = dot(v2, v1);
const float denom = d00 * d11 - d01 * d01;
u = (d11 * d20 - d01 * d21) / denom;
v = (d00 * d21 - d01 * d20) / denom;
//w = 1.0f - v - w;
int xx = 0; (void) xx;
}
/** 3D UVW */
void getUVW(const Point3 p, float& u, float& v, float& w) const {
getUV(p,u,v);
w = 1-u-v;
}
template <typename T> T interpolate(const Point3 p, const T val1, const T val2, const T val3) const {
float u, v, w;
getUVW(p.xy(),u,v,w);
return (w*val1) + (u*val2) + (v*val3);
}
/** does the triangle contain the given 3D point? */
bool contains(const Point3 p) const {
return (minZ <= p.z) && (maxZ >= p.z) && contains(p.xy());
@@ -131,15 +202,18 @@ namespace NM {
/** does the triangle contain the given 2D point? */
bool contains(const Point2 p) const {
const Point2 v2 = p - p1.xy();
// const Point2 v2 = p - p1.xy();
// Compute dot products
float dot02 = dot(v0, v2);
float dot12 = dot(v1, v2);
// // 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;
// // Compute barycentric coordinates
// float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
// float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
float u, v;
getUV(p, u, v);
// Check if point is in triangle
return (u >= 0) && (v >= 0) && (u + v <= 1);
@@ -149,17 +223,20 @@ namespace NM {
/** estimate the correct z-value for the given 2D point */
Point3 toPoint3(const Point2 p) const {
const Point2 v2 = p - p1.xy();
// const Point2 v2 = p - p1.xy();
// Compute dot products
float dot02 = dot(v0, v2);
float dot12 = dot(v1, v2);
// // 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;
// // Compute barycentric coordinates
// float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
// float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
const Point3 res = getPoint(v,u);
float u, v;
getUV(p, u, v);
const Point3 res = getPoint(u,v);
Assert::isNear(res.x, p.x, 1.0f, "TODO: high difference while mapping from 2D to 3D");
Assert::isNear(res.y, p.y, 1.0f, "TODO: high difference while mapping from 2D to 3D");
@@ -180,6 +257,14 @@ namespace NM {
return center;
}
/** cast to string */
operator std::string() const {return asString();}
/** get as string */
std::string asString() const {
return "(" + std::to_string(center.x) + "," + std::to_string(center.y) + "," + std::to_string(center.z) + ")";
}
private:
@@ -191,17 +276,17 @@ namespace NM {
minZ = std::min(p1.z, std::min(p2.z, p3.z)) - 0.15; // TODO the builder does not align on the same height as we did
maxZ = std::max(p1.z, std::max(p2.z, p3.z)) + 0.15;
// Compute vectors
v0 = p3.xy() - p1.xy();
v1 = p2.xy() - p1.xy();
// // Compute 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 dot products
// dot00 = dot(v0, v0);
// dot01 = dot(v0, v1);
// dot11 = dot(v1, v1);
// Compute barycentric coordinates
invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01);
// // Compute barycentric coordinates
// invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)dot01);