void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) { if (!tile) return; dtPolyRef base = getPolyRefBase(tile); // Base off-mesh connection start points. for (int i = 0; i < tile->header->offMeshConCount; ++i) { dtOffMeshConnection* con = &tile->offMeshCons[i]; dtPoly* poly = &tile->polys[con->poly]; const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad }; // Find polygon to connect to. const float* p = &con->pos[0]; // First vertex float nearestPt[3]; dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); if (!ref) continue; // findNearestPoly may return too optimistic results, further check to make sure. if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) continue; // Make sure the location is on current mesh. float* v = &tile->verts[poly->verts[0]*3]; dtVcopy(v, nearestPt); // Link off-mesh connection to target poly. unsigned int idx = allocLink(tile); if (idx != DT_NULL_LINK) { dtLink* link = &tile->links[idx]; link->ref = ref; link->edge = (unsigned char)0; link->side = 0xff; link->bmin = link->bmax = 0; // Add to linked list. link->next = poly->firstLink; poly->firstLink = idx; } // Start end-point is always connect back to off-mesh connection. unsigned int tidx = allocLink(tile); if (tidx != DT_NULL_LINK) { const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); dtPoly* landPoly = &tile->polys[landPolyIdx]; dtLink* link = &tile->links[tidx]; link->ref = base | (dtPolyRef)(con->poly); link->edge = 0xff; link->side = 0xff; link->bmin = link->bmax = 0; // Add to linked list. link->next = landPoly->firstLink; landPoly->firstLink = tidx; } } }
int dtNavMesh::findConnectingPolys(const float* va, const float* vb, const dtMeshTile* tile, int side, dtPolyRef* con, float* conarea, int maxcon) const { if (!tile) return 0; float amin[2], amax[2]; calcSlabEndPoints(va,vb, amin,amax, side); const float apos = getSlabCoord(va, side); // Remove links pointing to 'side' and compact the links array. float bmin[2], bmax[2]; unsigned short m = DT_EXT_LINK | (unsigned short)side; int n = 0; dtPolyRef base = getPolyRefBase(tile); for (int i = 0; i < tile->header->polyCount; ++i) { dtPoly* poly = &tile->polys[i]; const int nv = poly->vertCount; for (int j = 0; j < nv; ++j) { if (!poly->neis[j]) return 0; // Skip edges which do not point to the right side. if (poly->neis[j] != m) continue; const float* vc = &tile->verts[poly->verts[j]*3]; const float* vd = &tile->verts[poly->verts[(j+1) % nv]*3]; const float bpos = getSlabCoord(vc, side); // Segments are not close enough. if (dtAbs(apos-bpos) > 0.01f) continue; // Check if the segments touch. calcSlabEndPoints(vc,vd, bmin,bmax, side); if (!overlapSlabs(amin,amax, bmin,bmax, 0.01f, tile->header->walkableClimb)) continue; // Add return value. if (n < maxcon) { conarea[n*2+0] = dtMax(amin[0], bmin[0]); conarea[n*2+1] = dtMin(amax[0], bmax[0]); con[n] = base | (dtPolyRef)i; n++; } break; } } return n; }
void dtNavMesh::connectIntLinks(dtMeshTile* tile) { if (!tile) return; dtPolyRef base = getPolyRefBase(tile); for (int i = 0; i < tile->header->polyCount; ++i) { dtPoly* poly = &tile->polys[i]; poly->firstLink = DT_NULL_LINK; if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) continue; // Build edge links backwards so that the links will be // in the linked list from lowest index to highest. for (int j = poly->vertCount-1; j >= 0; --j) { // Skip hard and non-internal edges. if (poly->neis[j] == 0 || (poly->neis[j] & DT_EXT_LINK)) continue; unsigned int idx = allocLink(tile); if (idx != DT_NULL_LINK) { dtLink* link = &tile->links[idx]; link->ref = base | (dtPolyRef)(poly->neis[j]-1); link->edge = (unsigned char)j; link->side = 0xff; link->bmin = link->bmax = 0; // Add to linked list. link->next = poly->firstLink; poly->firstLink = idx; } } } }
int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, dtPolyRef* polys, const int maxPolys) const { if (tile->bvTree) { const dtBVNode* node = &tile->bvTree[0]; const dtBVNode* end = &tile->bvTree[tile->header->bvNodeCount]; const float* tbmin = tile->header->bmin; const float* tbmax = tile->header->bmax; const float qfac = tile->header->bvQuantFactor; // Calculate quantized box unsigned short bmin[3], bmax[3]; // dtClamp query box to world box. float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0]; float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1]; float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2]; float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0]; float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1]; float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2]; // Quantize bmin[0] = (unsigned short)(qfac * minx) & 0xfffe; bmin[1] = (unsigned short)(qfac * miny) & 0xfffe; bmin[2] = (unsigned short)(qfac * minz) & 0xfffe; bmax[0] = (unsigned short)(qfac * maxx + 1) | 1; bmax[1] = (unsigned short)(qfac * maxy + 1) | 1; bmax[2] = (unsigned short)(qfac * maxz + 1) | 1; // Traverse tree dtPolyRef base = getPolyRefBase(tile); int n = 0; while (node < end) { const bool overlap = dtOverlapQuantBounds(bmin, bmax, node->bmin, node->bmax); const bool isLeafNode = node->i >= 0; if (isLeafNode && overlap) { if (n < maxPolys) polys[n++] = base | (dtPolyRef)node->i; } if (overlap || isLeafNode) node++; else { const int escapeIndex = -node->i; node += escapeIndex; } } return n; } else { float bmin[3], bmax[3]; int n = 0; dtPolyRef base = getPolyRefBase(tile); for (int i = 0; i < tile->header->polyCount; ++i) { dtPoly* p = &tile->polys[i]; // Do not return off-mesh connection polygons. if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) continue; // Calc polygon bounds. const float* v = &tile->verts[p->verts[0]*3]; dtVcopy(bmin, v); dtVcopy(bmax, v); for (int j = 1; j < p->vertCount; ++j) { v = &tile->verts[p->verts[j]*3]; dtVmin(bmin, v); dtVmax(bmax, v); } if (dtOverlapBounds(qmin,qmax, bmin,bmax)) { if (n < maxPolys) polys[n++] = base | (dtPolyRef)i; } } return n; } }
void dtNavMesh::connectExtOffMeshLinks(dtMeshTile* tile, dtMeshTile* target, int side) { if (!tile) return; // Connect off-mesh links. // We are interested on links which land from target tile to this tile. const unsigned char oppositeSide = (side == -1) ? 0xff : (unsigned char)dtOppositeTile(side); for (int i = 0; i < target->header->offMeshConCount; ++i) { dtOffMeshConnection* targetCon = &target->offMeshCons[i]; if (targetCon->side != oppositeSide) continue; dtPoly* targetPoly = &target->polys[targetCon->poly]; // Skip off-mesh connections which start location could not be connected at all. if (targetPoly->firstLink == DT_NULL_LINK) continue; const float ext[3] = { targetCon->rad, target->header->walkableClimb, targetCon->rad }; // Find polygon to connect to. const float* p = &targetCon->pos[3]; float nearestPt[3]; dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); if (!ref) continue; // findNearestPoly may return too optimistic results, further check to make sure. if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(targetCon->rad)) continue; // Make sure the location is on current mesh. float* v = &target->verts[targetPoly->verts[1]*3]; dtVcopy(v, nearestPt); // Link off-mesh connection to target poly. unsigned int idx = allocLink(target); if (idx != DT_NULL_LINK) { dtLink* link = &target->links[idx]; link->ref = ref; link->edge = (unsigned char)1; link->side = oppositeSide; link->bmin = link->bmax = 0; // Add to linked list. link->next = targetPoly->firstLink; targetPoly->firstLink = idx; } // Link target poly to off-mesh connection. if (targetCon->flags & DT_OFFMESH_CON_BIDIR) { unsigned int tidx = allocLink(tile); if (tidx != DT_NULL_LINK) { const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); dtPoly* landPoly = &tile->polys[landPolyIdx]; dtLink* link = &tile->links[tidx]; link->ref = getPolyRefBase(target) | (dtPolyRef)(targetCon->poly); link->edge = 0xff; link->side = (unsigned char)(side == -1 ? 0xff : side); link->bmin = link->bmax = 0; // Add to linked list. link->next = landPoly->firstLink; landPoly->firstLink = tidx; } } } }
void dtNavMesh::connectIntOffMeshLinks(dtMeshTile* tile) { if (!tile) return; dtPolyRef base = getPolyRefBase(tile); // Find Off-mesh connection end points. for (int i = 0; i < tile->header->offMeshConCount; ++i) { dtOffMeshConnection* con = &tile->offMeshCons[i]; dtPoly* poly = &tile->polys[con->poly]; const float ext[3] = { con->rad, tile->header->walkableClimb, con->rad }; for (int j = 0; j < 2; ++j) { unsigned char side = j == 0 ? 0xff : con->side; if (side == 0xff) { // Find polygon to connect to. const float* p = &con->pos[j*3]; float nearestPt[3]; dtPolyRef ref = findNearestPolyInTile(tile, p, ext, nearestPt); if (!ref) continue; // findNearestPoly may return too optimistic results, further check to make sure. if (dtSqr(nearestPt[0]-p[0])+dtSqr(nearestPt[2]-p[2]) > dtSqr(con->rad)) continue; // Make sure the location is on current mesh. float* v = &tile->verts[poly->verts[j]*3]; dtVcopy(v, nearestPt); // Link off-mesh connection to target poly. unsigned int idx = allocLink(tile); if (idx != DT_NULL_LINK) { dtLink* link = &tile->links[idx]; link->ref = ref; link->edge = (unsigned char)j; link->side = 0xff; link->bmin = link->bmax = 0; // Add to linked list. link->next = poly->firstLink; poly->firstLink = idx; } // Start end-point is always connect back to off-mesh connection, // Destination end-point only if it is bidirectional link. if (j == 0 || (j == 1 && (con->flags & DT_OFFMESH_CON_BIDIR))) { // Link target poly to off-mesh connection. unsigned int idx = allocLink(tile); if (idx != DT_NULL_LINK) { const unsigned short landPolyIdx = (unsigned short)decodePolyIdPoly(ref); dtPoly* landPoly = &tile->polys[landPolyIdx]; dtLink* link = &tile->links[idx]; link->ref = base | (unsigned int)(con->poly); link->edge = 0xff; link->side = 0xff; link->bmin = link->bmax = 0; // Add to linked list. link->next = landPoly->firstLink; landPoly->firstLink = idx; } } } } } }