static float distPtTri(const float* p, const float* a, const float* b, const float* c)
{
float v0[3], v1[3], v2[3];
rcVsub(v0, c,a);
rcVsub(v1, b,a);
rcVsub(v2, p,a);
const float dot00 = vdot2(v0, v0);
const float dot01 = vdot2(v0, v1);
const float dot02 = vdot2(v0, v2);
const float dot11 = vdot2(v1, v1);
const float dot12 = vdot2(v1, v2);
// Compute barycentric coordinates
const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
const float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// If point lies inside the triangle, return interpolated y-coord.
static const float EPS = 1e-4f;
if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS)
{
const float y = a[1] + v0[1]*u + v1[1]*v;
return fabsf(y-p[1]);
}
return FLT_MAX;
}
static float distPtTri(const dtCoordinates& p, const dtCoordinates& a, const dtCoordinates& b, const dtCoordinates& c)
{
dtCoordinates v0, v1, v2;
rcVsub(v0, c,a);
rcVsub(v1, b,a);
rcVsub(v2, p,a);
const float dot00 = vdot2(v0, v0);
const float dot01 = vdot2(v0, v1);
const float dot02 = vdot2(v0, v2);
const float dot11 = vdot2(v1, v1);
const float dot12 = vdot2(v1, v2);
// Compute barycentric coordinates
const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
const float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// If point lies inside the triangle, return interpolated y-coord.
static const float EPS = 1e-4f;
if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS)
{
const float y = a.Y() + v0.Y()*u + v1.Y()*v;
return fabsf(y-p.Y());
}
return FLT_MAX;
}
static bool circumCircle(const float* p1, const float* p2, const float* p3,
float* c, float& r)
{
static const float EPS = 1e-6f;
// Calculate the circle relative to p1, to avoid some precision issues.
const float v1[3] = {0,0,0};
float v2[3], v3[3];
rcVsub(v2, p2,p1);
rcVsub(v3, p3,p1);
const float cp = vcross2(v1, v2, v3);
if (fabsf(cp) > EPS)
{
const float v1Sq = vdot2(v1,v1);
const float v2Sq = vdot2(v2,v2);
const float v3Sq = vdot2(v3,v3);
c[0] = (v1Sq*(v2[2]-v3[2]) + v2Sq*(v3[2]-v1[2]) + v3Sq*(v1[2]-v2[2])) / (2*cp);
c[1] = 0;
c[2] = (v1Sq*(v3[0]-v2[0]) + v2Sq*(v1[0]-v3[0]) + v3Sq*(v2[0]-v1[0])) / (2*cp);
r = vdist2(c, v1);
rcVadd(c, c, p1);
return true;
}
rcVcopy(c, p1);
r = 0;
return false;
}
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);
}
static void calcTriNormal(const dtCoordinates& v0, const dtCoordinates& v1, const dtCoordinates& v2, dtCoordinates& norm)
{
dtCoordinates e0, e1;
rcVsub(e0, v1, v0);
rcVsub(e1, v2, v0);
rcVcross(norm, e0, e1);
rcVnormalize(norm);
}
bool InputGeom::raycastMesh(float* src, float* dst, float& tmin)
{
float dir[3];
rcVsub(dir, dst, src);
int nt = m_mesh->getTriCount();
const float* verts = m_mesh->getVerts();
const float* normals = m_mesh->getNormals();
const int* tris = m_mesh->getTris();
tmin = 1.0f;
bool hit = false;
for (int i = 0; i < nt*3; i += 3)
{
const float* n = &normals[i];
if (rcVdot(dir, n) > 0)
continue;
float t = 1;
if (intersectSegmentTriangle(src, dst,
&verts[tris[i]*3],
&verts[tris[i+1]*3],
&verts[tris[i+2]*3], t))
{
if (t < tmin)
tmin = t;
hit = true;
}
}
return hit;
}
void TestCase::handleRender()
{
glLineWidth(2.0f);
glBegin(GL_LINES);
for (Test* iter = m_tests; iter; iter = iter->next)
{
float dir[3];
rcVsub(dir, iter->epos, iter->spos);
rcVnormalize(dir);
glColor4ub(128,25,0,192);
glVertex3f(iter->spos[0],iter->spos[1]-0.3f,iter->spos[2]);
glVertex3f(iter->spos[0],iter->spos[1]+0.3f,iter->spos[2]);
glVertex3f(iter->spos[0],iter->spos[1]+0.3f,iter->spos[2]);
glVertex3f(iter->spos[0]+dir[0]*0.3f,iter->spos[1]+0.3f+dir[1]*0.3f,iter->spos[2]+dir[2]*0.3f);
glColor4ub(51,102,0,129);
glVertex3f(iter->epos[0],iter->epos[1]-0.3f,iter->epos[2]);
glVertex3f(iter->epos[0],iter->epos[1]+0.3f,iter->epos[2]);
if (iter->expand)
glColor4ub(255,192,0,255);
else
glColor4ub(0,0,0,64);
for (int i = 0; i < iter->nstraight-1; ++i)
{
glVertex3f(iter->straight[i*3+0],iter->straight[i*3+1]+0.3f,iter->straight[i*3+2]);
glVertex3f(iter->straight[(i+1)*3+0],iter->straight[(i+1)*3+1]+0.3f,iter->straight[(i+1)*3+2]);
}
}
glEnd();
glLineWidth(1.0f);
}
bool rcIsOverlapBounds2D( const dtCoordinates& vertexPoint, const dtCoordinates& bmin, const dtCoordinates& bmax )
{
const dtCoordinates extend( 0.4f, 0.0f, 0.4f );
dtCoordinates amin, amax;
rcVsub( amin, vertexPoint, extend );
rcVadd( amax, vertexPoint, extend );
bool overlap = true;
overlap = (amin.X() > bmax.X() || amax.X() < bmin.X()) ? false : overlap;
overlap = (amin.Z() > bmax.Z() || amax.Z() < bmin.Z()) ? false : overlap;
return overlap;
}
static bool intersectSegmentTriangle(const float* sp, const float* sq,
const float* a, const float* b, const float* c,
float &t)
{
float v, w;
float ab[3], ac[3], qp[3], ap[3], norm[3], e[3];
rcVsub(ab, b, a);
rcVsub(ac, c, a);
rcVsub(qp, sp, sq);
// Compute triangle normal. Can be precalculated or cached if
// intersecting multiple segments against the same triangle
rcVcross(norm, ab, ac);
// Compute denominator d. If d <= 0, segment is parallel to or points
// away from triangle, so exit early
float d = rcVdot(qp, norm);
if (d <= 0.0f) return false;
// Compute intersection t value of pq with plane of triangle. A ray
// intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
// dividing by d until intersection has been found to pierce triangle
rcVsub(ap, sp, a);
t = rcVdot(ap, norm);
if (t < 0.0f) return false;
if (t > d) return false; // For segment; exclude this code line for a ray test
// Compute barycentric coordinate components and test if within bounds
rcVcross(e, qp, ap);
v = rcVdot(ac, e);
if (v < 0.0f || v > d) return false;
w = -rcVdot(ab, e);
if (w < 0.0f || v + w > d) return false;
// Segment/ray intersects triangle. Perform delayed division
t /= d;
return true;
}
static bool barDistSqPointToTri(const float* p, const float* a, const float* b, const float* c)
{
float v0[3], v1[3], v2[3];
rcVsub(v0, c,a);
rcVsub(v1, b,a);
rcVsub(v2, p,a);
const float dot00 = vdot2(v0, v0);
const float dot01 = vdot2(v0, v1);
const float dot02 = vdot2(v0, v2);
const float dot11 = vdot2(v1, v1);
const float dot12 = vdot2(v1, v2);
// Compute barycentric coordinates
float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
float ud = u<0.f ? -u : (u>1.f ? u-1.f : 0.f);
float vd = v<0.f ? -v : (v>1.f ? v-1.f : 0.f);
return ud * ud + vd * vd;
}
bool InputGeom::raycastMesh(float* src, float* dst, float& tmin)
{
float dir[3];
rcVsub(dir, dst, src);
// Prune hit ray.
float btmin, btmax;
if (!isectSegAABB(src, dst, &m_meshBMin[0], &m_meshBMax[0], btmin, btmax))
return false;
float p[2], q[2];
p[0] = src[0] + (dst[0]-src[0])*btmin;
p[1] = src[2] + (dst[2]-src[2])*btmin;
q[0] = src[0] + (dst[0]-src[0])*btmax;
q[1] = src[2] + (dst[2]-src[2])*btmax;
int cid[512];
const int ncid = rcGetChunksOverlappingSegment(m_chunkyMesh.get(), p, q, cid, 512);
if (!ncid)
return false;
tmin = 1.0f;
bool hit = false;
const float* verts = m_loader->getVerts();
for (int i = 0; i < ncid; ++i)
{
const rcChunkyTriMeshNode& node = m_chunkyMesh->nodes[cid[i]];
const int* tris = &m_chunkyMesh->tris[node.i*3];
const int ntris = node.n;
for (int j = 0; j < ntris*3; j += 3)
{
float t = 1;
if (intersectSegmentTriangle(
src, dst,
&verts[tris[j]*3],
&verts[tris[j+1]*3],
&verts[tris[j+2]*3], t))
{
if (t < tmin)
tmin = t;
hit = true;
}
}
}
return hit;
}
static bool isectSegAABB(const float* sp, const float* sq,
const float* amin, const float* amax,
float& tmin, float& tmax)
{
static const float EPS = 1e-6f;
float d[3];
rcVsub(d, sq, sp);
tmin = 0; // set to -FLT_MAX to get first hit on line
tmax = FLT_MAX; // set to max distance ray can travel (for segment)
// For all three slabs
for (int i = 0; i < 3; i++)
{
if (fabsf(d[i]) < EPS)
{
// Ray is parallel to slab. No hit if origin not within slab
if (sp[i] < amin[i] || sp[i] > amax[i])
return false;
}
else
{
// Compute intersection t value of ray with near and far plane of slab
const float ood = 1.0f / d[i];
float t1 = (amin[i] - sp[i]) * ood;
float t2 = (amax[i] - sp[i]) * ood;
// Make t1 be intersection with near plane, t2 with far plane
if (t1 > t2) rcSwap(t1, t2);
// Compute the intersection of slab intersections intervals
if (t1 > tmin) tmin = t1;
if (t2 < tmax) tmax = t2;
// Exit with no collision as soon as slab intersection becomes empty
if (tmin > tmax) return false;
}
}
return true;
}
void GameWorld::recalc()
{
if (!m_navMesh)
return;
if (m_sposSet)
m_startRef = m_navMesh->findNearestPoly(m_spos, m_polyPickExt, &m_filter, 0);
else
m_startRef = 0;
if (m_eposSet)
m_endRef = m_navMesh->findNearestPoly(m_epos, m_polyPickExt, &m_filter, 0);
else
m_endRef = 0;
#if 0
//if (m_toolMode == TOOLMODE_PATHFIND_ITER)
{
m_pathIterNum = 0;
if (m_sposSet && m_eposSet && m_startRef && m_endRef)
{
#ifdef DUMP_REQS
printf("pi %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.includeFlags, m_filter.excludeFlags);
rcGetLog()->log(RC_LOG_PROGRESS, "pi %f %f %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2],
m_filter.includeFlags, m_filter.excludeFlags );
#endif
m_npolys = m_navMesh->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, MAX_POLYS);
m_nsmoothPath = 0;
if (m_npolys)
{
// Iterate over the path to find smooth path on the detail mesh surface.
const dtPolyRef* polys = m_polys;
int npolys = m_npolys;
float iterPos[3], targetPos[3];
m_navMesh->closestPointOnPolyBoundary(m_startRef, m_spos, iterPos);
m_navMesh->closestPointOnPolyBoundary(polys[npolys-1], m_epos, targetPos);
static const float STEP_SIZE = 0.5f;
static const float SLOP = 0.01f;
m_nsmoothPath = 0;
rcVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_nsmoothPath++;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
while (npolys && m_nsmoothPath < MAX_SMOOTH)
{
// Find location to steer towards.
float steerPos[3];
unsigned char steerPosFlag;
dtPolyRef steerPosRef;
if (!getSteerTarget(m_navMesh, iterPos, targetPos, SLOP,
polys, npolys, steerPos, steerPosFlag, steerPosRef))
break;
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
// Find movement delta.
float delta[3], len;
rcVsub(delta, steerPos, iterPos);
len = sqrtf(rcVdot(delta,delta));
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
len = 1;
else
len = STEP_SIZE / len;
float moveTgt[3];
rcVmad(moveTgt, iterPos, delta, len);
// Move
float result[3];
int n = m_navMesh->moveAlongPathCorridor(iterPos, moveTgt, result, polys, npolys);
float h = 0;
m_navMesh->getPolyHeight(polys[n], result, &h);
result[1] = h;
// Shrink path corridor if advanced.
if (n)
{
polys += n;
npolys -= n;
}
// Update position.
rcVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f))
{
// Reached end of path.
rcVcopy(iterPos, targetPos);
if (m_nsmoothPath < MAX_SMOOTH)
{
rcVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_nsmoothPath++;
}
break;
}
else if (offMeshConnection && inRange(iterPos, steerPos, SLOP, 1.0f))
{
// Reached off-mesh connection.
float startPos[3], endPos[3];
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = 0, polyRef = polys[0];
while (npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[0];
polys++;
npolys--;
}
// Handle the connection.
if (m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos))
{
if (m_nsmoothPath < MAX_SMOOTH)
{
rcVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if (m_nsmoothPath & 1)
{
rcVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
rcVcopy(iterPos, endPos);
float h;
m_navMesh->getPolyHeight(polys[0], iterPos, &h);
iterPos[1] = h;
}
}
// Store results.
if (m_nsmoothPath < MAX_SMOOTH)
{
rcVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_nsmoothPath++;
}
}
}
}
else
{
m_npolys = 0;
m_nsmoothPath = 0;
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT)
bool TestCase::handleRenderOverlay(double* proj, double* model, int* view)
{
GLdouble x, y, z;
char text[64];
int n = 0;
static const float LABEL_DIST = 1.0f;
for (Test* iter = m_tests; iter; iter = iter->next)
{
float pt[3], dir[3];
if (iter->nstraight)
{
rcVcopy(pt, &iter->straight[3]);
if (rcVdist(pt, iter->spos) > LABEL_DIST)
{
rcVsub(dir, pt, iter->spos);
rcVnormalize(dir);
rcVmad(pt, iter->spos, dir, LABEL_DIST);
}
pt[1]+=0.5f;
}
else
{
rcVsub(dir, iter->epos, iter->spos);
rcVnormalize(dir);
rcVmad(pt, iter->spos, dir, LABEL_DIST);
pt[1]+=0.5f;
}
if (gluProject((GLdouble)pt[0], (GLdouble)pt[1], (GLdouble)pt[2],
model, proj, view, &x, &y, &z))
{
snprintf(text, 64, "Path %d\n", n);
unsigned int col = imguiRGBA(0,0,0,128);
if (iter->expand)
col = imguiRGBA(255,192,0,220);
imguiDrawText((int)x, (int)(y-25), IMGUI_ALIGN_CENTER, text, col);
}
n++;
}
static int resScroll = 0;
bool mouseOverMenu = imguiBeginScrollArea("Test Results", 10, view[3] - 10 - 350, 200, 350, &resScroll);
// mouseOverMenu = true;
n = 0;
for (Test* iter = m_tests; iter; iter = iter->next)
{
snprintf(text, 64, "Path %d\n", n);
if (imguiCollapse(text, iter->expand))
iter->expand = !iter->expand;
if (iter->expand)
{
snprintf(text, 64, "Poly: %.4f ms\n", (float)iter->findNearestPolyTime/1000.0f);
imguiValue(text);
snprintf(text, 64, "Path: %.4f ms\n", (float)iter->findPathTime/1000.0f);
imguiValue(text);
snprintf(text, 64, "Straight: %.4f ms\n", (float)iter->findStraightPathTime/1000.0f);
imguiValue(text);
}
rcTimeVal total = iter->findNearestPolyTime + iter->findPathTime + iter->findStraightPathTime;
snprintf(text, 64, "Total: %.4f ms\n", (float)total/1000.0f);
imguiValue(text);
// imguiDrawText(10, 700-n*20, IMGUI_ALIGN_LEFT, text, imguiRGBA(255,255,255,220));
n++;
}
imguiEndScrollArea();
return mouseOverMenu;
}
