bool Detour::DistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, float* ed, float* et) { // TODO: Replace pnpoly with triArea2D tests? int i, j; bool c = false; for (i = 0, j = nverts - 1; i < nverts; j = i++) { const float* vi = &verts[i * 3]; const float* vj = &verts[j * 3]; if (((vi[2] > pt[2]) != (vj[2] > pt[2])) && (pt[0] < (vj[0] - vi[0]) * (pt[2] - vi[2]) / (vj[2] - vi[2]) + vi[0])) c = !c; ed[j] = dtDistancePtSegSqr2D(pt, vj, vi, et[j]); } return c; }
void dtObstacleAvoidanceQuery::prepare(const float* pos, const float* dvel) { // Prepare obstacles for (int i = 0; i < m_ncircles; ++i) { dtObstacleCircle* cir = &m_circles[i]; // Side const float* pa = pos; const float* pb = cir->p; const float orig[3] = {0,0,0}; float dv[3]; dtVsub(cir->dp,pb,pa); dtVnormalize(cir->dp); dtVsub(dv, cir->dvel, dvel); const float a = dtTriArea2D(orig, cir->dp,dv); if (a < 0.01f) { cir->np[0] = -cir->dp[2]; cir->np[2] = cir->dp[0]; } else { cir->np[0] = cir->dp[2]; cir->np[2] = -cir->dp[0]; } } for (int i = 0; i < m_nsegments; ++i) { dtObstacleSegment* seg = &m_segments[i]; // Precalc if the agent is really close to the segment. const float r = 0.01f; float t; seg->touch = dtDistancePtSegSqr2D(pos, seg->p, seg->q, t) < dtSqr(r); } }
void dtLocalBoundary::update(dtPolyRef ref, const float* pos, const float collisionQueryRange, dtNavMeshQuery* navquery, const dtQueryFilter* filter) { static const int MAX_SEGS_PER_POLY = DT_VERTS_PER_POLYGON*3; if (!ref) { dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX); m_nsegs = 0; m_npolys = 0; return; } dtVcopy(m_center, pos); // First query non-overlapping polygons. navquery->findLocalNeighbourhood(ref, pos, collisionQueryRange, filter, m_polys, 0, &m_npolys, MAX_LOCAL_POLYS); // Secondly, store all polygon edges. m_nsegs = 0; float segs[MAX_SEGS_PER_POLY*6]; int nsegs = 0; for (int j = 0; j < m_npolys; ++j) { navquery->getPolyWallSegments(m_polys[j], filter, segs, 0, &nsegs, MAX_SEGS_PER_POLY); for (int k = 0; k < nsegs; ++k) { const float* s = &segs[k*6]; // Skip too distant segments. float tseg; const float distSqr = dtDistancePtSegSqr2D(pos, s, s+3, tseg); if (distSqr > dtSqr(collisionQueryRange)) continue; addSegment(distSqr, s); } } }
void NavMeshTesterTool::handleRender() { DebugDrawGL dd; static const unsigned int startCol = duRGBA(128,25,0,192); static const unsigned int endCol = duRGBA(51,102,0,129); static const unsigned int pathCol = duRGBA(0,0,0,64); const float agentRadius = m_sample->getAgentRadius(); const float agentHeight = m_sample->getAgentHeight(); const float agentClimb = m_sample->getAgentClimb(); dd.depthMask(false); if (m_sposSet) drawAgent(m_spos, agentRadius, agentHeight, agentClimb, startCol); if (m_eposSet) drawAgent(m_epos, agentRadius, agentHeight, agentClimb, endCol); dd.depthMask(true); if (!m_navMesh) { return; } if (m_toolMode == TOOLMODE_PATHFIND_FOLLOW) { duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol); duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_endRef, endCol); if (m_npolys) { for (int i = 1; i < m_npolys-1; ++i) duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol); } if (m_nsmoothPath) { dd.depthMask(false); const unsigned int pathCol = duRGBA(0,0,0,220); dd.begin(DU_DRAW_LINES, 3.0f); for (int i = 0; i < m_nsmoothPath; ++i) dd.vertex(m_smoothPath[i*3], m_smoothPath[i*3+1]+0.1f, m_smoothPath[i*3+2], pathCol); dd.end(); dd.depthMask(true); } if (m_pathIterNum) { duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_pathIterPolys[0], duRGBA(255,255,255,128)); dd.depthMask(false); dd.begin(DU_DRAW_LINES, 1.0f); const unsigned int prevCol = duRGBA(255,192,0,220); const unsigned int curCol = duRGBA(255,255,255,220); const unsigned int steerCol = duRGBA(0,192,255,220); dd.vertex(m_prevIterPos[0],m_prevIterPos[1]-0.3f,m_prevIterPos[2], prevCol); dd.vertex(m_prevIterPos[0],m_prevIterPos[1]+0.3f,m_prevIterPos[2], prevCol); dd.vertex(m_iterPos[0],m_iterPos[1]-0.3f,m_iterPos[2], curCol); dd.vertex(m_iterPos[0],m_iterPos[1]+0.3f,m_iterPos[2], curCol); dd.vertex(m_prevIterPos[0],m_prevIterPos[1]+0.3f,m_prevIterPos[2], prevCol); dd.vertex(m_iterPos[0],m_iterPos[1]+0.3f,m_iterPos[2], prevCol); dd.vertex(m_prevIterPos[0],m_prevIterPos[1]+0.3f,m_prevIterPos[2], steerCol); dd.vertex(m_steerPos[0],m_steerPos[1]+0.3f,m_steerPos[2], steerCol); for (int i = 0; i < m_steerPointCount-1; ++i) { dd.vertex(m_steerPoints[i*3+0],m_steerPoints[i*3+1]+0.2f,m_steerPoints[i*3+2], duDarkenCol(steerCol)); dd.vertex(m_steerPoints[(i+1)*3+0],m_steerPoints[(i+1)*3+1]+0.2f,m_steerPoints[(i+1)*3+2], duDarkenCol(steerCol)); } dd.end(); dd.depthMask(true); } } else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT || m_toolMode == TOOLMODE_PATHFIND_SLICED) { duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol); duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_endRef, endCol); if (m_npolys) { for (int i = 1; i < m_npolys-1; ++i) duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol); } if (m_nstraightPath) { dd.depthMask(false); const unsigned int pathCol = duRGBA(64,16,0,220); const unsigned int offMeshCol = duRGBA(128,96,0,220); dd.begin(DU_DRAW_LINES, 2.0f); for (int i = 0; i < m_nstraightPath-1; ++i) { unsigned int col = 0; if (m_straightPathFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) col = offMeshCol; else col = pathCol; dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], col); dd.vertex(m_straightPath[(i+1)*3], m_straightPath[(i+1)*3+1]+0.4f, m_straightPath[(i+1)*3+2], col); } dd.end(); dd.begin(DU_DRAW_POINTS, 6.0f); for (int i = 0; i < m_nstraightPath; ++i) { unsigned int col = 0; if (m_straightPathFlags[i] & DT_STRAIGHTPATH_START) col = startCol; else if (m_straightPathFlags[i] & DT_STRAIGHTPATH_START) col = endCol; else if (m_straightPathFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) col = offMeshCol; else col = pathCol; dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], pathCol); } dd.end(); dd.depthMask(true); } } else if (m_toolMode == TOOLMODE_RAYCAST) { duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol); if (m_nstraightPath) { for (int i = 1; i < m_npolys; ++i) duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol); dd.depthMask(false); const unsigned int pathCol = m_hitResult ? duRGBA(64,16,0,220) : duRGBA(240,240,240,220); dd.begin(DU_DRAW_LINES, 2.0f); for (int i = 0; i < m_nstraightPath-1; ++i) { dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], pathCol); dd.vertex(m_straightPath[(i+1)*3], m_straightPath[(i+1)*3+1]+0.4f, m_straightPath[(i+1)*3+2], pathCol); } dd.end(); dd.begin(DU_DRAW_POINTS, 4.0f); for (int i = 0; i < m_nstraightPath; ++i) dd.vertex(m_straightPath[i*3], m_straightPath[i*3+1]+0.4f, m_straightPath[i*3+2], pathCol); dd.end(); if (m_hitResult) { const unsigned int hitCol = duRGBA(0,0,0,128); dd.begin(DU_DRAW_LINES, 2.0f); dd.vertex(m_hitPos[0], m_hitPos[1] + 0.4f, m_hitPos[2], hitCol); dd.vertex(m_hitPos[0] + m_hitNormal[0]*agentRadius, m_hitPos[1] + 0.4f + m_hitNormal[1]*agentRadius, m_hitPos[2] + m_hitNormal[2]*agentRadius, hitCol); dd.end(); } dd.depthMask(true); } } else if (m_toolMode == TOOLMODE_DISTANCE_TO_WALL) { duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_startRef, startCol); dd.depthMask(false); duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], m_distanceToWall, duRGBA(64,16,0,220), 2.0f); dd.begin(DU_DRAW_LINES, 3.0f); dd.vertex(m_hitPos[0], m_hitPos[1] + 0.02f, m_hitPos[2], duRGBA(0,0,0,192)); dd.vertex(m_hitPos[0], m_hitPos[1] + agentHeight, m_hitPos[2], duRGBA(0,0,0,192)); dd.end(); dd.depthMask(true); } else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_CIRCLE) { for (int i = 0; i < m_npolys; ++i) { duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol); dd.depthMask(false); if (m_parent[i]) { float p0[3], p1[3]; dd.depthMask(false); getPolyCenter(m_navMesh, m_parent[i], p0); getPolyCenter(m_navMesh, m_polys[i], p1); duDebugDrawArc(&dd, p0[0],p0[1],p0[2], p1[0],p1[1],p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0,0,0,128), 2.0f); dd.depthMask(true); } dd.depthMask(true); } if (m_sposSet && m_eposSet) { dd.depthMask(false); const float dx = m_epos[0] - m_spos[0]; const float dz = m_epos[2] - m_spos[2]; const float dist = sqrtf(dx*dx + dz*dz); duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], dist, duRGBA(64,16,0,220), 2.0f); dd.depthMask(true); } } else if (m_toolMode == TOOLMODE_FIND_POLYS_IN_SHAPE) { for (int i = 0; i < m_npolys; ++i) { duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol); dd.depthMask(false); if (m_parent[i]) { float p0[3], p1[3]; dd.depthMask(false); getPolyCenter(m_navMesh, m_parent[i], p0); getPolyCenter(m_navMesh, m_polys[i], p1); duDebugDrawArc(&dd, p0[0],p0[1],p0[2], p1[0],p1[1],p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0,0,0,128), 2.0f); dd.depthMask(true); } dd.depthMask(true); } if (m_sposSet && m_eposSet) { dd.depthMask(false); const unsigned int col = duRGBA(64,16,0,220); dd.begin(DU_DRAW_LINES, 2.0f); for (int i = 0, j = 3; i < 4; j=i++) { const float* p0 = &m_queryPoly[j*3]; const float* p1 = &m_queryPoly[i*3]; dd.vertex(p0, col); dd.vertex(p1, col); } dd.end(); dd.depthMask(true); } } else if (m_toolMode == TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD) { for (int i = 0; i < m_npolys; ++i) { duDebugDrawNavMeshPoly(&dd, *m_navMesh, m_polys[i], pathCol); dd.depthMask(false); if (m_parent[i]) { float p0[3], p1[3]; dd.depthMask(false); getPolyCenter(m_navMesh, m_parent[i], p0); getPolyCenter(m_navMesh, m_polys[i], p1); duDebugDrawArc(&dd, p0[0],p0[1],p0[2], p1[0],p1[1],p1[2], 0.25f, 0.0f, 0.4f, duRGBA(0,0,0,128), 2.0f); dd.depthMask(true); } static const int MAX_SEGS = DT_VERTS_PER_POLYGON*2; float segs[MAX_SEGS*6]; int nsegs = 0; m_navQuery->getPolyWallSegments(m_polys[i], &m_filter, segs, &nsegs, MAX_SEGS); dd.begin(DU_DRAW_LINES, 2.0f); for (int j = 0; j < nsegs; ++j) { const float* s = &segs[j*6]; // Skip too distant segments. float tseg; float distSqr = dtDistancePtSegSqr2D(m_spos, s, s+3, tseg); if (distSqr > dtSqr(m_neighbourhoodRadius)) continue; float delta[3], norm[3], p0[3], p1[3]; dtVsub(delta, s+3,s); dtVmad(p0, s, delta, 0.5f); norm[0] = delta[2]; norm[1] = 0; norm[2] = -delta[0]; dtVnormalize(norm); dtVmad(p1, p0, norm, agentRadius*0.5f); // Skip backfacing segments. unsigned int col = duRGBA(255,255,255,192); if (dtTriArea2D(m_spos, s, s+3) < 0.0f) col = duRGBA(255,255,255,64); dd.vertex(p0[0],p0[1]+agentClimb,p0[2],duRGBA(0,0,0,128)); dd.vertex(p1[0],p1[1]+agentClimb,p1[2],duRGBA(0,0,0,128)); dd.vertex(s[0],s[1]+agentClimb,s[2],col); dd.vertex(s[3],s[4]+agentClimb,s[5],col); } dd.end(); dd.depthMask(true); } if (m_sposSet) { dd.depthMask(false); duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], m_neighbourhoodRadius, duRGBA(64,16,0,220), 2.0f); dd.depthMask(true); } } }