1065 lines
29 KiB
C++
1065 lines
29 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
#include "Detour/DetourNavLinkBuilder.h"
|
|
|
|
#include "Detour/DetourCommon.h"
|
|
#include "DetourTileCache/DetourTileCacheBuilder.h"
|
|
#include "Recast/Recast.h"
|
|
|
|
//@UE BEGIN
|
|
namespace UE::Detour::NavLink::Private
|
|
{
|
|
static void insertSort(unsigned char* a, const int n)
|
|
{
|
|
int j;
|
|
for (int i = 1; i < n; i++)
|
|
{
|
|
const unsigned char value = a[i];
|
|
for (j = i - 1; j >= 0 && a[j] > value; j--)
|
|
{
|
|
a[j+1] = a[j];
|
|
}
|
|
a[j+1] = value;
|
|
}
|
|
}
|
|
|
|
static dtReal getClosestPtPtSeg(const dtReal* pt, const dtReal* sp, const dtReal* sq)
|
|
{
|
|
dtReal dir[3], diff[3];
|
|
dtVsub(dir, sq, sp);
|
|
dtVsub(diff, pt, sp);
|
|
dtReal t = dtVdot(dir,diff);
|
|
if (t <= 0.0)
|
|
return 0.0;
|
|
|
|
const dtReal d = dtVdot(dir,dir);
|
|
if (t >= d)
|
|
return 1.0;
|
|
|
|
return t/d;
|
|
}
|
|
|
|
static bool isectSegAABB(const dtReal* sp, const dtReal* sq,
|
|
const float* amin, const float* amax,
|
|
float& tmin, float& tmax)
|
|
{
|
|
static constexpr float EPS = 1e-6f;
|
|
|
|
dtReal d[3];
|
|
dtVsub(d, sq, sp);
|
|
tmin = 0; // set to -FLT_MAX to get first hit on the 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 (dtAbs(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)
|
|
dtSwap(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;
|
|
}
|
|
|
|
static float getHeight(const float x, const float* pts, const int npts)
|
|
{
|
|
if (x <= pts[0])
|
|
return pts[1];
|
|
|
|
if (x >= pts[(npts-1)*2])
|
|
return pts[(npts-1)*2+1];
|
|
|
|
for (int i = 1; i < npts; ++i)
|
|
{
|
|
const float* q = &pts[i*2];
|
|
if (x <= q[0])
|
|
{
|
|
const float* p = &pts[(i-1)*2];
|
|
const float u = (x-p[0]) / (q[0]-p[0]);
|
|
return dtLerp(p[1], q[1], u);
|
|
}
|
|
}
|
|
|
|
return pts[(npts-1)*2+1];
|
|
}
|
|
|
|
inline bool overlapRange(const float amin, const float amax, const float bmin, const float bmax)
|
|
{
|
|
return (amin <= bmax && amax >= bmin);
|
|
}
|
|
|
|
inline void trans2d(dtReal* dst, const dtReal* ax, const dtReal* ay, const float* pt)
|
|
{
|
|
dst[0] = ax[0]*pt[0] + ay[0]*pt[1];
|
|
dst[1] = ax[1]*pt[0] + ay[1]*pt[1];
|
|
dst[2] = ax[2]*pt[0] + ay[2]*pt[1];
|
|
}
|
|
|
|
static float getDistanceThreshold(const dtLinkBuilderConfig& config, const dtNavLinkAction action)
|
|
{
|
|
switch(action)
|
|
{
|
|
case DT_LINK_ACTION_JUMP_DOWN:
|
|
return config.jumpDownConfig.filterDistanceThreshold;
|
|
case DT_LINK_ACTION_JUMP_OVER:
|
|
return config.jumpOverConfig.filterDistanceThreshold;
|
|
default:
|
|
const bool dtval = false;
|
|
dtAssert(dtval);
|
|
return 100.f;
|
|
}
|
|
}
|
|
}
|
|
|
|
dtNavLinkBuilder::GroundSegment::~GroundSegment()
|
|
{
|
|
}
|
|
|
|
bool dtNavLinkBuilder::findEdges(rcContext& ctx, const rcConfig& cfg, const dtLinkBuilderConfig& builderConfig,
|
|
const dtTileCacheContourSet& lcset, const dtReal* orig,
|
|
const rcHeightfield* solidHF, const rcCompactHeightfield* compactHF)
|
|
{
|
|
dtAssert(m_solid == nullptr && m_chf == nullptr && m_edges.IsEmpty() && m_links.IsEmpty());
|
|
m_linkBuilderConfig = builderConfig;
|
|
|
|
m_cs = cfg.cs;
|
|
m_csSquared = dtSqr(cfg.cs);
|
|
m_ch = cfg.ch;
|
|
m_invCs = 1.0/cfg.cs;
|
|
m_solid = solidHF;
|
|
m_chf = compactHF;
|
|
|
|
dtAssert(m_cs == m_chf->cs && m_ch == m_chf->ch);
|
|
|
|
// Build edges.
|
|
int edgeCount = 0;
|
|
for (int i = 0; i < lcset.nconts; ++i)
|
|
{
|
|
edgeCount += lcset.conts[i].nverts;
|
|
}
|
|
|
|
if (edgeCount == 0)
|
|
{
|
|
ctx.log(RC_LOG_ERROR, "fillEdges: No edges!");
|
|
return false;
|
|
}
|
|
|
|
m_edges.Reserve(edgeCount);
|
|
|
|
const dtReal cs = cfg.cs;
|
|
const dtReal ch = cfg.ch;
|
|
|
|
for (int i = 0; i < lcset.nconts; ++i)
|
|
{
|
|
const dtTileCacheContour& c = lcset.conts[i];
|
|
if (!c.nverts)
|
|
continue;
|
|
|
|
for (int j = 0, k = c.nverts-1; j < c.nverts; k=j++)
|
|
{
|
|
const unsigned short* va = &c.verts[k*4];
|
|
const unsigned short* vb = &c.verts[j*4];
|
|
|
|
if ((va[3] & 0xf) != 0xf) // A direction is set, so it's a portal edge.
|
|
continue;
|
|
|
|
// Check k-j for matching contour
|
|
bool matchFound = false;
|
|
for (int ii = 0; ii < lcset.nconts; ++ii)
|
|
{
|
|
if (i == ii)
|
|
continue;
|
|
|
|
const dtTileCacheContour& otherCont = lcset.conts[ii];
|
|
if (otherCont.nverts < 3)
|
|
continue;
|
|
|
|
for (int jj = 0, kk = otherCont.nverts-1; jj < otherCont.nverts; kk=jj++)
|
|
{
|
|
const unsigned short* otherVa = &otherCont.verts[kk*4];
|
|
const unsigned short* otherVb = &otherCont.verts[jj*4];
|
|
|
|
if( (dtVisEqual(va, otherVa) && dtVisEqual(vb, otherVb)) ||
|
|
(dtVisEqual(va, otherVb) && dtVisEqual(vb, otherVa)) )
|
|
{
|
|
// Same edge, skip it.
|
|
matchFound = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (matchFound)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!matchFound)
|
|
{
|
|
// Add edge
|
|
Edge& e = m_edges.Emplace_GetRef();
|
|
|
|
e.sp[0] = orig[0] + vb[0]*cs;
|
|
e.sp[1] = orig[1] + (vb[1]+2)*ch;
|
|
e.sp[2] = orig[2] + vb[2]*cs;
|
|
|
|
e.sq[0] = orig[0] + va[0]*cs;
|
|
e.sq[1] = orig[1] + (va[1]+2)*ch;
|
|
e.sq[2] = orig[2] + va[2]*cs;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void dtNavLinkBuilder::addEdgeLinks(const dtLinkBuilderConfig& builderConfig, const EdgeSampler* es, const int edgeIndex)
|
|
{
|
|
TRACE_CPUPROFILER_EVENT_SCOPE(dtNavLinkBuilder::addEdgeLinks);
|
|
|
|
using namespace UE::Detour::NavLink::Private;
|
|
|
|
if (es->start.ngsamples != es->end.ngsamples)
|
|
{
|
|
return;
|
|
}
|
|
|
|
const int nsamples = es->start.ngsamples;
|
|
|
|
// Filter small holes.
|
|
constexpr int RAD = 2;
|
|
GroundSampleFlag kernel[RAD*2+1];
|
|
|
|
TArray<GroundSampleFlag, TInlineAllocator<64>> groundSampleFlags;
|
|
groundSampleFlags.Reserve(nsamples);
|
|
|
|
for (int i = 0; i < nsamples; ++i)
|
|
{
|
|
const int a = dtMax(0, i-RAD);
|
|
const int b = dtMin(nsamples-1, i+RAD);
|
|
int nkernel = 0;
|
|
for (int j = a; j <= b; ++j)
|
|
{
|
|
kernel[nkernel++] = (GroundSampleFlag)(es->start.gsamples[i].flags & UNRESTRICTED);
|
|
}
|
|
insertSort((unsigned char*)kernel, nkernel);
|
|
groundSampleFlags.Add(kernel[(nkernel+1)/2]);
|
|
}
|
|
|
|
const dtReal edgeLength = dtVdist(es->rigp, es->rigq);
|
|
const dtReal distanceBetweenSamples = edgeLength / (es->start.ngsamples-1);
|
|
|
|
// Build segments
|
|
int start = -1;
|
|
for (int i = 0; i <= nsamples; ++i)
|
|
{
|
|
const bool valid = i < nsamples && groundSampleFlags[i] != UNSET;
|
|
if (start == -1)
|
|
{
|
|
if (valid)
|
|
start = i;
|
|
}
|
|
else
|
|
{
|
|
if (!valid)
|
|
{
|
|
const dtReal freeWidth = ((i-start)-1)*distanceBetweenSamples;
|
|
if (freeWidth >= builderConfig.agentRadius)
|
|
{
|
|
const float u0 = (float)start/(float)(nsamples-1);
|
|
const float u1 = (float)(i-1)/(float)(nsamples-1);
|
|
|
|
dtReal sp[3], sq[3], ep[3], eq[3];
|
|
|
|
dtVlerp(sp, es->start.p,es->start.q, u0);
|
|
dtVlerp(sq, es->start.p,es->start.q, u1);
|
|
dtVlerp(ep, es->end.p,es->end.q, u0);
|
|
dtVlerp(eq, es->end.p,es->end.q, u1);
|
|
sp[1] = es->start.gsamples[start].height;
|
|
sq[1] = es->start.gsamples[i-1].height;
|
|
ep[1] = es->end.gsamples[start].height;
|
|
eq[1] = es->end.gsamples[i-1].height;
|
|
|
|
JumpLink& link = m_links.Emplace_GetRef();
|
|
|
|
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
|
|
link.debugSourceEdge = (short)edgeIndex;
|
|
#endif
|
|
link.action = es->action;
|
|
link.flags = VALID;
|
|
link.nspine = es->trajectory.nspine;
|
|
|
|
const float startx = es->trajectory.spine[0];
|
|
const float endx = es->trajectory.spine[(es->trajectory.nspine-1)*2];
|
|
const float deltax = endx - startx;
|
|
|
|
const float starty = es->trajectory.spine[1];
|
|
const float endy = es->trajectory.spine[(es->trajectory.nspine-1)*2+1];
|
|
|
|
// Build link->spine0
|
|
for (int j = 0; j < es->trajectory.nspine; ++j)
|
|
{
|
|
const float* spt = &es->trajectory.spine[j*2];
|
|
const float u = (spt[0] - startx)/deltax;
|
|
const float dy = spt[1] - dtLerp(starty, endy, u) + m_linkBuilderConfig.agentClimb;
|
|
dtReal* p = &link.spine0[j*3];
|
|
dtVlerp(p, sp, ep, u);
|
|
dtVmad(p, p, es->ay, dy);
|
|
}
|
|
|
|
// Build link->spine1
|
|
for (int j = 0; j < es->trajectory.nspine; ++j)
|
|
{
|
|
const float* spt = &es->trajectory.spine[j*2];
|
|
const float u = (spt[0] - startx)/deltax;
|
|
const float dy = spt[1] - dtLerp(starty, endy, u) + m_linkBuilderConfig.agentClimb;
|
|
dtReal* p = &link.spine1[j*3];
|
|
dtVlerp(p, sq, eq, u);
|
|
dtVmad(p, p, es->ay, dy);
|
|
}
|
|
}
|
|
|
|
start = -1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void dtNavLinkBuilder::filterOverlappingLinks(const float edgeDistanceThreshold)
|
|
{
|
|
TRACE_CPUPROFILER_EVENT_SCOPE(dtNavLinkBuilder::filterOverlappingLinks);
|
|
|
|
using namespace UE::Detour::NavLink::Private;
|
|
|
|
// Filter out links which overlap
|
|
const float thresholdSquared = dtSqr(edgeDistanceThreshold);
|
|
|
|
for (int i = 0; i < m_links.Num()-1; ++i)
|
|
{
|
|
JumpLink& li = m_links[i];
|
|
if (li.flags == FILTERED)
|
|
continue;
|
|
|
|
const dtReal* spi = &li.spine0[0];
|
|
const dtReal* sqi = &li.spine1[0];
|
|
const dtReal* epi = &li.spine0[(li.nspine-1)*3];
|
|
const dtReal* eqi = &li.spine1[(li.nspine-1)*3];
|
|
|
|
for (int j = i+1; j < m_links.Num(); ++j)
|
|
{
|
|
JumpLink& lj = m_links[j];
|
|
if (lj.flags == FILTERED)
|
|
continue;
|
|
|
|
const dtReal* spj = &lj.spine0[0];
|
|
const dtReal* sqj = &lj.spine1[0];
|
|
const dtReal* epj = &lj.spine0[(lj.nspine-1)*3];
|
|
const dtReal* eqj = &lj.spine1[(lj.nspine-1)*3];
|
|
|
|
const dtReal d0 = dtDistancePtSegSqr(spj, epi, eqi);
|
|
const dtReal d1 = dtDistancePtSegSqr(sqj, epi, eqi);
|
|
const dtReal d2 = dtDistancePtSegSqr(epj, spi, sqi);
|
|
const dtReal d3 = dtDistancePtSegSqr(eqj, spi, sqi);
|
|
|
|
if (d0 < thresholdSquared && d1 < thresholdSquared && d2 < thresholdSquared && d3 < thresholdSquared)
|
|
{
|
|
// Remove one of the link, keeping the wider one
|
|
if (dtVdistSqr(spi,sqi) > dtVdistSqr(spj,sqj))
|
|
{
|
|
lj.flags = FILTERED;
|
|
}
|
|
else
|
|
{
|
|
li.flags = FILTERED;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void dtNavLinkBuilder::buildForAllEdges(rcContext& ctx, const dtLinkBuilderConfig& builderConfig, dtNavLinkAction action)
|
|
{
|
|
for (int i = 0; i < m_edges.Num(); ++i)
|
|
{
|
|
EdgeSampler sampler;
|
|
const bool success = sampleEdge(builderConfig, action, m_edges[i].sp, m_edges[i].sq, &sampler);
|
|
if (success)
|
|
{
|
|
addEdgeLinks(builderConfig, &sampler, i);
|
|
}
|
|
}
|
|
|
|
ctx.log(RC_LOG_PROGRESS, " %i links added.", m_links.Num());
|
|
|
|
const float distanceThreshold = UE::Detour::NavLink::Private::getDistanceThreshold(builderConfig, action);
|
|
filterOverlappingLinks(distanceThreshold);
|
|
}
|
|
|
|
void dtNavLinkBuilder::debugBuildEdge(const dtLinkBuilderConfig& builderConfig, dtNavLinkAction action, int edgeIndex, EdgeSampler& sampler)
|
|
{
|
|
if (edgeIndex >= m_edges.Num())
|
|
{
|
|
return;
|
|
}
|
|
|
|
m_debugSelectedEdge = edgeIndex;
|
|
|
|
const bool success = sampleEdge(builderConfig, action, m_edges[edgeIndex].sp, m_edges[edgeIndex].sq, &sampler);
|
|
if (success)
|
|
{
|
|
addEdgeLinks(builderConfig, &sampler, edgeIndex);
|
|
}
|
|
|
|
const float distanceThreshold = UE::Detour::NavLink::Private::getDistanceThreshold(builderConfig, action);
|
|
filterOverlappingLinks(distanceThreshold);
|
|
}
|
|
|
|
bool dtNavLinkBuilder::getCompactHeightfieldHeight(const dtReal* pt, const dtReal hrange, dtReal* height) const
|
|
{
|
|
const int chfWidth = m_chf->width;
|
|
const int chfHeight = m_chf->height;
|
|
|
|
const dtReal range = m_cs;
|
|
const int ix0 = dtClamp((int)dtFloor((pt[0]-range - m_chf->bmin[0])*m_invCs), 0, chfWidth-1);
|
|
const int iz0 = dtClamp((int)dtFloor((pt[2]-range - m_chf->bmin[2])*m_invCs), 0, chfHeight-1);
|
|
const int ix1 = dtClamp((int)dtFloor((pt[0]+range - m_chf->bmin[0])*m_invCs), 0, chfWidth-1);
|
|
const int iz1 = dtClamp((int)dtFloor((pt[2]+range - m_chf->bmin[2])*m_invCs), 0, chfHeight-1);
|
|
|
|
dtReal bestDist = DT_REAL_MAX;
|
|
dtReal bestHeight = DT_REAL_MAX;
|
|
bool found = false;
|
|
|
|
for (int z = iz0; z <= iz1; ++z)
|
|
{
|
|
for (int x = ix0; x <= ix1; ++x)
|
|
{
|
|
const rcCompactCell& c = m_chf->cells[x+z*chfWidth];
|
|
for (unsigned int i = c.index, ni = c.index+c.count; i < ni; ++i)
|
|
{
|
|
if (m_chf->areas[i] == RC_NULL_AREA)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
const dtReal y = m_chf->bmin[1] + m_chf->spans[i].y * m_ch;
|
|
const dtReal dist = abs(y - pt[1]);
|
|
if (dist < hrange && dist < bestDist)
|
|
{
|
|
bestDist = dist;
|
|
bestHeight = y;
|
|
found = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (found)
|
|
{
|
|
*height = bestHeight;
|
|
}
|
|
else
|
|
{
|
|
*height = pt[1];
|
|
}
|
|
|
|
return found;
|
|
}
|
|
|
|
// Compare ymin, ymax range with the solid height field spans to see if it collides.
|
|
// Returns true if there is a collision.
|
|
bool dtNavLinkBuilder::checkHeightfieldCollision(const dtReal x, const dtReal ymin, const dtReal ymax, const dtReal z) const
|
|
{
|
|
using namespace UE::Detour::NavLink::Private;
|
|
|
|
const int w = m_solid->width;
|
|
const int h = m_solid->height;
|
|
const rcReal* orig = m_solid->bmin;
|
|
const int ix = (int)dtFloor((x - orig[0])*m_invCs);
|
|
const int iz = (int)dtFloor((z - orig[2])*m_invCs);
|
|
|
|
if (ix < 0 || iz < 0 || ix > w || iz > h)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
const rcSpan* s = m_solid->spans[ix + iz*w];
|
|
if (!s)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
while (s)
|
|
{
|
|
const float symin = orig[1] + s->data.smin*m_ch;
|
|
const float symax = orig[1] + s->data.smax*m_ch;
|
|
if (overlapRange(ymin, ymax, symin, symax))
|
|
return true;
|
|
|
|
s = s->next;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
// Returns true if none of the samples ymin, ymax collide with the heghtfield.
|
|
bool dtNavLinkBuilder::isTrajectoryClear(const dtReal* pa, const dtReal* pb, const Trajectory2D* trajectory, const dtReal* trajectoryDir) const
|
|
{
|
|
dtReal start[3];
|
|
dtReal end[3];
|
|
dtVcopy(start, pa);
|
|
dtVcopy(end, pb);
|
|
|
|
// Offset start and end points to account for the agent radius.
|
|
dtVmad(start, pa, trajectoryDir, -trajectory->radiusOverflow);
|
|
dtVmad(end, pb, trajectoryDir, trajectory->radiusOverflow);
|
|
|
|
const int nsamples = trajectory->samples.Num();
|
|
const float invLastSample = 1.f / (nsamples-1);
|
|
for (int i = 0; i < nsamples; ++i)
|
|
{
|
|
dtReal p[3];
|
|
const TrajectorySample& s = trajectory->samples[i];
|
|
const float u = (float)i * invLastSample;
|
|
dtVlerp(p, start, end, u);
|
|
|
|
if (checkHeightfieldCollision(p[0], p[1] + s.ymin, p[1] + s.ymax, p[2]))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
// Add ground samples and set height on them.
|
|
void dtNavLinkBuilder::sampleGroundSegment(GroundSegment* seg, const int nsamples, const float groundRange) const
|
|
{
|
|
TRACE_CPUPROFILER_EVENT_SCOPE(dtNavLinkBuilder::sampleGroundSegment);
|
|
|
|
dtReal delta[3];
|
|
dtVsub(delta, seg->p, seg->q);
|
|
|
|
seg->ngsamples = nsamples;
|
|
seg->npass = 0;
|
|
|
|
const float invLastIndex = 1.f/(nsamples-1);
|
|
for (int i = 0; i < nsamples; ++i)
|
|
{
|
|
const float u = (float)i*invLastIndex;
|
|
dtReal pt[3];
|
|
|
|
GroundSample& s = seg->gsamples.Emplace_GetRef();
|
|
dtVlerp(pt, seg->p, seg->q, u);
|
|
s.flags = dtNavLinkBuilder::UNSET;
|
|
if (!getCompactHeightfieldHeight(pt, groundRange, &s.height))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
s.flags = static_cast<GroundSampleFlag>((unsigned char)s.flags | (unsigned char)HAS_GROUND);
|
|
seg->npass++;
|
|
}
|
|
}
|
|
|
|
void dtNavLinkBuilder::updateTrajectorySamples(EdgeSampler* es) const
|
|
{
|
|
TRACE_CPUPROFILER_EVENT_SCOPE(dtNavLinkBuilder::updateTrajectorySamples);
|
|
|
|
if (es->start.ngsamples != es->end.ngsamples)
|
|
return;
|
|
|
|
const int nsamples = es->start.ngsamples;
|
|
|
|
for (int i = 0; i < nsamples; ++i)
|
|
{
|
|
GroundSample& ssmp = es->start.gsamples[i];
|
|
GroundSample& esmp = es->end.gsamples[i];
|
|
|
|
// If there is no ground, the ground height will not be set.
|
|
if ((ssmp.flags & HAS_GROUND) == 0 || (esmp.flags & HAS_GROUND) == 0)
|
|
continue;
|
|
|
|
// When we sample ground segments, in sampleEdges, we have add least 2 samples.
|
|
check(nsamples >= 2);
|
|
const dtReal u = (dtReal)i/(dtReal)(nsamples-1);
|
|
dtReal spt[3], ept[3];
|
|
dtVlerp(spt, es->start.p, es->start.q, u);
|
|
dtVlerp(ept, es->end.p, es->end.q, u);
|
|
|
|
// Offset start and end points to account for the agent radius.
|
|
dtVmad(spt, spt, es->az, -es->trajectory.radiusOverflow);
|
|
dtVmad(ept, ept, es->az, es->trajectory.radiusOverflow);
|
|
|
|
const int nTrajectorySamples = es->trajectory.samples.Num();
|
|
// When we initialize trajectory samples (initTrajectorySamples), we add at least 2 trajectory samples.
|
|
check(nTrajectorySamples >= 2);
|
|
const float invLastTrajSample = 1.f / (nTrajectorySamples-1);
|
|
for (int trajIndex = 0; trajIndex < nTrajectorySamples; ++trajIndex)
|
|
{
|
|
dtReal p[3];
|
|
TrajectorySample& s = es->trajectory.samples[trajIndex];
|
|
const float trajU = (float)trajIndex * invLastTrajSample;
|
|
dtVlerp(p, spt, ept, trajU);
|
|
|
|
if (s.floorStart)
|
|
{
|
|
s.ymin = (ssmp.height + m_linkBuilderConfig.agentClimb) - p[1]; // -p[1] to stay relative to p[1]
|
|
|
|
// Update ymax if ymin is now higher than ymax.
|
|
s.ymax = dtMax(s.ymin, s.ymax);
|
|
}
|
|
else if (s.floorEnd)
|
|
{
|
|
s.ymin = (esmp.height + m_linkBuilderConfig.agentClimb) - p[1]; // -p[1] to stay relative to p[1]
|
|
|
|
// Update ymax if ymin is now higher than ymax.
|
|
s.ymax = dtMax(s.ymin, s.ymax);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void dtNavLinkBuilder::sampleAction(EdgeSampler* es) const
|
|
{
|
|
TRACE_CPUPROFILER_EVENT_SCOPE(dtNavLinkBuilder::sampleAction);
|
|
|
|
if (es->start.ngsamples != es->end.ngsamples)
|
|
return;
|
|
|
|
const int nsamples = es->start.ngsamples;
|
|
|
|
for (int i = 0; i < nsamples; ++i)
|
|
{
|
|
GroundSample& ssmp = es->start.gsamples[i];
|
|
GroundSample& esmp = es->end.gsamples[i];
|
|
|
|
if ((ssmp.flags & HAS_GROUND) == 0 || (esmp.flags & HAS_GROUND) == 0)
|
|
continue;
|
|
|
|
const dtReal u = (dtReal)i/(dtReal)(nsamples-1);
|
|
dtReal spt[3], ept[3];
|
|
dtVlerp(spt, es->start.p, es->start.q, u);
|
|
dtVlerp(ept, es->end.p, es->end.q, u);
|
|
|
|
if (!isTrajectoryClear(spt, ept, &es->trajectory, es->az))
|
|
continue;
|
|
|
|
ssmp.flags = static_cast<GroundSampleFlag>((unsigned char)ssmp.flags | (unsigned char)UNRESTRICTED);
|
|
}
|
|
}
|
|
|
|
void dtNavLinkBuilder::initTrajectorySamples(const float groundRange, Trajectory2D* trajectory) const
|
|
{
|
|
using namespace UE::Detour::NavLink::Private;
|
|
|
|
const float agentRadius = (float)m_linkBuilderConfig.agentRadius;
|
|
trajectory->radiusOverflow = agentRadius;
|
|
|
|
// Spine points [x,y]. y is up and x is in the direction of the trajectory, relative to the edge.
|
|
float pa[2] = { trajectory->spine[0], trajectory->spine[1] };
|
|
float pb[2] = { trajectory->spine[(trajectory->nspine-1)*2], trajectory->spine[(trajectory->nspine-1)*2+1] };
|
|
|
|
// Finding samples along the spine accounting for the agent size,
|
|
// so we need to look a bit before and after the desired trajectory.
|
|
pa[0] -= agentRadius;
|
|
pb[0] += agentRadius;
|
|
|
|
const float dx = pb[0] - pa[0];
|
|
const int nsamples = dtMax(2, (int)ceilf(dx*m_invCs));
|
|
trajectory->samples.Reserve(nsamples);
|
|
|
|
const float dxSample = dx/nsamples;
|
|
const float roundedAgentRadius = dxSample > 0.f ? ceilf(agentRadius/dxSample)*dxSample : 0.f;
|
|
|
|
const float* spine = trajectory->spine;
|
|
unsigned char nspine = trajectory->nspine;
|
|
|
|
const unsigned short lastSampleIndex = nsamples-1;
|
|
const float invLastIndex = 1.f/lastSampleIndex;
|
|
for (int i = 0; i < nsamples; ++i)
|
|
{
|
|
const float u = (float)i * invLastIndex;
|
|
const float xRef = dtLerp(pa[0], pb[0], u);
|
|
const float yRef = dtLerp(pa[1], pb[1], u);
|
|
|
|
// Sample the height on the spine at 3 locations to get an approximated min and max y.
|
|
const float y0 = getHeight(xRef - agentRadius, spine, nspine);
|
|
const float y1 = getHeight(xRef + agentRadius, spine, nspine);
|
|
const float y2 = getHeight(xRef, spine, nspine);
|
|
|
|
TrajectorySample& s = trajectory->samples.Emplace_GetRef();
|
|
s.ymin = dtMin(dtMin(y0,y1), y2) + m_linkBuilderConfig.agentClimb - yRef;
|
|
s.ymax = dtMax(dtMax(y0,y1), y2) + m_linkBuilderConfig.agentHeight - yRef;
|
|
|
|
// Mark start samples that need to be floored.
|
|
if (xRef >= (spine[0]-roundedAgentRadius) && xRef <= spine[0]+roundedAgentRadius)
|
|
s.floorStart = true;
|
|
|
|
// More importantly, mark samples that need to be floored at the end since the ground could be far from trajectory end point.
|
|
// We use the upper bound of the tolerance at the end segment (groundRange) to identify samples that need to be floored.
|
|
// Min values below the upper bound need to be marked.
|
|
const float endSplineHeight = pb[1];
|
|
if (s.ymin + yRef < endSplineHeight + groundRange)
|
|
{
|
|
s.floorEnd = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
int dtNavLinkBuilder::findPotentialJumpOverEdges(const dtReal* sp, const dtReal* sq,
|
|
const float depthRange, const float heightRange,
|
|
dtReal* outSegs, const int maxOutSegs) const
|
|
{
|
|
TRACE_CPUPROFILER_EVENT_SCOPE(dtNavLinkBuilder::findPotentialJumpOverEdges);
|
|
|
|
using namespace UE::Detour::NavLink::Private;
|
|
|
|
// Find potential edges to join to.
|
|
const float widthRange = sqrtf(dtVdistSqr(sp,sq));
|
|
const float amin[3] = { 0, -heightRange*0.5f, 0 };
|
|
const float amax[3] = { widthRange, heightRange*0.5f, depthRange};
|
|
|
|
const dtReal thr = cosf((180.0 - 45.0)/180.0*RC_PI);
|
|
|
|
dtReal ax[3], ay[3], az[3];
|
|
dtVsub(ax, sq, sp);
|
|
dtVnormalize(ax);
|
|
dtVset(az, ax[2], 0, -ax[0]);
|
|
dtVnormalize(az);
|
|
dtVset(ay, 0, 1, 0);
|
|
|
|
static constexpr int MAX_SEGS = 64;
|
|
PotentialSeg segs[MAX_SEGS];
|
|
int nsegs = 0;
|
|
|
|
for (int i = 0; i < m_edges.Num(); ++i)
|
|
{
|
|
dtReal p[3], lsp[3], lsq[3];
|
|
dtVsub(p, m_edges[i].sp, sp);
|
|
lsp[0] = ax[0]*p[0] + ay[0]*p[1] + az[0]*p[2];
|
|
lsp[1] = ax[1]*p[0] + ay[1]*p[1] + az[1]*p[2];
|
|
lsp[2] = ax[2]*p[0] + ay[2]*p[1] + az[2]*p[2];
|
|
|
|
dtVsub(p, m_edges[i].sq, sp);
|
|
lsq[0] = ax[0]*p[0] + ay[0]*p[1] + az[0]*p[2];
|
|
lsq[1] = ax[1]*p[0] + ay[1]*p[1] + az[1]*p[2];
|
|
lsq[2] = ax[2]*p[0] + ay[2]*p[1] + az[2]*p[2];
|
|
|
|
float tmin, tmax;
|
|
if (isectSegAABB(lsp, lsq, amin, amax, tmin, tmax))
|
|
{
|
|
if (tmin > 1.0f)
|
|
continue;
|
|
if (tmax < 0.0f)
|
|
continue;
|
|
|
|
dtReal edir[3];
|
|
dtVsub(edir, m_edges[i].sq, m_edges[i].sp);
|
|
edir[1] = 0;
|
|
dtVnormalize(edir);
|
|
|
|
if (dtVdot(ax, edir) > thr)
|
|
continue;
|
|
|
|
if (nsegs < MAX_SEGS)
|
|
{
|
|
segs[nsegs].umin = dtClamp(tmin, 0.0f, 1.0f);
|
|
segs[nsegs].umax = dtClamp(tmax, 0.0f, 1.0f);
|
|
segs[nsegs].dmin = dtMin(lsp[2], lsq[2]);
|
|
segs[nsegs].dmax = dtMax(lsp[2], lsq[2]);
|
|
segs[nsegs].idx = i;
|
|
segs[nsegs].mark = 0;
|
|
nsegs++;
|
|
}
|
|
}
|
|
}
|
|
|
|
const float eps = m_chf->cs;
|
|
unsigned char mark = 1;
|
|
for (int i = 0; i < nsegs; ++i)
|
|
{
|
|
if (segs[i].mark != 0)
|
|
continue;
|
|
|
|
segs[i].mark = mark;
|
|
|
|
for (int j = i+1; j < nsegs; ++j)
|
|
{
|
|
if (overlapRange(segs[i].dmin-eps, segs[i].dmax+eps,
|
|
segs[j].dmin-eps, segs[j].dmax+eps))
|
|
{
|
|
segs[j].mark = mark;
|
|
}
|
|
}
|
|
|
|
mark++;
|
|
}
|
|
|
|
|
|
int nout = 0;
|
|
|
|
for (int i = 1; i < mark; ++i)
|
|
{
|
|
// Find destination mid point.
|
|
constexpr float toUU = 100.f;
|
|
float umin = 10.0f * toUU;
|
|
float umax = -10.0f * toUU;
|
|
dtReal ptmin[3], ptmax[3];
|
|
|
|
for (int j = 0; j < nsegs; ++j)
|
|
{
|
|
PotentialSeg* seg = &segs[j];
|
|
if (seg->mark != (unsigned char)i)
|
|
continue;
|
|
|
|
dtReal pa[3], pb[3];
|
|
dtVlerp(pa, m_edges[seg->idx].sp, m_edges[seg->idx].sq, seg->umin);
|
|
dtVlerp(pb, m_edges[seg->idx].sp, m_edges[seg->idx].sq, seg->umax);
|
|
|
|
const float ua = getClosestPtPtSeg(pa, sp, sq);
|
|
const float ub = getClosestPtPtSeg(pb, sp, sq);
|
|
|
|
if (ua < umin)
|
|
{
|
|
dtVcopy(ptmin, pa);
|
|
umin = ua;
|
|
}
|
|
if (ua > umax)
|
|
{
|
|
dtVcopy(ptmax, pa);
|
|
umax = ua;
|
|
}
|
|
|
|
if (ub < umin)
|
|
{
|
|
dtVcopy(ptmin, pb);
|
|
umin = ub;
|
|
}
|
|
if (ub > umax)
|
|
{
|
|
dtVcopy(ptmax, pb);
|
|
umax = ub;
|
|
}
|
|
}
|
|
|
|
if (umin > umax)
|
|
continue;
|
|
|
|
dtReal end[3];
|
|
dtVlerp(end, ptmin, ptmax, 0.5f);
|
|
|
|
dtReal start[3];
|
|
dtVlerp(start, sp, sq, (umin+umax)*0.5f);
|
|
|
|
dtReal orig[3];
|
|
dtVlerp(orig, start, end, 0.5f);
|
|
|
|
dtReal dir[3], norm[3];
|
|
dtVsub(dir, end, start);
|
|
dir[1] = 0;
|
|
dtVnormalize(dir);
|
|
dtVset(norm, dir[2], 0, -dir[0]);
|
|
|
|
dtReal ssp[3], ssq[3];
|
|
|
|
const dtReal width = widthRange * (umax-umin);
|
|
dtVmad(ssp, orig, norm, width*0.5f);
|
|
dtVmad(ssq, orig, norm, -width*0.5f);
|
|
|
|
if (nout < maxOutSegs)
|
|
{
|
|
dtVcopy(&outSegs[nout*6+0], ssp);
|
|
dtVcopy(&outSegs[nout*6+3], ssq);
|
|
nout++;
|
|
}
|
|
}
|
|
|
|
return nout;
|
|
}
|
|
|
|
|
|
void dtNavLinkBuilder::initJumpDownRig(EdgeSampler* es, const dtReal* sp, const dtReal* sq,
|
|
const dtNavLinkBuilderJumpDownConfig& config) const
|
|
{
|
|
es->action = DT_LINK_ACTION_JUMP_DOWN;
|
|
|
|
// Set axes
|
|
dtVsub(es->ax, sq, sp);
|
|
dtVnormalize(es->ax);
|
|
dtVset(es->az, es->ax[2], 0, -es->ax[0]);
|
|
dtVnormalize(es->az);
|
|
dtVset(es->ay, 0, 1, 0);
|
|
|
|
// Set edge
|
|
const dtReal edgeLengthSqr = dtVdistSqr(sp, sq);
|
|
if (edgeLengthSqr > m_csSquared)
|
|
{
|
|
// Trim tips by cellSize to account for edges overlapping the rasterization borders.
|
|
// This avoids getting the wrong height in getCompactHeightfieldHeight that need to lookup multiple cells.
|
|
dtVmad(es->rigp, sp, es->ax, m_cs);
|
|
dtVmad(es->rigq, sq, es->ax, -m_cs);
|
|
}
|
|
else
|
|
{
|
|
// If it's impossible because the edge is too short, just keep the original edge.
|
|
dtVcopy(es->rigp, sp);
|
|
dtVcopy(es->rigq, sq);
|
|
}
|
|
|
|
// Parabolic equation y(x) = ax^2 + (-d/l - al)x
|
|
// Where 'a' is constant
|
|
// 'l' is the jump length from the starting point
|
|
// 'd' is the distance below the starting point
|
|
|
|
const float jumpStartDist = config.jumpDistanceFromEdge;
|
|
const float jumpLength = config.jumpLength;
|
|
const float a = config.cachedParabolaConstant;
|
|
const float downRatio = config.cachedDownRatio; // -d/l
|
|
|
|
// Build action sampling spine.
|
|
es->trajectory.nspine = MAX_SPINE;
|
|
for (int i = 0; i < MAX_SPINE; ++i)
|
|
{
|
|
float* pt = &es->trajectory.spine[i*2]; // pt: [xy] (x is toward jump end, y is up)
|
|
const float u = (float)i/(float)(MAX_SPINE-1);
|
|
pt[0] = -jumpStartDist + (u*jumpLength);
|
|
|
|
// Parabolic equation y(x) = ax^2 + (-d/l - al)x
|
|
// y(x) = x * (ax + (-d/l - al))
|
|
pt[1] = (u*jumpLength) * (a*(u*jumpLength) + (downRatio - a*jumpLength));
|
|
}
|
|
|
|
es->groundRange = config.jumpEndsHeightTolerance;
|
|
}
|
|
|
|
void dtNavLinkBuilder::initJumpOverRig(EdgeSampler* es, const dtReal* sp, const dtReal* sq,
|
|
const float jumpStartDist, const float jumpEndDist,
|
|
const float jumpHeight, const float groundRange)
|
|
{
|
|
es->action = DT_LINK_ACTION_JUMP_OVER;
|
|
|
|
// Set edge
|
|
dtVcopy(es->rigp, sp);
|
|
dtVcopy(es->rigq, sq);
|
|
|
|
// Set axes
|
|
dtVsub(es->ax, sq, sp);
|
|
dtVnormalize(es->ax);
|
|
dtVset(es->az, es->ax[2], 0, -es->ax[0]);
|
|
dtVnormalize(es->az);
|
|
dtVset(es->ay, 0, 1, 0);
|
|
|
|
// Build action sampling spine.
|
|
es->trajectory.nspine = MAX_SPINE;
|
|
for (int i = 0; i < MAX_SPINE; ++i)
|
|
{
|
|
float* pt = &es->trajectory.spine[i*2];
|
|
const float u = (float)i/(float)(MAX_SPINE-1);
|
|
pt[0] = jumpStartDist + u * (jumpEndDist - jumpStartDist);
|
|
pt[1] = (1-dtSqr(u*2-1)) * jumpHeight;
|
|
}
|
|
|
|
es->groundRange = groundRange;
|
|
}
|
|
|
|
bool dtNavLinkBuilder::sampleEdge(const dtLinkBuilderConfig& builderConfig, dtNavLinkAction desiredAction, const dtReal* sp, const dtReal* sq, dtNavLinkBuilder::EdgeSampler* es) const
|
|
{
|
|
TRACE_CPUPROFILER_EVENT_SCOPE(dtNavLinkBuilder::sampleEdge);
|
|
|
|
using namespace UE::Detour::NavLink::Private;
|
|
|
|
float samplingSeparationFactor = 1.f;
|
|
|
|
if (desiredAction == DT_LINK_ACTION_JUMP_DOWN)
|
|
{
|
|
const dtNavLinkBuilderJumpDownConfig& config = builderConfig.jumpDownConfig;
|
|
samplingSeparationFactor = config.samplingSeparationFactor;
|
|
initJumpDownRig(es, sp, sq, config);
|
|
}
|
|
else if (desiredAction == DT_LINK_ACTION_JUMP_OVER)
|
|
{
|
|
const dtNavLinkBuilderJumpOverConfig& config = builderConfig.jumpOverConfig;
|
|
const float jumpDist = config.jumpGapWidth;
|
|
const float heightRange = config.jumpGapHeightTolerance;
|
|
static constexpr int NSEGS = 8;
|
|
dtReal segs[NSEGS*6];
|
|
int nsegs = findPotentialJumpOverEdges(sp, sq, jumpDist, heightRange, segs, NSEGS);
|
|
|
|
int ibest = -1;
|
|
float dbest = 0;
|
|
for (int i = 0; i < nsegs; ++i)
|
|
{
|
|
const dtReal* seg = &segs[i*6];
|
|
const float d = dtVdistSqr(seg,seg+3);
|
|
if (d > dbest)
|
|
{
|
|
dbest = d;
|
|
ibest = i;
|
|
}
|
|
}
|
|
if (ibest == -1)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
const float jumpStartDist = config.jumpDistanceFromGapCenter;
|
|
const float jumpHeight = config.jumpHeight;
|
|
const float groundRange = config.jumpEndsHeightTolerance;
|
|
samplingSeparationFactor = config.samplingSeparationFactor;
|
|
initJumpOverRig(es, &segs[ibest*6+0], &segs[ibest*6+3], -jumpStartDist, jumpStartDist, jumpHeight, groundRange);
|
|
}
|
|
|
|
initTrajectorySamples(es->groundRange, &es->trajectory);
|
|
|
|
// Init start end segments.
|
|
dtReal offset[3];
|
|
trans2d(offset, es->az, es->ay, &es->trajectory.spine[0]);
|
|
dtVadd(es->start.p, es->rigp, offset);
|
|
dtVadd(es->start.q, es->rigq, offset);
|
|
trans2d(offset, es->az, es->ay, &es->trajectory.spine[(es->trajectory.nspine-1)*2]);
|
|
dtVadd(es->end.p, es->rigp, offset);
|
|
dtVadd(es->end.q, es->rigq, offset);
|
|
|
|
// Sample start and end ground segments.
|
|
const float dist = sqrtf(dtVdistSqr(es->rigp, es->rigq));
|
|
|
|
const dtReal distBetweenSamples = samplingSeparationFactor*m_cs;
|
|
const int ngsamples = dtMax(2, (int)ceilf(dist/distBetweenSamples));
|
|
|
|
sampleGroundSegment(&es->start, ngsamples, es->groundRange);
|
|
sampleGroundSegment(&es->end, ngsamples, es->groundRange);
|
|
|
|
// Now that we have ground heights, update the trajectory samples.
|
|
updateTrajectorySamples(es);
|
|
|
|
sampleAction(es);
|
|
|
|
return true;
|
|
}
|
|
//@UE END
|