float dtDistancePtSegSqr(const float* pt, const float* p, const float* q)
{
float seg[3], toPt[3], closest[3];
dtVsub(seg, q, p);
dtVsub(toPt, pt, p);
const float d1 = dtVdot(toPt, seg);
const float d2 = dtVdot(seg, seg);
if (d1 <= 0)
{
dtVcopy(closest, p);
}
else if (d2 <= d1)
{
dtVcopy(closest, q);
}
else
{
dtVmad(closest, p, seg, d1 / d2);
}
dtVsub(toPt, closest, pt);
return dtVlenSqr(toPt);
}
dtStatus PathInfo::findSmoothPath(const float* startPos, const float* endPos,
const dtPolyRef* polyPath, const uint32 polyPathSize,
float* smoothPath, int* smoothPathSize, bool &usedOffmesh, const uint32 maxSmoothPathSize)
{
MANGOS_ASSERT(polyPathSize <= MAX_PATH_LENGTH);
*smoothPathSize = 0;
uint32 nsmoothPath = 0;
usedOffmesh = false;
dtPolyRef polys[MAX_PATH_LENGTH];
memcpy(polys, polyPath, sizeof(dtPolyRef)*polyPathSize);
uint32 npolys = polyPathSize;
float iterPos[VERTEX_SIZE], targetPos[VERTEX_SIZE];
if(DT_SUCCESS != m_navMeshQuery->closestPointOnPolyBoundary(polys[0], startPos, iterPos))
return DT_FAILURE;
if(DT_SUCCESS != m_navMeshQuery->closestPointOnPolyBoundary(polys[npolys-1], endPos, targetPos))
return DT_FAILURE;
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
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 && nsmoothPath < maxSmoothPathSize)
{
// Find location to steer towards.
float steerPos[VERTEX_SIZE];
unsigned char steerPosFlag;
dtPolyRef steerPosRef = INVALID_POLYREF;
if (!getSteerTarget(iterPos, targetPos, SMOOTH_PATH_SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef))
break;
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END);
bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION);
// Find movement delta.
float delta[VERTEX_SIZE];
dtVsub(delta, steerPos, iterPos);
float len = dtSqrt(dtVdot(delta,delta));
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < SMOOTH_PATH_STEP_SIZE)
len = 1.0f;
else
len = SMOOTH_PATH_STEP_SIZE / len;
float moveTgt[VERTEX_SIZE];
dtVmad(moveTgt, iterPos, delta, len);
// Move
float result[VERTEX_SIZE];
const static uint32 MAX_VISIT_POLY = 16;
dtPolyRef visited[MAX_VISIT_POLY];
uint32 nvisited = 0;
m_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter, result, visited, (int*)&nvisited, MAX_VISIT_POLY);
npolys = fixupCorridor(polys, npolys, MAX_PATH_LENGTH, visited, nvisited);
m_navMeshQuery->getPolyHeight(polys[0], result, &result[1]);
dtVcopy(iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 2.0f))
{
// Reached end of path.
dtVcopy(iterPos, targetPos);
if (nsmoothPath < maxSmoothPathSize)
{
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
nsmoothPath++;
}
break;
}
else if (offMeshConnection && inRangeYZX(iterPos, steerPos, SMOOTH_PATH_SLOP, 2.0f))
{
// Reached off-mesh connection.
usedOffmesh = true;
// Advance the path up to and over the off-mesh connection.
dtPolyRef prevRef = INVALID_POLYREF;
dtPolyRef polyRef = polys[0];
uint32 npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (uint32 i = npos; i < npolys; ++i)
polys[i-npos] = polys[i];
npolys -= npos;
// Handle the connection.
float startPos[VERTEX_SIZE], endPos[VERTEX_SIZE];
if (DT_SUCCESS == m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos))
{
if (nsmoothPath < maxSmoothPathSize)
{
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], startPos);
nsmoothPath++;
}
// Move position at the other side of the off-mesh link.
dtVcopy(iterPos, endPos);
m_navMeshQuery->getPolyHeight(polys[0], iterPos, &iterPos[1]);
}
}
// Store results.
if (nsmoothPath < maxSmoothPathSize)
{
dtVcopy(&smoothPath[nsmoothPath*VERTEX_SIZE], iterPos);
nsmoothPath++;
}
}
*smoothPathSize = nsmoothPath;
// this is most likely loop
return nsmoothPath < maxSmoothPathSize ? DT_SUCCESS : DT_FAILURE;
}
void NavMeshTesterTool::recalc()
{
if (!m_navMesh)
return;
if (m_sposSet)
m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
else
m_startRef = 0;
if (m_eposSet)
m_navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, 0);
else
m_endRef = 0;
m_pathFindStatus = DT_FAILURE;
if (m_toolMode == TOOLMODE_PATHFIND_FOLLOW)
{
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.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
m_nsmoothPath = 0;
if (m_npolys)
{
// Iterate over the path to find smooth path on the detail mesh surface.
dtPolyRef polys[MAX_POLYS];
memcpy(polys, m_polys, sizeof(dtPolyRef)*m_npolys);
int npolys = m_npolys;
float iterPos[3], targetPos[3];
m_navQuery->closestPointOnPolyBoundary(m_startRef, m_spos, iterPos);
m_navQuery->closestPointOnPolyBoundary(polys[npolys-1], m_epos, targetPos);
static const float STEP_SIZE = 0.5f;
static const float SLOP = 0.01f;
m_nsmoothPath = 0;
dtVcopy(&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_navQuery, 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;
dtVsub(delta, steerPos, iterPos);
len = dtSqrt(dtVdot(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];
dtVmad(moveTgt, iterPos, delta, len);
// Move
float result[3];
dtPolyRef visited[16];
int nvisited = 0;
m_navQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &m_filter,
result, visited, &nvisited, 16);
npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited);
float h = 0;
m_navQuery->getPolyHeight(polys[0], result, &h);
result[1] = h;
dtVcopy(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.
dtVcopy(iterPos, targetPos);
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&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];
int npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (int i = npos; i < npolys; ++i)
polys[i-npos] = polys[i];
npolys -= npos;
// Handle the connection.
if (m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos) == DT_SUCCESS)
{
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if (m_nsmoothPath & 1)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
dtVcopy(iterPos, endPos);
float h;
m_navQuery->getPolyHeight(polys[0], iterPos, &h);
iterPos[1] = h;
}
}
// Store results.
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos);
m_nsmoothPath++;
}
}
}
}
else
{
m_npolys = 0;
m_nsmoothPath = 0;
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT)
{
if (m_sposSet && m_eposSet && m_startRef && m_endRef)
{
#ifdef DUMP_REQS
printf("ps %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.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
m_nstraightPath = 0;
if (m_npolys)
{
// In case of partial path, make sure the end point is clamped to the last polygon.
float epos[3];
dtVcopy(epos, m_epos);
if (m_polys[m_npolys-1] != m_endRef)
m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos);
m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
m_straightPath, m_straightPathFlags,
m_straightPathPolys, &m_nstraightPath, MAX_POLYS);
}
}
else
{
m_npolys = 0;
m_nstraightPath = 0;
}
}
else if (m_toolMode == TOOLMODE_PATHFIND_SLICED)
{
if (m_sposSet && m_eposSet && m_startRef && m_endRef)
{
#ifdef DUMP_REQS
printf("ps %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.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_npolys = 0;
m_nstraightPath = 0;
m_pathFindStatus = m_navQuery->initSlicedFindPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter);
}
else
{
m_npolys = 0;
m_nstraightPath = 0;
}
}
else if (m_toolMode == TOOLMODE_RAYCAST)
{
m_nstraightPath = 0;
if (m_sposSet && m_eposSet && m_startRef)
{
#ifdef DUMP_REQS
printf("rc %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.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
float t = 0;
m_npolys = 0;
m_nstraightPath = 2;
m_straightPath[0] = m_spos[0];
m_straightPath[1] = m_spos[1];
m_straightPath[2] = m_spos[2];
m_navQuery->raycast(m_startRef, m_spos, m_epos, &m_filter, &t, m_hitNormal, m_polys, &m_npolys, MAX_POLYS);
if (t > 1)
{
// No hit
dtVcopy(m_hitPos, m_epos);
m_hitResult = false;
}
else
{
// Hit
m_hitPos[0] = m_spos[0] + (m_epos[0] - m_spos[0]) * t;
m_hitPos[1] = m_spos[1] + (m_epos[1] - m_spos[1]) * t;
m_hitPos[2] = m_spos[2] + (m_epos[2] - m_spos[2]) * t;
if (m_npolys)
{
float h = 0;
m_navQuery->getPolyHeight(m_polys[m_npolys-1], m_hitPos, &h);
m_hitPos[1] = h;
}
m_hitResult = true;
}
dtVcopy(&m_straightPath[3], m_hitPos);
}
}
else if (m_toolMode == TOOLMODE_DISTANCE_TO_WALL)
{
m_distanceToWall = 0;
if (m_sposSet && m_startRef)
{
#ifdef DUMP_REQS
printf("dw %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], 100.0f,
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_distanceToWall = 0.0f;
m_navQuery->findDistanceToWall(m_startRef, m_spos, 100.0f, &m_filter, &m_distanceToWall, m_hitPos, m_hitNormal);
}
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_CIRCLE)
{
if (m_sposSet && m_startRef && m_eposSet)
{
const float dx = m_epos[0] - m_spos[0];
const float dz = m_epos[2] - m_spos[2];
float dist = sqrtf(dx*dx + dz*dz);
#ifdef DUMP_REQS
printf("fpc %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], dist,
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPolysAroundCircle(m_startRef, m_spos, dist, &m_filter,
m_polys, m_parent, 0, &m_npolys, MAX_POLYS);
}
}
else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_SHAPE)
{
if (m_sposSet && m_startRef && m_eposSet)
{
const float nx = (m_epos[2] - m_spos[2])*0.25f;
const float nz = -(m_epos[0] - m_spos[0])*0.25f;
const float agentHeight = m_sample ? m_sample->getAgentHeight() : 0;
m_queryPoly[0] = m_spos[0] + nx*1.2f;
m_queryPoly[1] = m_spos[1] + agentHeight/2;
m_queryPoly[2] = m_spos[2] + nz*1.2f;
m_queryPoly[3] = m_spos[0] - nx*1.3f;
m_queryPoly[4] = m_spos[1] + agentHeight/2;
m_queryPoly[5] = m_spos[2] - nz*1.3f;
m_queryPoly[6] = m_epos[0] - nx*0.8f;
m_queryPoly[7] = m_epos[1] + agentHeight/2;
m_queryPoly[8] = m_epos[2] - nz*0.8f;
m_queryPoly[9] = m_epos[0] + nx;
m_queryPoly[10] = m_epos[1] + agentHeight/2;
m_queryPoly[11] = m_epos[2] + nz;
#ifdef DUMP_REQS
printf("fpp %f %f %f %f %f %f %f %f %f %f %f %f 0x%x 0x%x\n",
m_queryPoly[0],m_queryPoly[1],m_queryPoly[2],
m_queryPoly[3],m_queryPoly[4],m_queryPoly[5],
m_queryPoly[6],m_queryPoly[7],m_queryPoly[8],
m_queryPoly[9],m_queryPoly[10],m_queryPoly[11],
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findPolysAroundShape(m_startRef, m_queryPoly, 4, &m_filter,
m_polys, m_parent, 0, &m_npolys, MAX_POLYS);
}
}
else if (m_toolMode == TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD)
{
if (m_sposSet && m_startRef)
{
#ifdef DUMP_REQS
printf("fln %f %f %f %f 0x%x 0x%x\n",
m_spos[0],m_spos[1],m_spos[2], m_neighbourhoodRadius,
m_filter.getIncludeFlags(), m_filter.getExcludeFlags());
#endif
m_navQuery->findLocalNeighbourhood(m_startRef, m_spos, m_neighbourhoodRadius, &m_filter,
m_polys, m_parent, &m_npolys, MAX_POLYS);
}
}
}
void NavMeshTesterTool::handleToggle()
{
// TODO: merge separate to a path iterator. Use same code in recalc() too.
if (m_toolMode != TOOLMODE_PATHFIND_FOLLOW)
return;
if (!m_sposSet || !m_eposSet || !m_startRef || !m_endRef)
return;
static const float STEP_SIZE = 0.5f;
static const float SLOP = 0.01f;
if (m_pathIterNum == 0)
{
m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
m_nsmoothPath = 0;
m_pathIterPolyCount = m_npolys;
if (m_pathIterPolyCount)
memcpy(m_pathIterPolys, m_polys, sizeof(dtPolyRef)*m_pathIterPolyCount);
if (m_pathIterPolyCount)
{
// Iterate over the path to find smooth path on the detail mesh surface.
m_navQuery->closestPointOnPolyBoundary(m_startRef, m_spos, m_iterPos);
m_navQuery->closestPointOnPolyBoundary(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos);
m_nsmoothPath = 0;
dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos);
m_nsmoothPath++;
}
}
dtVcopy(m_prevIterPos, m_iterPos);
m_pathIterNum++;
if (!m_pathIterPolyCount)
return;
if (m_nsmoothPath >= MAX_SMOOTH)
return;
// Move towards target a small advancement at a time until target reached or
// when ran out of memory to store the path.
// Find location to steer towards.
float steerPos[3];
unsigned char steerPosFlag;
dtPolyRef steerPosRef;
if (!getSteerTarget(m_navQuery, m_iterPos, m_targetPos, SLOP,
m_pathIterPolys, m_pathIterPolyCount, steerPos, steerPosFlag, steerPosRef,
m_steerPoints, &m_steerPointCount))
return;
dtVcopy(m_steerPos, steerPos);
bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false;
bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false;
// Find movement delta.
float delta[3], len;
dtVsub(delta, steerPos, m_iterPos);
len = sqrtf(dtVdot(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];
dtVmad(moveTgt, m_iterPos, delta, len);
// Move
float result[3];
dtPolyRef visited[16];
int nvisited = 0;
m_navQuery->moveAlongSurface(m_pathIterPolys[0], m_iterPos, moveTgt, &m_filter,
result, visited, &nvisited, 16);
m_pathIterPolyCount = fixupCorridor(m_pathIterPolys, m_pathIterPolyCount, MAX_POLYS, visited, nvisited);
float h = 0;
m_navQuery->getPolyHeight(m_pathIterPolys[0], result, &h);
result[1] = h;
dtVcopy(m_iterPos, result);
// Handle end of path and off-mesh links when close enough.
if (endOfPath && inRange(m_iterPos, steerPos, SLOP, 1.0f))
{
// Reached end of path.
dtVcopy(m_iterPos, m_targetPos);
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos);
m_nsmoothPath++;
}
return;
}
else if (offMeshConnection && inRange(m_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 = m_pathIterPolys[0];
int npos = 0;
while (npos < m_pathIterPolyCount && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = m_pathIterPolys[npos];
npos++;
}
for (int i = npos; i < m_pathIterPolyCount; ++i)
m_pathIterPolys[i-npos] = m_pathIterPolys[i];
m_pathIterPolyCount -= npos;
// Handle the connection.
if (m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos) == DT_SUCCESS)
{
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if (m_nsmoothPath & 1)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], startPos);
m_nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
dtVcopy(m_iterPos, endPos);
float h;
m_navQuery->getPolyHeight(m_pathIterPolys[0], m_iterPos, &h);
m_iterPos[1] = h;
}
}
// Store results.
if (m_nsmoothPath < MAX_SMOOTH)
{
dtVcopy(&m_smoothPath[m_nsmoothPath*3], m_iterPos);
m_nsmoothPath++;
}
}
void dtClosestPtPointTriangle(float* closest, const float* p,
const float* a, const float* b, const float* c)
{
// Check if P in vertex region outside A
float ab[3], ac[3], ap[3];
dtVsub(ab, b, a);
dtVsub(ac, c, a);
dtVsub(ap, p, a);
float d1 = dtVdot(ab, ap);
float d2 = dtVdot(ac, ap);
if (d1 <= 0.0f && d2 <= 0.0f)
{
// barycentric coordinates (1,0,0)
dtVcopy(closest, a);
return;
}
// Check if P in vertex region outside B
float bp[3];
dtVsub(bp, p, b);
float d3 = dtVdot(ab, bp);
float d4 = dtVdot(ac, bp);
if (d3 >= 0.0f && d4 <= d3)
{
// barycentric coordinates (0,1,0)
dtVcopy(closest, b);
return;
}
// Check if P in edge region of AB, if so return projection of P onto AB
float vc = d1*d4 - d3*d2;
if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
{
// barycentric coordinates (1-v,v,0)
float v = d1 / (d1 - d3);
closest[0] = a[0] + v * ab[0];
closest[1] = a[1] + v * ab[1];
closest[2] = a[2] + v * ab[2];
return;
}
// Check if P in vertex region outside C
float cp[3];
dtVsub(cp, p, c);
float d5 = dtVdot(ab, cp);
float d6 = dtVdot(ac, cp);
if (d6 >= 0.0f && d5 <= d6)
{
// barycentric coordinates (0,0,1)
dtVcopy(closest, c);
return;
}
// Check if P in edge region of AC, if so return projection of P onto AC
float vb = d5*d2 - d1*d6;
if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
{
// barycentric coordinates (1-w,0,w)
float w = d2 / (d2 - d6);
closest[0] = a[0] + w * ac[0];
closest[1] = a[1] + w * ac[1];
closest[2] = a[2] + w * ac[2];
return;
}
// Check if P in edge region of BC, if so return projection of P onto BC
float va = d3*d6 - d5*d4;
if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
{
// barycentric coordinates (0,1-w,w)
float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
closest[0] = b[0] + w * (c[0] - b[0]);
closest[1] = b[1] + w * (c[1] - b[1]);
closest[2] = b[2] + w * (c[2] - b[2]);
return;
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
float denom = 1.0f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
closest[0] = a[0] + ab[0] * v + ac[0] * w;
closest[1] = a[1] + ab[1] * v + ac[1] * w;
closest[2] = a[2] + ab[2] * v + ac[2] * w;
}
void cocos2d::NavMesh::findPath(const Vec3 &start, const Vec3 &end, std::vector<Vec3> &pathPoints)
{
static const int MAX_POLYS = 256;
static const int MAX_SMOOTH = 2048;
float ext[3];
ext[0] = 2; ext[1] = 4; ext[2] = 2;
dtQueryFilter filter;
dtPolyRef startRef, endRef;
dtPolyRef polys[MAX_POLYS];
int npolys = 0;
_navMeshQuery->findNearestPoly(&start.x, ext, &filter, &startRef, 0);
_navMeshQuery->findNearestPoly(&end.x, ext, &filter, &endRef, 0);
_navMeshQuery->findPath(startRef, endRef, &start.x, &end.x, &filter, polys, &npolys, MAX_POLYS);
if (npolys)
{
//// Iterate over the path to find smooth path on the detail mesh surface.
//dtPolyRef polys[MAX_POLYS];
//memcpy(polys, polys, sizeof(dtPolyRef)*npolys);
//int npolys = npolys;
float iterPos[3], targetPos[3];
_navMeshQuery->closestPointOnPoly(startRef, &start.x, iterPos, 0);
_navMeshQuery->closestPointOnPoly(polys[npolys - 1], &end.x, targetPos, 0);
static const float STEP_SIZE = 0.5f;
static const float SLOP = 0.01f;
int nsmoothPath = 0;
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2]));
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 && nsmoothPath < MAX_SMOOTH)
{
// Find location to steer towards.
float steerPos[3];
unsigned char steerPosFlag;
dtPolyRef steerPosRef;
if (!getSteerTarget(_navMeshQuery, 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;
dtVsub(delta, steerPos, iterPos);
len = dtMathSqrtf(dtVdot(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];
dtVmad(moveTgt, iterPos, delta, len);
// Move
float result[3];
dtPolyRef visited[16];
int nvisited = 0;
_navMeshQuery->moveAlongSurface(polys[0], iterPos, moveTgt, &filter,
result, visited, &nvisited, 16);
npolys = fixupCorridor(polys, npolys, MAX_POLYS, visited, nvisited);
npolys = fixupShortcuts(polys, npolys, _navMeshQuery);
float h = 0;
_navMeshQuery->getPolyHeight(polys[0], result, &h);
result[1] = h;
dtVcopy(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.
dtVcopy(iterPos, targetPos);
if (nsmoothPath < MAX_SMOOTH)
{
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2]));
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];
int npos = 0;
while (npos < npolys && polyRef != steerPosRef)
{
prevRef = polyRef;
polyRef = polys[npos];
npos++;
}
for (int i = npos; i < npolys; ++i)
polys[i - npos] = polys[i];
npolys -= npos;
// Handle the connection.
dtStatus status = _navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos);
if (dtStatusSucceed(status))
{
if (nsmoothPath < MAX_SMOOTH)
{
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(startPos[0], startPos[1], startPos[2]));
nsmoothPath++;
// Hack to make the dotted path not visible during off-mesh connection.
if (nsmoothPath & 1)
{
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], startPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(startPos[0], startPos[1], startPos[2]));
nsmoothPath++;
}
}
// Move position at the other side of the off-mesh link.
dtVcopy(iterPos, endPos);
float eh = 0.0f;
_navMeshQuery->getPolyHeight(polys[0], iterPos, &eh);
iterPos[1] = eh;
}
}
// Store results.
if (nsmoothPath < MAX_SMOOTH)
{
//dtVcopy(&m_smoothPath[m_nsmoothPath * 3], iterPos);
//m_nsmoothPath++;
pathPoints.push_back(Vec3(iterPos[0], iterPos[1], iterPos[2]));
nsmoothPath++;
}
}
}
}