335 lines
8.3 KiB
C++
335 lines
8.3 KiB
C++
#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
|