Here is the Triangle class we are using for out navmesh. This is a mesh representing the walkable areas in the game. It is quick to implement and is working well.
#include <string.h>
#include "vec3.h"
#define SMALL_NUM 0.00000001 // anything that avoids division overflow
class Triangle {
vec3 m_v0,m_v1,m_v2; //verteces
vec3 m_edge1,m_edge2,m_normal;
public:
Triangle(const vec3 &_v0, const vec3 &_v1, const vec3 &_v2);
~Triangle();
bool intersectRay(const vec3 &_rayStart, vec3 *_intersectPoint);
void print() const;
};
//the intersect method
bool Triangle::intersectRay(const vec3 &_rayStart, vec3 *_intersectPoint)
{
//vec3 _intersectPoint;
vec3 dir, w0, w; // ray vectors
float r, a, b; // params to calc ray-plane intersect
dir = vec3(0.0,-100.0,0.0) - _rayStart; // ray direction vector
dir.Normalize();
w0 = _rayStart - m_v0;
a = -m_normal.Dot(w0);
b = m_normal.Dot(dir);
if (fabs(b) < SMALL_NUM)
{
// ray is parallel to triangle plane or ray disjoint from plane
return false;
}
// get intersect point of ray with triangle plane
r = a / b;
if (r < 0.0) // ray goes away from triangle
return false; // => no intersect
// for a segment, also test if (r > 1.0) => no intersect
*_intersectPoint = _rayStart + (r * dir); // intersect point of ray and plane
// is I inside T?
float uu, uv, vv, wu, wv, D;
uu = m_edge1.Dot(m_edge1);
uv = m_edge1.Dot(m_edge2);
vv = m_edge2.Dot(m_edge2);
w = *_intersectPoint - m_v0;
wu = w.Dot(m_edge1);
wv = w.Dot(m_edge2);
D = uv * uv - uu * vv;
// get and test parametric coords
float s, t;
s = (uv * wv - vv * wu) / D;
if (s < 0.0 || s > 1.0) // I is outside T
return false;
t = (uv * wu - uu * wv) / D;
if (t < 0.0 || (s + t) > 1.0) // I is outside T
return false;
return true; // I is in T
}