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;
}
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;
}
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)
