345 lines
8.6 KiB
C++
345 lines
8.6 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef NAVMESHTRIANGLE_H
|
||
#define NAVMESHTRIANGLE_H
|
||
|
||
#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 {
|
||
|
||
/**
|
||
* represents one triangle within the NavMesh
|
||
* each Triangle has up to 3 neighbors (one per edge)
|
||
*
|
||
* for performance enhancements,
|
||
* some memeber attributes are pre-calculated once
|
||
*/
|
||
class NavMeshTriangle {
|
||
|
||
private:
|
||
|
||
template<typename> friend class NavMesh;
|
||
|
||
const Point3 p1;
|
||
const Point3 p2;
|
||
const Point3 p3;
|
||
const uint8_t type;
|
||
|
||
NavMeshTriangle* _neighbors[3];
|
||
int _numNeighbors;
|
||
|
||
protected: // precalculated stuff
|
||
|
||
// Point2 v0;
|
||
// Point2 v1;
|
||
// float dot00;
|
||
// float dot01;
|
||
// float dot11;
|
||
// double invDenom;
|
||
|
||
float area;
|
||
|
||
float minZ;
|
||
float maxZ;
|
||
|
||
const Point3 center;
|
||
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),
|
||
_det(1.0*(p2.y - p3.y)*(p1.x - p3.x) + (p3.x - p2.x)*(p1.y - p3.y)) {
|
||
|
||
precompute();
|
||
|
||
}
|
||
|
||
/** get the triangle's type */
|
||
uint8_t getType() const {return type;}
|
||
|
||
Point3 getP1() const {return p1;}
|
||
|
||
Point3 getP2() const {return p2;}
|
||
|
||
Point3 getP3() const {return p3;}
|
||
|
||
|
||
/** 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 {
|
||
|
||
// const float d1 = pt.getDistance(p1);
|
||
// const float d2 = pt.getDistance(p2);
|
||
// const float d3 = pt.getDistance(p3);
|
||
// const float d4 = pt.getDistance(center);
|
||
// const float d5 = pt.getDistance((p1-p2)/2);
|
||
// const float d6 = pt.getDistance((p2-p3)/2);
|
||
// const float d7 = pt.getDistance((p3-p1)/2);
|
||
// return std::min(d1, std::min(d2, std::min(d3, std::min(d4, std::min(d5, std::min(d6,d7))))));
|
||
|
||
// const float d1 = pt.getDistance(p1);
|
||
// const float d2 = pt.getDistance(p2);
|
||
// const float d3 = pt.getDistance(p3);
|
||
// const float d4 = pt.getDistance(center);
|
||
// return std::min(d1, std::min(d2, std::min(d3,d4)));
|
||
|
||
float bestD = 99999;
|
||
Point3 bestP;
|
||
Point3 dir12 = p2-p1;
|
||
Point3 dir13 = p3-p1;
|
||
Point3 dir23 = p3-p2;
|
||
for (float f = 0; f < 1; f += 0.05f) {
|
||
const Point3 pos1 = p1 + dir12 * f; const float dist1 = pos1.getDistance(pt);
|
||
const Point3 pos2 = p1 + dir13 * f; const float dist2 = pos2.getDistance(pt);
|
||
const Point3 pos3 = p2 + dir23 * f; const float dist3 = pos3.getDistance(pt);
|
||
if (dist1 < bestD) {bestP = pos1; bestD = dist1;}
|
||
if (dist2 < bestD) {bestP = pos2; bestD = dist2;}
|
||
if (dist3 < bestD) {bestP = pos3; bestD = dist3;}
|
||
}
|
||
|
||
return bestD;
|
||
|
||
|
||
}
|
||
|
||
|
||
bool operator == (const NavMeshTriangle& o) const {
|
||
return (p1 == o.p1) && (p2 == o.p2) && (p3 == o.p3);
|
||
}
|
||
|
||
/** is the triangle plain? (same Z for all points) */
|
||
bool isPlain() const {
|
||
const float d1 = std::abs(p1.z - p2.z);
|
||
const float d2 = std::abs(p2.z - p3.z);
|
||
return (d1 < 0.1) && (d2 < 0.1);
|
||
}
|
||
|
||
const NavMeshTriangle* const* begin() const {return &_neighbors[0];}
|
||
|
||
const NavMeshTriangle* const* end() const {return &_neighbors[_numNeighbors];}
|
||
|
||
Point3 getPoint(const float u, const float v) const {
|
||
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;
|
||
}
|
||
|
||
/** barycentric interpolation at Point p for val1@p1, val2@p2, val3@p3 */
|
||
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());
|
||
}
|
||
|
||
/** does the triangle contain the given 2D point? */
|
||
bool contains(const Point2 p) const {
|
||
|
||
// const Point2 v2 = p - p1.xy();
|
||
|
||
// // Compute dot products
|
||
// float dot02 = dot(v0, v2);
|
||
// float dot12 = dot(v1, v2);
|
||
|
||
// // Compute barycentric coordinates
|
||
// float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
|
||
// float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
|
||
|
||
float u, v;
|
||
getUV(p, u, v);
|
||
|
||
// Check if point is in triangle
|
||
return (u >= 0) && (v >= 0) && (u + v <= 1);
|
||
|
||
}
|
||
|
||
/** estimate the correct z-value for the given 2D point */
|
||
Point3 toPoint3(const Point2 p) const {
|
||
|
||
// const Point2 v2 = p - p1.xy();
|
||
|
||
// // Compute dot products
|
||
// float dot02 = dot(v0, v2);
|
||
// float dot12 = dot(v1, v2);
|
||
|
||
// // Compute barycentric coordinates
|
||
// float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
|
||
// float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
|
||
|
||
float u, v;
|
||
getUV(p, u, v);
|
||
|
||
const Point3 res = getPoint(u,v);
|
||
Assert::isNear<float>(res.x, p.x, 1.0f, "TODO: high difference while mapping from 2D to 3D");
|
||
Assert::isNear<float>(res.y, p.y, 1.0f, "TODO: high difference while mapping from 2D to 3D");
|
||
|
||
//return res;
|
||
return Point3(p.x, p.y, res.z); // only use the new z, keep input as-is
|
||
|
||
}
|
||
|
||
/** nearest point on the triangle */
|
||
Point3 toPoint3Near(const Point2 p) const {
|
||
|
||
float u, v;
|
||
getUV(p, u, v);
|
||
|
||
if (u < 0) {u = 0;}
|
||
if (u > 1) {u = 1;}
|
||
if (v < 0) {v = 0;}
|
||
if (v > 1) {v = 1;}
|
||
|
||
return getPoint(u,v);
|
||
|
||
}
|
||
|
||
|
||
|
||
/** get the triangle's size */
|
||
float getArea() const {
|
||
return area;
|
||
}
|
||
|
||
/** get the triangle's center-point */
|
||
Point3 getCenter() const {
|
||
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:
|
||
|
||
/** perform some pre-calculations to speed things up */
|
||
void precompute() {
|
||
|
||
#pragma message "TODO, z buffer"
|
||
minZ = std::min(p1.z, std::min(p2.z, p3.z)) - 0.15; // TODO the builder does not align on the same height as we did
|
||
maxZ = std::max(p1.z, std::max(p2.z, p3.z)) + 0.15;
|
||
|
||
// // Compute vectors
|
||
// v0 = p3.xy() - p1.xy();
|
||
// v1 = p2.xy() - p1.xy();
|
||
|
||
// // Compute dot products
|
||
// dot00 = dot(v0, v0);
|
||
// dot01 = dot(v0, v1);
|
||
// dot11 = dot(v1, v1);
|
||
|
||
// // Compute barycentric coordinates
|
||
// invDenom = 1.0 / ((double)dot00 * (double)dot11 - (double)dot01 * (double)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) );
|
||
|
||
}
|
||
|
||
protected:
|
||
|
||
void addNeighbor(NavMeshTriangle* o) {
|
||
Assert::isBetween(_numNeighbors, 0, 3, "number of neighbors out of bounds");
|
||
_neighbors[_numNeighbors] = o;
|
||
++_numNeighbors;
|
||
}
|
||
|
||
|
||
};
|
||
|
||
}
|
||
|
||
#endif // NAVMESHTRIANGLE_H
|