632 lines
18 KiB
C++
632 lines
18 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
#include "DebugUtils/DetourNavLinkDebugDraw.h"
|
|
|
|
#include "DebugUtils/DebugDraw.h"
|
|
#include "DebugUtils/RecastDebugDraw.h"
|
|
#include "Detour/DetourCommon.h"
|
|
#include "Detour/DetourNavLinkBuilder.h"
|
|
#include "Recast/Recast.h"
|
|
|
|
//@UE BEGIN
|
|
namespace UE::Detour::Private
|
|
{
|
|
dtReal getPathLen(const dtReal* path, int npath)
|
|
{
|
|
if (npath < 2)
|
|
return 0;
|
|
|
|
dtReal totd = 0;
|
|
for (int i = 0; i < npath-1; ++i)
|
|
{
|
|
const dtReal* sp = &path[i*3];
|
|
const dtReal* sq = &path[(i+1)*3];
|
|
totd += sqrtf(dtVdistSqr(sp,sq));
|
|
}
|
|
|
|
return totd;
|
|
}
|
|
|
|
void getPointAlongPath(dtReal dist, const dtReal* path, int npath, dtReal* pt, dtReal* dir)
|
|
{
|
|
if (npath == 0)
|
|
return;
|
|
|
|
if (npath == 1)
|
|
{
|
|
if (dir)
|
|
dtVset(dir,1,0,0);
|
|
dtVcopy(pt, path);
|
|
return;
|
|
}
|
|
|
|
if (dist <= 0)
|
|
{
|
|
if (dir)
|
|
dtVsub(dir, &path[1*3], &path[0*3]);
|
|
|
|
dtVcopy(pt, path);
|
|
return;
|
|
}
|
|
|
|
float totd = 0;
|
|
for (int i = 0; i < npath-1; ++i)
|
|
{
|
|
const dtReal* sp = &path[i*3];
|
|
const dtReal* sq = &path[(i+1)*3];
|
|
const float sd = sqrtf(dtVdistSqr(sp,sq));
|
|
if (dist >= totd && dist <= totd+sd)
|
|
{
|
|
dtVlerp(pt, sp,sq,(dist-totd)/sd);
|
|
if (dir)
|
|
dtVsub(dir, sq,sp);
|
|
return;
|
|
}
|
|
totd += sd;
|
|
}
|
|
|
|
dtVcopy(pt, &path[(npath-1)*3]);
|
|
if (dir)
|
|
{
|
|
dtVsub(dir, &path[(npath-1)*3], &path[(npath-2)*3]);
|
|
}
|
|
}
|
|
|
|
void drawTrajectory(duDebugDraw* dd, const dtReal* pa, const dtReal* pb, const dtNavLinkBuilder::Trajectory2D* trajectory, const unsigned int color)
|
|
{
|
|
const float startx = trajectory->spine[0];
|
|
const float endx = trajectory->spine[(trajectory->nspine-1)*2];
|
|
const float deltax = endx - startx;
|
|
|
|
const float starty = trajectory->spine[1];
|
|
const float endy = trajectory->spine[(trajectory->nspine-1)*2+1];
|
|
|
|
dtReal pts[3*dtNavLinkBuilder::MAX_SPINE];
|
|
int npts = trajectory->nspine;
|
|
for (int i = 0; i < trajectory->nspine; ++i)
|
|
{
|
|
const float* spt = &trajectory->spine[i*2];
|
|
const float u = (spt[0] - startx)/deltax;
|
|
const float dy = spt[1] - dtLerp(starty, endy, u);
|
|
dtReal* p = &pts[i*3];
|
|
dtVlerp(p, pa, pb, u);
|
|
p[1] += dy;
|
|
}
|
|
|
|
const float len = getPathLen(pts, npts);
|
|
const int nsegs = ceilf(len / 0.3f);
|
|
|
|
for (int i = 0; i < nsegs*2; ++i)
|
|
{
|
|
float u = (float)i / (float)(nsegs*2);
|
|
dtReal pt[3];
|
|
getPointAlongPath(u*len, pts, npts, pt, nullptr);
|
|
dd->vertex(pt, color);
|
|
}
|
|
}
|
|
|
|
void drawTrajectorySlice(duDebugDraw* dd, const dtReal* pa, const dtReal* pb, const dtNavLinkBuilder::Trajectory2D* trajectory, const dtReal* trajectoryDir, const unsigned int color)
|
|
{
|
|
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);
|
|
|
|
unsigned int colt = duTransCol(color, 50);
|
|
unsigned int colb = duTransCol(duLerpCol(color,duColor::black,96), 50);
|
|
|
|
dd->begin(DU_DRAW_QUADS);
|
|
dtReal p0[3], p1[3], p2[3], p3[3];
|
|
const int nsamples = trajectory->samples.Num();
|
|
for (int i = 0; i < nsamples; ++i)
|
|
{
|
|
const dtNavLinkBuilder::TrajectorySample& s = trajectory->samples[i];
|
|
const dtReal u = (float)i / (float)(nsamples-1);
|
|
dtVlerp(p1, start, end, u);
|
|
dtVcopy(p0, p1);
|
|
p0[1] += s.ymin;
|
|
p1[1] += s.ymax;
|
|
|
|
if (i > 0)
|
|
{
|
|
dd->vertex(p0, colb);
|
|
dd->vertex(p1, colt);
|
|
dd->vertex(p3, colt);
|
|
dd->vertex(p2, colb);
|
|
|
|
dd->vertex(p2, colb);
|
|
dd->vertex(p3, colt);
|
|
dd->vertex(p1, colt);
|
|
dd->vertex(p0, colb);
|
|
}
|
|
|
|
dtVcopy(p2, p0);
|
|
dtVcopy(p3, p1);
|
|
}
|
|
dd->end();
|
|
|
|
colb = duDarkenCol(colb);
|
|
colt = duDarkenCol(colt);
|
|
|
|
dd->begin(DU_DRAW_LINES, 2.0f);
|
|
for (int i = 0; i < nsamples; ++i)
|
|
{
|
|
const dtNavLinkBuilder::TrajectorySample& s = trajectory->samples[i];
|
|
const float u = (float)i / (float)(nsamples-1);
|
|
dtVlerp(p1, start, end, u);
|
|
dtVcopy(p0, p1);
|
|
p0[1] += s.ymin;
|
|
p1[1] += s.ymax;
|
|
|
|
if (i == 0 || i == (nsamples-1))
|
|
{
|
|
dd->vertex(p0, colb);
|
|
dd->vertex(p1, colt);
|
|
}
|
|
if (i > 0)
|
|
{
|
|
dd->vertex(p0, colb);
|
|
dd->vertex(p2, colb);
|
|
dd->vertex(p1, colt);
|
|
dd->vertex(p3, colt);
|
|
}
|
|
|
|
dtVcopy(p2, p0);
|
|
dtVcopy(p3, p1);
|
|
}
|
|
dd->end();
|
|
}
|
|
} // UE::Detour::Private
|
|
|
|
void duDebugDrawTrajectorySamples(duDebugDraw* dd, const dtNavLinkBuilder& linkBuilder, const dtReal* pa, const dtReal* pb,
|
|
const dtNavLinkBuilder::Trajectory2D* trajectory, const dtReal* trajectoryDir)
|
|
{
|
|
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 dtNavLinkBuilder::TrajectorySample& s = trajectory->samples[i];
|
|
const float u = (float)i * invLastSample;
|
|
dtVlerp(p, start, end, u);
|
|
|
|
// Draw an additional point if the sample was marked to be snap to the floor.
|
|
if (s.floorStart)
|
|
{
|
|
dd->begin(DU_DRAW_POINTS, 5.0f);
|
|
dd->vertex(p[0], p[1]+s.ymin, p[2], duColor::black);
|
|
dd->end();
|
|
}
|
|
else if (s.floorEnd)
|
|
{
|
|
dd->begin(DU_DRAW_POINTS, 5.0f);
|
|
dd->vertex(p[0], p[1]+s.ymin, p[2], duColor::cyan);
|
|
dd->end();
|
|
}
|
|
|
|
// Check with the heightfield if a collision is hit.
|
|
const bool hit = linkBuilder.checkHeightfieldCollision(p[0], p[1] + s.ymin, p[1] + s.ymax, p[2]);
|
|
unsigned int color = hit ? duColor::orangeRed : duColor::lightGreen;
|
|
|
|
// Draw a vertical line for the sample.
|
|
dd->begin(DU_DRAW_LINES, 5.0f);
|
|
dd->vertex(p[0], p[1]+s.ymin, p[2], color);
|
|
dd->vertex(p[0], p[1]+s.ymax, p[2], color);
|
|
dd->end();
|
|
}
|
|
}
|
|
|
|
void duDebugDrawNavLinkBuilder(duDebugDraw* dd, const dtNavLinkBuilder& linkBuilder, unsigned int drawFlags, const dtNavLinkBuilder::EdgeSampler* es)
|
|
{
|
|
if (dd == nullptr)
|
|
return;
|
|
|
|
if (drawFlags & DRAW_WALKABLE_SURFACE)
|
|
{
|
|
if (linkBuilder.m_chf)
|
|
{
|
|
duDebugDrawCompactHeightfieldSolid(dd, *linkBuilder.m_chf);
|
|
}
|
|
}
|
|
|
|
if (drawFlags & DRAW_BORDERS)
|
|
{
|
|
const TArray<dtNavLinkBuilder::Edge, TInlineAllocator<32>>& edges = linkBuilder.m_edges;
|
|
const int nedges = linkBuilder.m_edges.Num();
|
|
const int selectedEdge = linkBuilder.m_debugSelectedEdge;
|
|
|
|
if (nedges)
|
|
{
|
|
dd->begin(DU_DRAW_LINES, 3.0f);
|
|
for (int i = 0; i < nedges; ++i)
|
|
{
|
|
// Display edge index
|
|
dtReal x = 0.5*(edges[i].sp[0]+edges[i].sq[0]);
|
|
dtReal y = 0.5*(edges[i].sp[1]+edges[i].sq[1]);
|
|
dtReal z = 0.5*(edges[i].sp[2]+edges[i].sq[2]);
|
|
constexpr int bufferSize = 32;
|
|
char buffer[bufferSize];
|
|
snprintf(buffer, bufferSize, "%i", i);
|
|
dd->text(x, y, z, buffer);
|
|
|
|
unsigned int col = duRGBA(0,96,128,255);
|
|
if (i == selectedEdge)
|
|
continue;
|
|
|
|
dd->vertex(edges[i].sp, col);
|
|
dd->vertex(edges[i].sq, col);
|
|
}
|
|
dd->end();
|
|
|
|
dd->begin(DU_DRAW_POINTS, 8.0f);
|
|
for (int i = 0; i < nedges; ++i)
|
|
{
|
|
unsigned int col = duRGBA(0,96,128,255);
|
|
if (i == selectedEdge)
|
|
continue;
|
|
|
|
dd->vertex(edges[i].sp, col);
|
|
dd->vertex(edges[i].sq, col);
|
|
}
|
|
dd->end();
|
|
|
|
if (selectedEdge >= 0 && selectedEdge < nedges)
|
|
{
|
|
unsigned int col = duRGBA(68,36,36,255);
|
|
dd->begin(DU_DRAW_LINES, 3.0f);
|
|
dd->vertex(edges[selectedEdge].sp, col);
|
|
dd->vertex(edges[selectedEdge].sq, col);
|
|
dd->end();
|
|
dd->begin(DU_DRAW_POINTS, 8.0f);
|
|
dd->vertex(edges[selectedEdge].sp, col);
|
|
dd->vertex(edges[selectedEdge].sq, col);
|
|
dd->end();
|
|
}
|
|
|
|
dd->begin(DU_DRAW_POINTS, 4.0f);
|
|
for (int i = 0; i < nedges; ++i)
|
|
{
|
|
unsigned int col = duColor::lightGrey;
|
|
dd->vertex(edges[i].sp, col);
|
|
dd->vertex(edges[i].sq, col);
|
|
}
|
|
dd->end();
|
|
}
|
|
}
|
|
|
|
if (drawFlags & DRAW_LINKS)
|
|
{
|
|
const bool drawFilteredLinks = drawFlags & DRAW_FILTERED_LINKS;
|
|
unsigned int jumpDownCol0 = duLerpCol(duColor::blue, duColor::white, 200);
|
|
unsigned int jumpDownCol1 = duColor::blue;
|
|
unsigned int jumpOverCol0 = duLerpCol(duColor::lightGrey, duColor::white, 200);
|
|
unsigned int jumpOverCol1 = duColor::lightGrey;
|
|
|
|
auto selectColors = [&](dtNavLinkBuilder::JumpLinkFlag flag, dtNavLinkAction action, unsigned int& col0, unsigned int& col1)
|
|
{
|
|
if (action == DT_LINK_ACTION_JUMP_DOWN)
|
|
{
|
|
col0 = jumpDownCol0;
|
|
col1 = jumpDownCol1;
|
|
}
|
|
else if (action == DT_LINK_ACTION_JUMP_OVER)
|
|
{
|
|
col0 = jumpOverCol0;
|
|
col1 = jumpOverCol1;
|
|
}
|
|
|
|
if (flag == dtNavLinkBuilder::FILTERED)
|
|
{
|
|
col0 = duColor::grey;
|
|
col1 = duColor::darkGrey;
|
|
}
|
|
};
|
|
|
|
unsigned int col0 = duColor::black;
|
|
unsigned int col1 = duColor::black;
|
|
|
|
const int nlinks = linkBuilder.m_links.Num();
|
|
if (nlinks)
|
|
{
|
|
dd->begin(DU_DRAW_QUADS);
|
|
for (const dtNavLinkBuilder::JumpLink& link : linkBuilder.m_links)
|
|
{
|
|
if (!drawFilteredLinks && link.flags == dtNavLinkBuilder::FILTERED)
|
|
continue;
|
|
|
|
selectColors(link.flags, link.action, col0, col1);
|
|
|
|
for (int j = 0; j < link.nspine-1; ++j)
|
|
{
|
|
int u = (j*255)/link.nspine;
|
|
unsigned int col = duTransCol(duLerpCol(col0,col1,u),128);
|
|
|
|
dd->vertex(&link.spine1[j*3], col);
|
|
dd->vertex(&link.spine1[(j+1)*3], col);
|
|
dd->vertex(&link.spine0[(j+1)*3], col);
|
|
dd->vertex(&link.spine0[j*3], col);
|
|
}
|
|
}
|
|
dd->end();
|
|
|
|
dd->begin(DU_DRAW_LINES, 3.0f);
|
|
for (const dtNavLinkBuilder::JumpLink& link : linkBuilder.m_links)
|
|
{
|
|
if (!drawFilteredLinks && link.flags == dtNavLinkBuilder::FILTERED)
|
|
continue;
|
|
|
|
selectColors(link.flags, link.action, col0, col1);
|
|
|
|
for (int j = 0; j < link.nspine-1; ++j)
|
|
{
|
|
unsigned int col = duTransCol(duDarkenCol(col1),128);
|
|
|
|
dd->vertex(&link.spine0[j*3], col);
|
|
dd->vertex(&link.spine0[(j+1)*3], col);
|
|
dd->vertex(&link.spine1[j*3], col);
|
|
dd->vertex(&link.spine1[(j+1)*3], col);
|
|
}
|
|
|
|
dd->vertex(&link.spine0[0], duDarkenCol(col1));
|
|
dd->vertex(&link.spine1[0], duDarkenCol(col1));
|
|
|
|
dd->vertex(&link.spine0[(link.nspine-1)*3], duDarkenCol(col1));
|
|
dd->vertex(&link.spine1[(link.nspine-1)*3], duDarkenCol(col1));
|
|
}
|
|
dd->end();
|
|
|
|
dd->begin(DU_DRAW_POINTS, 8.0f);
|
|
for (const dtNavLinkBuilder::JumpLink& link : linkBuilder.m_links)
|
|
{
|
|
if (link.flags == dtNavLinkBuilder::FILTERED)
|
|
continue;
|
|
|
|
selectColors(link.flags, link.action, col0, col1);
|
|
|
|
dd->vertex(&link.spine0[0], duDarkenCol(col1));
|
|
dd->vertex(&link.spine1[0], duDarkenCol(col1));
|
|
dd->vertex(&link.spine0[(link.nspine-1)*3], duDarkenCol(col1));
|
|
dd->vertex(&link.spine1[(link.nspine-1)*3], duDarkenCol(col1));
|
|
}
|
|
dd->end();
|
|
|
|
dd->begin(DU_DRAW_POINTS, 4.0f);
|
|
for (const dtNavLinkBuilder::JumpLink& link : linkBuilder.m_links)
|
|
{
|
|
if (link.flags == dtNavLinkBuilder::FILTERED)
|
|
continue;
|
|
|
|
dd->vertex(&link.spine0[0], duColor::lightGrey);
|
|
dd->vertex(&link.spine1[0], duColor::lightGrey);
|
|
dd->vertex(&link.spine0[(link.nspine-1)*3], duColor::lightGrey);
|
|
dd->vertex(&link.spine1[(link.nspine-1)*3], duColor::lightGrey);
|
|
}
|
|
dd->end();
|
|
}
|
|
}
|
|
|
|
if (drawFlags & DRAW_SELECTED_EDGE)
|
|
{
|
|
if (es && es->action != DT_LINK_ACTION_UNSET)
|
|
{
|
|
dd->begin(DU_DRAW_LINES, 2.0f);
|
|
|
|
// Axis
|
|
constexpr float d = 20.f;
|
|
dd->vertex(es->rigp, duColor::red);
|
|
dd->vertex(es->rigp[0]+es->ax[0]*d, es->rigp[1]+es->ax[1]*d, es->rigp[2]+es->ax[2]*d, duColor::red);
|
|
|
|
dd->vertex(es->rigp, duColor::green);
|
|
dd->vertex(es->rigp[0]+es->ay[0]*d, es->rigp[1]+es->ay[1]*d, es->rigp[2]+es->ay[2]*d, duColor::green);
|
|
|
|
dd->vertex(es->rigp, duColor::blue);
|
|
dd->vertex(es->rigp[0]+es->az[0]*d, es->rigp[1]+es->az[1]*d, es->rigp[2]+es->az[2]*d, duColor::blue);
|
|
|
|
dd->end();
|
|
|
|
if (drawFlags & DRAW_TRAJECTORY)
|
|
{
|
|
const float r = es->groundRange;
|
|
|
|
unsigned int col = duLerpCol(duRGBA(255,192,0,255), duColor::white, 64);
|
|
unsigned int cola = duTransCol(col,192);
|
|
unsigned int colb = duColor::white;
|
|
|
|
// Start segment.
|
|
dd->begin(DU_DRAW_LINES, 3.0f);
|
|
dd->vertex(es->start.p[0],es->start.p[1],es->start.p[2], col);
|
|
dd->vertex(es->start.q[0],es->start.q[1],es->start.q[2], col);
|
|
dd->end();
|
|
|
|
dd->begin(DU_DRAW_LINES, 1.0f);
|
|
dd->vertex(es->start.p[0],es->start.p[1]-r,es->start.p[2], colb);
|
|
dd->vertex(es->start.p[0],es->start.p[1]+r,es->start.p[2], colb);
|
|
dd->vertex(es->start.p[0],es->start.p[1]+r,es->start.p[2], colb);
|
|
dd->vertex(es->start.q[0],es->start.q[1]+r,es->start.q[2], colb);
|
|
dd->vertex(es->start.q[0],es->start.q[1]+r,es->start.q[2], colb);
|
|
dd->vertex(es->start.q[0],es->start.q[1]-r,es->start.q[2], colb);
|
|
dd->vertex(es->start.q[0],es->start.q[1]-r,es->start.q[2], colb);
|
|
dd->vertex(es->start.p[0],es->start.p[1]-r,es->start.p[2], colb);
|
|
dd->end();
|
|
|
|
// End segment.
|
|
dd->begin(DU_DRAW_LINES, 3.0f);
|
|
dd->vertex(es->end.p[0],es->end.p[1],es->end.p[2], col);
|
|
dd->vertex(es->end.q[0],es->end.q[1],es->end.q[2], col);
|
|
dd->end();
|
|
|
|
dd->begin(DU_DRAW_LINES, 1.0f);
|
|
dd->vertex(es->end.p[0],es->end.p[1]-r,es->end.p[2], colb);
|
|
dd->vertex(es->end.p[0],es->end.p[1]+r,es->end.p[2], colb);
|
|
dd->vertex(es->end.p[0],es->end.p[1]+r,es->end.p[2], colb);
|
|
dd->vertex(es->end.q[0],es->end.q[1]+r,es->end.q[2], colb);
|
|
dd->vertex(es->end.q[0],es->end.q[1]+r,es->end.q[2], colb);
|
|
dd->vertex(es->end.q[0],es->end.q[1]-r,es->end.q[2], colb);
|
|
dd->vertex(es->end.q[0],es->end.q[1]-r,es->end.q[2], colb);
|
|
dd->vertex(es->end.p[0],es->end.p[1]-r,es->end.p[2], colb);
|
|
dd->end();
|
|
|
|
dd->begin(DU_DRAW_LINES, 4.0f);
|
|
UE::Detour::Private::drawTrajectory(dd, es->start.p, es->end.p, &es->trajectory, cola);
|
|
UE::Detour::Private::drawTrajectory(dd, es->start.q, es->end.q, &es->trajectory, cola);
|
|
dd->end();
|
|
|
|
|
|
dd->begin(DU_DRAW_POINTS, 8.0f);
|
|
dd->vertex(es->start.p[0],es->start.p[1],es->start.p[2], duDarkenCol(col));
|
|
dd->vertex(es->start.q[0],es->start.q[1],es->start.q[2], duDarkenCol(col));
|
|
dd->vertex(es->end.p[0],es->end.p[1],es->end.p[2], duDarkenCol(col));
|
|
dd->vertex(es->end.q[0],es->end.q[1],es->end.q[2], duDarkenCol(col));
|
|
dd->end();
|
|
|
|
unsigned int colm = duColor::white;
|
|
dd->begin(DU_DRAW_POINTS, 3.0f);
|
|
dd->vertex(es->start.p[0],es->start.p[1],es->start.p[2], colm);
|
|
dd->vertex(es->start.q[0],es->start.q[1],es->start.q[2], colm);
|
|
dd->vertex(es->end.p[0],es->end.p[1],es->end.p[2], colm);
|
|
dd->vertex(es->end.q[0],es->end.q[1],es->end.q[2], colm);
|
|
dd->end();
|
|
}
|
|
|
|
if (drawFlags & DRAW_LAND_SAMPLES)
|
|
{
|
|
if (es->start.ngsamples)
|
|
{
|
|
dd->begin(DU_DRAW_POINTS, 8.0f);
|
|
for (int i = 0; i < es->start.ngsamples; ++i)
|
|
{
|
|
const dtNavLinkBuilder::GroundSample* s = &es->start.gsamples[i];
|
|
const float u = (float)i/(float)(es->start.ngsamples-1);
|
|
dtReal spt[3], ept[3];
|
|
dtVlerp(spt, es->start.p, es->start.q, u);
|
|
dtVlerp(ept, es->start.p, es->start.q, u);
|
|
|
|
unsigned int col = duColor::darkGrey;
|
|
dtReal off = 1;
|
|
if ((s->flags & dtNavLinkBuilder::HAS_GROUND) == 0)
|
|
{
|
|
off = 0;
|
|
col = duColor::orangeRed;
|
|
}
|
|
|
|
spt[1] = s->height + off;
|
|
dd->vertex(spt, col);
|
|
}
|
|
dd->end();
|
|
|
|
dd->begin(DU_DRAW_POINTS, 4.0f);
|
|
for (int i = 0; i < es->start.ngsamples; ++i)
|
|
{
|
|
const dtNavLinkBuilder::GroundSample* s = &es->start.gsamples[i];
|
|
const float u = (float)i/(float)(es->start.ngsamples-1);
|
|
dtReal spt[3], ept[3];
|
|
dtVlerp(spt, es->start.p, es->start.q,u);
|
|
dtVlerp(ept, es->start.p, es->start.q,u);
|
|
float off = 0;
|
|
if (s->flags & dtNavLinkBuilder::HAS_GROUND)
|
|
{
|
|
off = 1.f;
|
|
}
|
|
spt[1] = s->height + off;
|
|
dd->vertex(spt, duColor::green);
|
|
}
|
|
dd->end();
|
|
|
|
}
|
|
|
|
if (es->end.ngsamples)
|
|
{
|
|
dd->begin(DU_DRAW_POINTS, 8.0f);
|
|
for (int i = 0; i < es->end.ngsamples; ++i)
|
|
{
|
|
const dtNavLinkBuilder::GroundSample* s = &es->end.gsamples[i];
|
|
const float u = (float)i/(float)(es->end.ngsamples-1);
|
|
dtReal spt[3], ept[3];
|
|
dtVlerp(spt, es->end.p, es->end.q, u);
|
|
dtVlerp(ept, es->end.p, es->end.q, u);
|
|
|
|
unsigned int col = duColor::darkGrey;
|
|
float off = 1.f;
|
|
if ((s->flags & dtNavLinkBuilder::HAS_GROUND) == 0)
|
|
{
|
|
off = 0;
|
|
col = duColor::orangeRed;
|
|
}
|
|
|
|
spt[1] = s->height + off;
|
|
dd->vertex(spt, col);
|
|
}
|
|
dd->end();
|
|
|
|
dd->begin(DU_DRAW_POINTS, 4.0f);
|
|
for (int i = 0; i < es->end.ngsamples; ++i)
|
|
{
|
|
const dtNavLinkBuilder::GroundSample* s = &es->end.gsamples[i];
|
|
const float u = (float)i/(float)(es->end.ngsamples-1);
|
|
dtReal spt[3], ept[3];
|
|
dtVlerp(spt, es->end.p, es->end.q, u);
|
|
dtVlerp(ept, es->end.p, es->end.q, u);
|
|
|
|
float off = 0;
|
|
if (s->flags & dtNavLinkBuilder::HAS_GROUND)
|
|
{
|
|
off = 1.f;
|
|
}
|
|
spt[1] = s->height + off;
|
|
dd->vertex(spt, duColor::green);
|
|
}
|
|
dd->end();
|
|
}
|
|
}
|
|
|
|
const bool drawCollisionSlices = drawFlags & DRAW_COLLISION_SLICES;
|
|
const bool drawCollisionSamples = drawFlags & DRAW_COLLISION_SAMPLES;
|
|
if (drawCollisionSlices || drawCollisionSamples)
|
|
{
|
|
if (es->start.ngsamples && es->start.ngsamples == es->end.ngsamples)
|
|
{
|
|
const int nsamples = es->start.ngsamples;
|
|
for (int i = 0; i < nsamples; ++i)
|
|
{
|
|
const dtNavLinkBuilder::GroundSample* ssmp = &es->start.gsamples[i];
|
|
const dtNavLinkBuilder::GroundSample* esmp = &es->end.gsamples[i];
|
|
if ((ssmp->flags & dtNavLinkBuilder::HAS_GROUND) == 0 || (esmp->flags & dtNavLinkBuilder::HAS_GROUND) == 0)
|
|
continue;
|
|
|
|
const float u = (float)i/(float)(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 (drawCollisionSlices)
|
|
{
|
|
if (ssmp->flags & dtNavLinkBuilder::UNRESTRICTED)
|
|
UE::Detour::Private::drawTrajectorySlice(dd, spt, ept, &es->trajectory, es->az, duColor::green);
|
|
else
|
|
UE::Detour::Private::drawTrajectorySlice(dd, spt, ept, &es->trajectory, es->az, duColor::orangeRed);
|
|
}
|
|
|
|
if (drawCollisionSamples)
|
|
{
|
|
duDebugDrawTrajectorySamples(dd, linkBuilder, spt, ept, &es->trajectory, es->az);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//@UE END
|