/* * All or portions of this file Copyright (c) Amazon.com, Inc. or its affiliates or * its licensors. * * For complete copyright and license terms please see the LICENSE at the root of this * distribution (the "License"). All use of this software is governed by the License, * or, if provided, by the license below or the license accompanying this file. Do not * remove or modify any license notices. This file is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * */ // Original file Copyright Crytek GMBH or its affiliates, used under license. #include "CryLegacy_precompiled.h" #include "MeshGrid.h" #include "OffGridLinks.h" #include "Tile.h" #include "IslandConnections.h" #if defined(min) #undef min #endif #if defined(max) #undef max #endif //#pragma optimize("", off) //#pragma inline_depth(0) namespace MNM { bool MeshGrid::WayQueryRequest::CanUseOffMeshLink(const OffMeshLinkID linkID, float* costMultiplier) const { if (m_pRequester) { if (IEntity* pEntity = m_pRequester->GetPathAgentEntity()) { return GetOffMeshNavigation().CanUseLink(pEntity, linkID, costMultiplier); } } return true; // Always allow by default } bool MeshGrid::WayQueryRequest::IsPointValidForAgent(const Vec3& pos, uint32 flags) const { if (m_pRequester) { return m_pRequester->IsPointValidForAgent(pos, flags); } return true; // Always allow by default } const real_t MeshGrid::kMinPullingThreshold = real_t(0.05f); const real_t MeshGrid::kMaxPullingThreshold = real_t(0.95f); const real_t MeshGrid::kAdjecencyCalculationToleranceSq = square(real_t(0.02f)); static const int NeighbourOffset_MeshGrid[14][3] = { { 1, 0, 0}, { 1, 0, 1}, { 1, 0, -1}, { 0, 1, 0}, { 0, 1, 1}, { 0, 1, -1}, { 0, 0, 1}, {-1, 0, 0}, {-1, 0, -1}, {-1, 0, 1}, { 0, -1, 0}, { 0, -1, -1}, { 0, -1, 1}, { 0, 0, -1}, }; MeshGrid::MeshGrid() : m_tiles(0) , m_tileCount(0) , m_tileCapacity(0) , m_triangleCount(0) { m_islands.reserve(32); } MeshGrid::~MeshGrid() { for (size_t i = 0; i < m_tileCapacity; ++i) { m_tiles[i].tile.Destroy(); } SAFE_DELETE_ARRAY(m_tiles); } void MeshGrid::Init(const Params& params) { assert(m_tiles == NULL); assert(m_tileCapacity == 0); m_params = params; Grow(params.tileCount); } #pragma warning(push) # pragma warning(disable:28285) size_t MeshGrid::GetTriangles(aabb_t aabb, TriangleID* triangles, size_t maxTriCount, float minIslandArea) const { const size_t minX = (std::max(aabb.min.x, real_t(0)) / real_t(m_params.tileSize.x)).as_uint(); const size_t minY = (std::max(aabb.min.y, real_t(0)) / real_t(m_params.tileSize.y)).as_uint(); const size_t minZ = (std::max(aabb.min.z, real_t(0)) / real_t(m_params.tileSize.z)).as_uint(); const size_t maxX = (std::max(aabb.max.x, real_t(0)) / real_t(m_params.tileSize.x)).as_uint(); const size_t maxY = (std::max(aabb.max.y, real_t(0)) / real_t(m_params.tileSize.y)).as_uint(); const size_t maxZ = (std::max(aabb.max.z, real_t(0)) / real_t(m_params.tileSize.z)).as_uint(); size_t triCount = 0; for (uint y = minY; y <= maxY; ++y) { for (uint x = minX; x <= maxX; ++x) { for (uint z = minZ; z <= maxZ; ++z) { if (const TileID tileID = GetTileID(x, y, z)) { const Tile& tile = GetTile(tileID); const vector3_t tileOrigin( real_t(x * m_params.tileSize.x), real_t(y * m_params.tileSize.y), real_t(z * m_params.tileSize.z)); aabb_t relative(aabb); relative.min = vector3_t::maximize(relative.min - tileOrigin, vector3_t(0, 0, 0)); relative.max = vector3_t::minimize(relative.max - tileOrigin, vector3_t(m_params.tileSize.x, m_params.tileSize.y, m_params.tileSize.z)); if (!tile.nodeCount) { for (size_t i = 0; i < tile.triangleCount; ++i) { const Tile::Triangle& triangle = tile.triangles[i]; const Tile::Vertex& v0 = tile.vertices[triangle.vertex[0]]; const Tile::Vertex& v1 = tile.vertices[triangle.vertex[1]]; const Tile::Vertex& v2 = tile.vertices[triangle.vertex[2]]; const aabb_t triaabb(vector3_t::minimize(v0, v1, v2), vector3_t::maximize(v0, v1, v2)); if (relative.overlaps(triaabb)) { TriangleID triangleID = ComputeTriangleID(tileID, i); if (minIslandArea <= 0.f || GetIslandAreaForTriangle(triangleID) >= minIslandArea) { triangles[triCount++] = triangleID; if (triCount == maxTriCount) { return triCount; } } } } } else { size_t nodeID = 0; const size_t nodeCount = tile.nodeCount; while (nodeID < nodeCount) { const Tile::BVNode& node = tile.nodes[nodeID]; if (!relative.overlaps(node.aabb)) { nodeID += node.leaf ? 1 : node.offset; } else { ++nodeID; if (node.leaf) { TriangleID triangleID = ComputeTriangleID(tileID, node.offset); if (minIslandArea <= 0.f || GetIslandAreaForTriangle(triangleID) >= minIslandArea) { triangles[triCount++] = triangleID; if (triCount == maxTriCount) { return triCount; } } } } } } } } } } return triCount; } #pragma warning(pop) TriangleID MeshGrid::GetTriangleAt(const vector3_t& location, const real_t verticalDownwardRange, const real_t verticalUpwardRange, float minIslandArea) const { const MNM::aabb_t aabb( MNM::vector3_t(MNM::real_t(location.x), MNM::real_t(location.y), MNM::real_t(location.z - verticalDownwardRange)), MNM::vector3_t(MNM::real_t(location.x), MNM::real_t(location.y), MNM::real_t(location.z + verticalUpwardRange))); const size_t MaxTriCandidateCount = 1024; TriangleID candidates[MaxTriCandidateCount]; TriangleID closestID = 0; const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount, minIslandArea); MNM::real_t::unsigned_overflow_type distMinSq = std::numeric_limits::max(); if (candidateCount) { MNM::vector3_t a, b, c; for (size_t i = 0; i < candidateCount; ++i) { GetVertices(candidates[i], a, b, c); if (PointInTriangle(vector2_t(location), vector2_t(a), vector2_t(b), vector2_t(c))) { const MNM::vector3_t ptClosest = ClosestPtPointTriangle(location, a, b, c); const MNM::real_t::unsigned_overflow_type dSq = (ptClosest - location).lenSqNoOverflow(); if (dSq < distMinSq) { distMinSq = dSq; closestID = candidates[i]; } } } } return closestID; } TriangleID MeshGrid::GetTriangleEdgeAlongLine(const vector3_t& startLocation, const vector3_t& endLocation, const real_t verticalDownwardRange, const real_t verticalUpwardRange, vector3_t& hit, float minIslandArea) const { // Determine bounding box real_t minX, minY, minZ; real_t maxX = (startLocation.x > endLocation.x) ? (minX = endLocation.x, startLocation.x) : (minX = startLocation.x, endLocation.x); real_t maxY = (startLocation.y > endLocation.y) ? (minY = endLocation.y, startLocation.y) : (minY = startLocation.y, endLocation.y); real_t maxZ = (startLocation.z > endLocation.z) ? (minZ = endLocation.z, startLocation.z) : (minZ = startLocation.z, endLocation.z); const aabb_t aabb(vector3_t(minX, minY, minZ - verticalDownwardRange), vector3_t(maxX, maxY, maxZ + verticalUpwardRange)); // Gather intersecting triangles const size_t MaxTriCandidateCount = 1024; TriangleID candidates[MaxTriCandidateCount]; const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount /*, minIslandArea parameter is no longer supported*/); TriangleID triangleID = 0; if (candidateCount) { vector3_t verts[3]; real_t s, t; real_t closest = real_t::max(); for (size_t i = 0; i < candidateCount; ++i) { if (GetVertices(candidates[i], verts)) { for (uint8 v = 0; v < 3; ++v) { if (IntersectSegmentSegment(startLocation, endLocation, verts[v], verts[next_mod3(v)], s, t)) { if (s < closest) { closest = s; vector3_t d = (endLocation - startLocation); hit = startLocation + (d * s); triangleID = candidates[i]; } } } } } } return triangleID; } bool MeshGrid::IsTriangleAcceptableForLocation(const vector3_t& location, TriangleID triangleID) const { const MNM::real_t range = MNM::real_t(1.0f); if (triangleID) { const MNM::aabb_t aabb( MNM::vector3_t(MNM::real_t(location.x - range), MNM::real_t(location.y - range), MNM::real_t(location.z - range)), MNM::vector3_t(MNM::real_t(location.x + range), MNM::real_t(location.y + range), MNM::real_t(location.z + range))); const size_t MaxTriCandidateCount = 1024; TriangleID candidates[MaxTriCandidateCount]; const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount); MNM::real_t distMinSq = MNM::real_t::max(); if (candidateCount) { MNM::vector3_t start = location; MNM::vector3_t a, b, c; for (size_t i = 0; i < candidateCount; ++i) { GetVertices(candidates[i], a, b, c); if (candidates[i] == triangleID && PointInTriangle(vector2_t(location), vector2_t(a), vector2_t(b), vector2_t(c))) { return true; } } } } return false; } TriangleID MeshGrid::GetClosestTriangle(const vector3_t& location, real_t vrange, real_t hrange, real_t* distSq, vector3_t* closest, float minIslandArea) const { const MNM::aabb_t aabb( MNM::vector3_t(MNM::real_t(location.x - hrange), MNM::real_t(location.y - hrange), MNM::real_t(location.z - vrange)), MNM::vector3_t(MNM::real_t(location.x + hrange), MNM::real_t(location.y + hrange), MNM::real_t(location.z + vrange))); const size_t MaxTriCandidateCount = 1024; TriangleID candidates[MaxTriCandidateCount]; TriangleID closestID = 0; const size_t candidateCount = GetTriangles(aabb, candidates, MaxTriCandidateCount, minIslandArea); MNM::real_t distMinSq = MNM::real_t::max(); if (candidateCount) { MNM::vector3_t a, b, c; for (size_t i = 0; i < candidateCount; ++i) { GetVertices(candidates[i], a, b, c); const MNM::vector3_t ptClosest = ClosestPtPointTriangle(location, a, b, c); const MNM::real_t dSq = (ptClosest - location).lenNoOverflow(); if (dSq < distMinSq) { if (closest) { *closest = ptClosest; } distMinSq = dSq; closestID = candidates[i]; } } } if (distSq) { *distSq = distMinSq; } return closestID; } bool MeshGrid::GetVertices(TriangleID triangleID, vector3_t& v0, vector3_t& v1, vector3_t& v2) const { if (const TileID tileID = ComputeTileID(triangleID)) { const TileContainer& container = m_tiles[tileID - 1]; const vector3_t origin( real_t(container.x * m_params.tileSize.x), real_t(container.y * m_params.tileSize.y), real_t(container.z * m_params.tileSize.z)); const Tile::Triangle& triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)]; v0 = origin + vector3_t(container.tile.vertices[triangle.vertex[0]]); v1 = origin + vector3_t(container.tile.vertices[triangle.vertex[1]]); v2 = origin + vector3_t(container.tile.vertices[triangle.vertex[2]]); return true; } return false; } bool MeshGrid::GetVertices(TriangleID triangleID, vector3_t* verts) const { return GetVertices(triangleID, verts[0], verts[1], verts[2]); } bool MeshGrid::GetLinkedEdges(TriangleID triangleID, size_t& linkedEdges) const { if (const TileID tileID = ComputeTileID(triangleID)) { const TileContainer& container = m_tiles[tileID - 1]; const Tile::Triangle& triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)]; linkedEdges = 0; for (size_t l = 0; l < triangle.linkCount; ++l) { const Tile::Link& link = container.tile.links[triangle.firstLink + l]; const size_t edge = link.edge; linkedEdges |= 1ULL << edge; } return true; } return false; } bool MeshGrid::GetTriangle(TriangleID triangleID, Tile::Triangle& triangle) const { if (const TileID tileID = ComputeTileID(triangleID)) { const TileContainer& container = m_tiles[tileID - 1]; triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)]; return true; } return false; } bool MeshGrid::PushPointInsideTriangle(const TriangleID triangleID, vector3_t& location, real_t amount) const { if (amount <= 0) { return false; } vector3_t v0, v1, v2; if (const TileID tileID = ComputeTileID(triangleID)) { const TileContainer& container = m_tiles[tileID - 1]; const vector3_t origin( real_t(container.x * m_params.tileSize.x), real_t(container.y * m_params.tileSize.y), real_t(container.z * m_params.tileSize.z)); const vector3_t locationTileOffsetted = (location - origin); const Tile::Triangle& triangle = container.tile.triangles[ComputeTriangleIndex(triangleID)]; v0 = vector3_t(container.tile.vertices[triangle.vertex[0]]); v1 = vector3_t(container.tile.vertices[triangle.vertex[1]]); v2 = vector3_t(container.tile.vertices[triangle.vertex[2]]); const vector3_t triangleCenter = ((v0 + v1 + v2) * real_t::fraction(1, 3)); const vector3_t locationToCenter = triangleCenter - locationTileOffsetted; const real_t locationToCenterLen = locationToCenter.lenNoOverflow(); if (locationToCenterLen > amount) { location += (locationToCenter / locationToCenterLen) * amount; } else { // If locationToCenterLen is smaller than the amount I wanna push // the point, then it's safer use directly the center position // otherwise the point could end up outside the other side of the triangle location = triangleCenter + origin; } return true; } return false; } void MeshGrid::ResetConnectedIslandsIDs() { for (TileMap::iterator tileIt = m_tileMap.begin(); tileIt != m_tileMap.end(); ++tileIt) { Tile& tile = m_tiles[tileIt->second - 1].tile; for (uint16 i = 0; i < tile.triangleCount; ++i) { Tile::Triangle& triangle = tile.triangles[i]; triangle.islandID = MNM::Constants::eStaticIsland_InvalidIslandID; } } m_islands.clear(); } void MeshGrid::ComputeStaticIslandsAndConnections(const NavigationMeshID meshID, const MNM::OffMeshNavigation& offMeshNavigation, MNM::IslandConnections& islandConnections) { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); ResetConnectedIslandsIDs(); ComputeStaticIslands(); ResolvePendingIslandConnectionRequests(meshID, offMeshNavigation, islandConnections); } void MeshGrid::ComputeStaticIslands() { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); typedef std::vector Triangles; Triangles trianglesToVisit; trianglesToVisit.reserve(4096); // The idea is to iterate through the existing tiles and then finding the triangles from which // start the assigning of the different island ids. for (TileMap::iterator tileIt = m_tileMap.begin(); tileIt != m_tileMap.end(); ++tileIt) { const TileID tileID = tileIt->second; Tile& tile = m_tiles[tileID - 1].tile; for (uint16 triangleIndex = 0; triangleIndex < tile.triangleCount; ++triangleIndex) { Tile::Triangle& sourceTriangle = tile.triangles[triangleIndex]; if (sourceTriangle.islandID == MNM::Constants::eStaticIsland_InvalidIslandID) { Island& newIsland = GetNewIsland(); sourceTriangle.islandID = newIsland.id; const TriangleID triangleID = ComputeTriangleID(tileID, triangleIndex); newIsland.area = tile.GetTriangleArea(triangleID).as_float(); trianglesToVisit.push_back(triangleID); // We now have another triangle to start from to assign the new island ids to all the connected // triangles size_t totalTrianglesToVisit = 1; for (size_t index = 0; index < totalTrianglesToVisit; ++index) { // Get next triangle to start the evaluation from const MNM::TriangleID currentTriangleID = trianglesToVisit[index]; const TileID currentTileId = ComputeTileID(currentTriangleID); CRY_ASSERT_MESSAGE(currentTileId > 0, "ComputeStaticIslands is trying to access a triangle ID associated with an invalid tile id."); const TileContainer& container = m_tiles[currentTileId - 1]; const Tile& currentTile = container.tile; const Tile::Triangle& currentTriangle = currentTile.triangles[ComputeTriangleIndex(currentTriangleID)]; // Calc area of this triangle float farea = currentTile.GetTriangleArea(currentTriangleID).as_float(); for (uint16 l = 0; l < currentTriangle.linkCount; ++l) { const Tile::Link& link = currentTile.links[currentTriangle.firstLink + l]; if (link.side == Tile::Link::Internal) { Tile::Triangle& nextTriangle = currentTile.triangles[link.triangle]; if (nextTriangle.islandID == MNM::Constants::eGlobalIsland_InvalidIslandID) { ++totalTrianglesToVisit; nextTriangle.islandID = newIsland.id; newIsland.area += farea; trianglesToVisit.push_back(ComputeTriangleID(currentTileId, link.triangle)); } } else if (link.side == Tile::Link::OffMesh) { QueueIslandConnectionSetup(currentTriangle.islandID, currentTriangleID, link.triangle); } else { const TileID neighbourTileID = GetNeighbourTileID(container.x, container.y, container.z, link.side); CRY_ASSERT_MESSAGE(neighbourTileID > 0, "ComputeStaticIslands is trying to access an invalid neighbour tile."); const Tile& neighbourTile = m_tiles[neighbourTileID - 1].tile; Tile::Triangle& nextTriangle = neighbourTile.triangles[link.triangle]; if (nextTriangle.islandID == MNM::Constants::eGlobalIsland_InvalidIslandID) { ++totalTrianglesToVisit; nextTriangle.islandID = newIsland.id; newIsland.area += farea; trianglesToVisit.push_back(ComputeTriangleID(neighbourTileID, link.triangle)); } } } } trianglesToVisit.clear(); } } } } MeshGrid::Island& MeshGrid::GetNewIsland() { assert(m_islands.size() != std::numeric_limits::max()); // Generate new id (NOTE: Invalid is 0) StaticIslandID id = (m_islands.size() + 1); m_islands.push_back(Island(id)); return m_islands.back(); } void MeshGrid::QueueIslandConnectionSetup(const StaticIslandID islandID, const TriangleID startingTriangleID, const uint16 offMeshLinkIndex) { m_islandConnectionRequests.push_back(IslandConnectionRequest(islandID, startingTriangleID, offMeshLinkIndex)); } void MeshGrid::ResolvePendingIslandConnectionRequests(const NavigationMeshID meshID, const MNM::OffMeshNavigation& offMeshNavigation, MNM::IslandConnections& islandConnections) { while (!m_islandConnectionRequests.empty()) { IslandConnectionRequest& request = m_islandConnectionRequests.back(); OffMeshNavigation::QueryLinksResult links = offMeshNavigation.GetLinksForTriangle(request.startingTriangleID, request.offMeshLinkIndex); while (WayTriangleData nextTri = links.GetNextTriangle()) { Tile::Triangle endTriangle; if (GetTriangle(nextTri.triangleID, endTriangle)) { const OffMeshLink* pLink = offMeshNavigation.GetObjectLinkInfo(nextTri.offMeshLinkID); assert(pLink); MNM::IslandConnections::Link link(nextTri.triangleID, nextTri.offMeshLinkID, GlobalIslandID(meshID, endTriangle.islandID), pLink->GetEntityIdForOffMeshLink()); islandConnections.SetOneWayConnectionBetweenIsland(GlobalIslandID(meshID, request.startingIslandID), link); } } m_islandConnectionRequests.pop_back(); } } void MeshGrid::SearchForIslandConnectionsToRefresh(const TileID tileID) { Tile& tile = m_tiles[tileID - 1].tile; for (uint16 triangleIndex = 0; triangleIndex < tile.triangleCount; ++triangleIndex) { Tile::Triangle& triangle = tile.triangles[triangleIndex]; for (uint16 l = 0; l < triangle.linkCount; ++l) { const Tile::Link& link = tile.links[triangle.firstLink + l]; if (link.side == Tile::Link::OffMesh) { QueueIslandConnectionSetup(triangle.islandID, ComputeTriangleID(tileID, triangleIndex), link.triangle); } } } } float MeshGrid::GetIslandArea(StaticIslandID islandID) const { bool isValid = (islandID >= MNM::Constants::eStaticIsland_FirstValidIslandID && islandID <= m_islands.size()); return (isValid) ? m_islands[islandID - 1].area : -1.f; } float MeshGrid::GetIslandAreaForTriangle(TriangleID triangleID) const { Tile::Triangle triangle; if (GetTriangle(triangleID, triangle)) { return GetIslandArea(triangle.islandID); } return -1.f; } void MeshGrid::PredictNextTriangleEntryPosition(const TriangleID bestNodeTriangleID, const vector3_t& bestNodePosition, const TriangleID nextTriangleID, const unsigned int edgeIndex , const vector3_t finalLocation, vector3_t& outPosition) const { IF_UNLIKELY (edgeIndex == MNM::Constants::InvalidEdgeIndex) { // For SO links we don't set up the edgeIndex value since it's more probable that the animations // ending point is better approximated by the triangle center value vector3_t v0, v1, v2; GetVertices(nextTriangleID, v0, v1, v2); outPosition = (v0 + v1 + v2) * real_t::fraction(1, 3); return; } Tile::Triangle triangle; if (GetTriangle(bestNodeTriangleID, triangle)) { const TileID bestTriangleTileID = ComputeTileID(bestNodeTriangleID); assert(bestTriangleTileID); const TileContainer& currentContainer = m_tiles[bestTriangleTileID - 1]; const Tile& currentTile = currentContainer.tile; const vector3_t tileOrigin(real_t(currentContainer.x * m_params.tileSize.x), real_t(currentContainer.y * m_params.tileSize.y), real_t(currentContainer.z * m_params.tileSize.z)); assert(edgeIndex < 3); const vector3_t v0 = tileOrigin + vector3_t(currentTile.vertices[triangle.vertex[edgeIndex]]); const vector3_t v1 = tileOrigin + vector3_t(currentTile.vertices[triangle.vertex[next_mod3(edgeIndex)]]); switch (gAIEnv.CVars.MNMPathfinderPositionInTrianglePredictionType) { case ePredictionType_TriangleCenter: { const vector3_t v2 = tileOrigin + vector3_t(currentTile.vertices[triangle.vertex[dec_mod3[edgeIndex]]]); outPosition = (v0 + v1 + v2) * real_t::fraction(1, 3); } break; case ePredictionType_Advanced: default: { const vector3_t v0v1 = v1 - v0; real_t s, t; if (IntersectSegmentSegment(v0, v1, bestNodePosition, finalLocation, s, t)) { // If the two segments intersect, // let's choose the point that goes in the direction we want to go s = clamp(s, kMinPullingThreshold, kMaxPullingThreshold); outPosition = v0 + v0v1 * s; } else { // Otherwise we need to understand where the segment is in relation // of where we want to go. // Let's choose the point on the segment that is closer towards the target const real_t::unsigned_overflow_type distSqAE = (v0 - finalLocation).lenSqNoOverflow(); const real_t::unsigned_overflow_type distSqBE = (v1 - finalLocation).lenSqNoOverflow(); const real_t segmentPercentage = (distSqAE < distSqBE) ? kMinPullingThreshold : kMaxPullingThreshold; outPosition = v0 + v0v1 * segmentPercentage; } } break; } } else { // At this point it's not acceptable to have an invalid triangle assert(0); } } MeshGrid::EWayQueryResult MeshGrid::FindWay(WayQueryRequest& inputRequest, WayQueryWorkingSet& workingSet, WayQueryResult& result) const { result.SetWaySize(0); if (result.GetWayMaxSize() < 2) { return eWQR_Done; } if (inputRequest.From() && inputRequest.To()) { if (inputRequest.From() == inputRequest.To()) { WayTriangleData* pOutputWay = result.GetWayData(); pOutputWay[0] = WayTriangleData(inputRequest.From(), 0); pOutputWay[1] = WayTriangleData(inputRequest.To(), 0); result.SetWaySize(2); return eWQR_Done; } else { const vector3_t origin(m_params.origin); const vector3_t startLocation = inputRequest.GetFromLocation(); const vector3_t endLocation = inputRequest.GetToLocation(); WayTriangleData lastBestNodeID(inputRequest.From(), 0); while (workingSet.aStarOpenList.CanDoStep()) { // switch the smallest element with the last one and pop the last element AStarOpenList::OpenNodeListElement element = workingSet.aStarOpenList.PopBestNode(); WayTriangleData bestNodeID = element.triData; lastBestNodeID = bestNodeID; IF_UNLIKELY (bestNodeID.triangleID == inputRequest.To()) { workingSet.aStarOpenList.StepDone(); break; } AStarOpenList::Node* bestNode = element.pNode; const vector3_t bestNodeLocation = bestNode->location - origin; const TileID tileID = ComputeTileID(bestNodeID.triangleID); // Clear from previous step workingSet.nextLinkedTriangles.clear(); if (tileID) { const TileContainer& container = m_tiles[tileID - 1]; const Tile& tile = container.tile; const uint16 triangleIdx = ComputeTriangleIndex(bestNodeID.triangleID); const Tile::Triangle& triangle = tile.triangles[triangleIdx]; //Gather all accessible triangles first for (size_t l = 0; l < triangle.linkCount; ++l) { const Tile::Link& link = tile.links[triangle.firstLink + l]; WayTriangleData nextTri(0, 0); if (link.side == Tile::Link::Internal) { nextTri.triangleID = ComputeTriangleID(tileID, link.triangle); nextTri.incidentEdge = link.edge; } else if (link.side == Tile::Link::OffMesh) { OffMeshNavigation::QueryLinksResult links = inputRequest.GetOffMeshNavigation().GetLinksForTriangle(bestNodeID.triangleID, link.triangle); while (nextTri = links.GetNextTriangle()) { if (inputRequest.CanUseOffMeshLink(nextTri.offMeshLinkID, &nextTri.costMultiplier)) { workingSet.nextLinkedTriangles.push_back(nextTri); nextTri.incidentEdge = (unsigned int)MNM::Constants::InvalidEdgeIndex; } } continue; } else { TileID neighbourTileID = GetNeighbourTileID(container.x, container.y, container.z, link.side); nextTri.triangleID = ComputeTriangleID(neighbourTileID, link.triangle); nextTri.incidentEdge = link.edge; } Vec3 edgeMidPoint; if (CalculateMidEdge(bestNodeID.triangleID, nextTri.triangleID, edgeMidPoint)) { const uint32 flags = 0; if (inputRequest.IsPointValidForAgent(edgeMidPoint, flags)) { workingSet.nextLinkedTriangles.push_back(nextTri); } } ////////////////////////////////////////////////////////////////////////// // NOTE: This is user defined only at compile time BreakOnInvalidTriangle(nextTri.triangleID, m_tileCapacity); ////////////////////////////////////////////////////////////////////////// } } else { AIError("MeshGrid::FindWay - Bad Navmesh data Tile: %d, Triangle: %d, skipping ", tileID, ComputeTriangleIndex(bestNodeID.triangleID)); BreakOnInvalidTriangle(bestNodeID.triangleID, m_tileCapacity); } const size_t triangleCount = workingSet.nextLinkedTriangles.size(); for (size_t t = 0; t < triangleCount; ++t) { WayTriangleData nextTri = workingSet.nextLinkedTriangles[t]; if (nextTri == bestNode->prevTriangle) { continue; } AStarOpenList::Node* nextNode = NULL; const bool inserted = workingSet.aStarOpenList.InsertNode(nextTri, &nextNode); assert(nextNode); if (inserted) { IF_UNLIKELY (nextTri.triangleID == inputRequest.To()) { nextNode->location = endLocation; } else { PredictNextTriangleEntryPosition(bestNodeID.triangleID, bestNodeLocation, nextTri.triangleID, nextTri.incidentEdge, endLocation, nextNode->location); } nextNode->open = false; } //Euclidean distance const vector3_t targetDistance = endLocation - nextNode->location; const vector3_t stepDistance = bestNodeLocation - nextNode->location; const real_t heuristic = targetDistance.lenNoOverflow(); const real_t stepCost = stepDistance.lenNoOverflow(); //Euclidean distance approximation //const real_t heuristic = targetDistance.approximatedLen(); //const real_t stepCost = stepDistance.approximatedLen(); const real_t dangersTotalCost = CalculateHeuristicCostForDangers(nextNode->location, startLocation, m_params.origin, inputRequest.GetDangersInfos()); real_t costMultiplier = real_t(nextTri.costMultiplier); const real_t cost = bestNode->cost + (stepCost * costMultiplier) + dangersTotalCost; const real_t total = cost + heuristic; if (nextNode->open && nextNode->estimatedTotalCost <= total) { continue; } nextNode->cost = cost; nextNode->estimatedTotalCost = total; nextNode->prevTriangle = bestNodeID; if (!nextNode->open) { nextNode->open = true; nextNode->location += origin; workingSet.aStarOpenList.AddToOpenList(nextTri, nextNode, total); } } workingSet.aStarOpenList.StepDone(); } if (lastBestNodeID.triangleID == inputRequest.To()) { size_t wayTriCount = 0; WayTriangleData wayTriangle = lastBestNodeID; WayTriangleData nextInsertion(wayTriangle.triangleID, 0); WayTriangleData* outputWay = result.GetWayData(); while (wayTriangle.triangleID != inputRequest.From()) { const AStarOpenList::Node* node = workingSet.aStarOpenList.FindNode(wayTriangle); assert(node); outputWay[wayTriCount++] = nextInsertion; //Iterate the path backwards, and shift the offMeshLink to the previous triangle (start of the link) nextInsertion.offMeshLinkID = wayTriangle.offMeshLinkID; wayTriangle = node->prevTriangle; nextInsertion.triangleID = wayTriangle.triangleID; if (wayTriCount == result.GetWayMaxSize()) { break; } } if (wayTriCount < result.GetWayMaxSize()) { outputWay[wayTriCount++] = WayTriangleData(inputRequest.From(), nextInsertion.offMeshLinkID); } result.SetWaySize(wayTriCount); return eWQR_Done; } else if (!workingSet.aStarOpenList.Empty()) { //We did not finish yet... return eWQR_Continuing; } } } return eWQR_Done; } struct CostAccumulator { CostAccumulator(const Vec3& locationToEval, const Vec3& startingLocation, real_t& totalCost) : m_totalCost(totalCost) , m_locationToEvaluate(locationToEval) , m_startingLocation(startingLocation) {} void operator()(const DangerAreaConstPtr& dangerInfo) { m_totalCost += dangerInfo->GetDangerHeuristicCost(m_locationToEvaluate, m_startingLocation); } private: real_t& m_totalCost; const Vec3& m_locationToEvaluate; const Vec3& m_startingLocation; }; real_t MeshGrid::CalculateHeuristicCostForDangers(const vector3_t& locationToEval, const vector3_t& startingLocation, const Vec3& meshOrigin, const DangerousAreasList& dangersInfos) const { real_t totalCost(0.0f); const Vec3 startingLocationInWorldSpace = startingLocation.GetVec3() + meshOrigin; const Vec3 locationInWorldSpace = locationToEval.GetVec3() + meshOrigin; std::for_each(dangersInfos.begin(), dangersInfos.end(), CostAccumulator(locationInWorldSpace, startingLocationInWorldSpace, totalCost)); return totalCost; } void MeshGrid::PullString(const vector3_t from, const TriangleID fromTriID, const vector3_t to, const TriangleID toTriID, vector3_t& middlePoint) const { if (const TileID fromTileID = ComputeTileID(fromTriID)) { const TileContainer& startContainer = m_tiles[fromTileID - 1]; const Tile& startTile = startContainer.tile; uint16 fromTriangleIdx = ComputeTriangleIndex(fromTriID); const Tile::Triangle& fromTriangle = startTile.triangles[fromTriangleIdx]; uint16 vi0 = 0, vi1 = 0; for (int l = 0; l < fromTriangle.linkCount; ++l) { const Tile::Link& link = startTile.links[fromTriangle.firstLink + l]; if (link.side == Tile::Link::Internal) { TriangleID newTriangleID = ComputeTriangleID(fromTileID, link.triangle); if (newTriangleID == toTriID) { vi0 = link.edge; vi1 = next_mod3(link.edge); assert(vi0 < 3); assert(vi1 < 3); break; } } else { if (link.side != Tile::Link::OffMesh) { TileID neighbourTileID = GetNeighbourTileID(startContainer.x, startContainer.y, startContainer.z, link.side); TriangleID newTriangleID = ComputeTriangleID(neighbourTileID, link.triangle); if (newTriangleID == toTriID) { vi0 = link.edge; vi1 = next_mod3(link.edge); assert(vi0 < 3); assert(vi1 < 3); break; } } } } vector3_t fromVertices[3]; GetVertices(fromTriID, fromVertices[0], fromVertices[1], fromVertices[2]); if (vi0 != vi1) { real_t s, t; PREFAST_ASSUME(vi0 < 3 && vi1 < 3); vector3_t dir = fromVertices[vi1] - fromVertices[vi0]; dir.normalized(); if (IntersectSegmentSegment(vector2_t(fromVertices[vi0]), vector2_t(fromVertices[vi1]), vector2_t(from), vector2_t(to), s, t)) { const Tile::Triangle& triangle = startContainer.tile.triangles[ComputeTriangleIndex(fromTriID)]; s = clamp(s, kMinPullingThreshold, kMaxPullingThreshold); middlePoint = fromVertices[vi0] + dir * s; } else { if (s < 0) { middlePoint = fromVertices[vi0] + dir * kMinPullingThreshold; } else { middlePoint = fromVertices[vi0] + dir * kMaxPullingThreshold; } } } } } void MeshGrid::AddOffMeshLinkToTile(const TileID tileID, const TriangleID triangleID, const uint16 offMeshIndex) { Tile& tile = GetTile(tileID); m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link)); m_profiler.AddStat(LinkCount, -(int)tile.linkCount); tile.AddOffMeshLink(triangleID, offMeshIndex); m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link)); m_profiler.AddStat(LinkCount, tile.linkCount); } void MeshGrid::UpdateOffMeshLinkForTile(const TileID tileID, const TriangleID triangleID, const uint16 offMeshIndex) { Tile& tile = GetTile(tileID); tile.UpdateOffMeshLink(triangleID, offMeshIndex); } void MeshGrid::RemoveOffMeshLinkFromTile(const TileID tileID, const TriangleID triangleID) { Tile& tile = GetTile(tileID); m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link)); m_profiler.AddStat(LinkCount, -(int)tile.linkCount); tile.RemoveOffMeshLink(triangleID); m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link)); m_profiler.AddStat(LinkCount, tile.linkCount); } inline size_t OppositeSide(size_t side) { return (side + 7) % 14; } std::tuple IntersectSegmentTriangle(const Vec3& segP0, const Vec3& segP1, const Vec3& triV0, const Vec3& triV1, const Vec3& triV2) { const auto noIntersect = std::make_tuple(false, -1.0f, Vec3(IDENTITY)); Vec3 u = triV1 - triV0; Vec3 v = triV2 - triV0; Vec3 n = u.Cross(v); // Check for degenerate triangle if (fcmp(n.GetLengthSquared(), 0.0f)) { return noIntersect; } // Get the ray direction vector Vec3 dir = segP1 - segP0; Vec3 w0 = segP0 - triV0; float a = -n.Dot(w0); float b = n.Dot(dir); // Ray is parallel to triangle plane if (fcmp(b, 0.0f)) { return noIntersect; } // Compute intersection point of ray with triangle plane float r = a / b; // If ray goes away from triangle, or intersects past the segment length, reject if (r < 0.0f || r > 1.0f) { return noIntersect; } // Check to see if the intersection point is within the triangle (Barycentric coordinate check) // http://blogs.msdn.com/b/rezanour/archive/2011/08/07/barycentric-coordinates-and-point-in-triangle-tests.aspx Vec3 I = segP0 + r * dir; Vec3 w = I - triV0; float uu = u.Dot(u); float uv = u.Dot(v); float vv = v.Dot(v); float wu = w.Dot(u); float wv = w.Dot(v); float D = uv * uv - uu * vv; float s = (uv * wv - vv * wu) / D; if (s < 0.0f || s > 1.0f) { return noIntersect; } float t = (uv * wu - uu * wv) / D; if (t < 0.0f || (s + t) > 1.0f) { return noIntersect; } return std::make_tuple(true, r, I); } std::tuple MeshGrid::RayCastWorld(const Vec3& segP0, const Vec3& segP1) const { auto minResult = std::make_tuple(false, FLT_MAX, Vec3(IDENTITY)); for (int i = 0; i < this->m_tileCount; ++i) { Tile& tile = m_tiles[i].tile; for (int j = 0; j < tile.triangleCount; ++j) { // Get the triangle vertices Vec3 v0, v1, v2; { // Vertices are stored in packed format; unpack them vector3_t vec0, vec1, vec2; GetVertices(ComputeTriangleID(i + 1, j), vec0, vec1, vec2); v0 = vec0.GetVec3(); v1 = vec1.GetVec3(); v2 = vec2.GetVec3(); } // Intersect and record closest auto result = IntersectSegmentTriangle(segP0, segP1, v0, v1, v2); if (std::get<0>(result) && std::get<1>(result) < std::get<1>(minResult)) { minResult = result; } } } return minResult; } MeshGrid::ERayCastResult MeshGrid::RayCast(const vector3_t& from, TriangleID fromTri, const vector3_t& to, TriangleID toTri, RaycastRequestBase& raycastRequest) const { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); const bool useNewRaycast = gAIEnv.CVars.MNMRaycastImplementation != 0; if (useNewRaycast) { return RayCast_new(from, fromTri, to, toTri, raycastRequest); } else { return RayCast_old(from, fromTri, to, toTri, raycastRequest); } } struct RaycastNode { RaycastNode() : triangleID(MNM::Constants::InvalidTriangleID) , percentageOfTotalDistance(-1.0f) , incidentEdge((uint16) MNM::Constants::InvalidEdgeIndex) {} RaycastNode(const TriangleID _triangleID, const real_t _percentageOfTotalDistance, const uint16 _incidentEdge) : triangleID(_triangleID) , percentageOfTotalDistance(_percentageOfTotalDistance) , incidentEdge(_incidentEdge) {} RaycastNode(const RaycastNode& otherNode) : triangleID(otherNode.triangleID) , percentageOfTotalDistance(otherNode.percentageOfTotalDistance) , incidentEdge(otherNode.incidentEdge) {} ILINE bool IsValid() const { return triangleID != MNM::Constants::InvalidTriangleID; } ILINE bool operator<(const RaycastNode& other) const { return triangleID < other.triangleID; } ILINE bool operator==(const RaycastNode& other) const { return triangleID == other.triangleID; } TriangleID triangleID; real_t percentageOfTotalDistance; uint16 incidentEdge; }; struct IsNodeCloserToEndPredicate { bool operator() (const RaycastNode& firstNode, const RaycastNode& secondNode) { return firstNode.percentageOfTotalDistance > secondNode.percentageOfTotalDistance; } }; typedef OpenList RaycastOpenList; typedef VectorSet RaycastClosedList; MeshGrid::ERayCastResult MeshGrid::RayCast_new(const vector3_t& from, TriangleID fromTriangleID, const vector3_t& to, TriangleID toTriangleID, RaycastRequestBase& raycastRequest) const { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); if (!IsLocationInTriangle(from, fromTriangleID)) { return eRayCastResult_InvalidStart; } #ifdef RAYCAST_DO_NOT_ACCEPT_INVALID_END if (!IsLocationInTriangle(to, toTriangleID)) { return eRayCastResult_InvalidEnd; } #endif RaycastClosedList closedList; closedList.reserve(raycastRequest.maxWayTriCount); RaycastCameFromMap cameFrom; cameFrom.reserve(raycastRequest.maxWayTriCount); const size_t expectedOpenListSize = 10; RaycastOpenList openList(expectedOpenListSize); RaycastNode furtherNodeVisited; RayHit rayHit; RaycastNode currentNode(fromTriangleID, real_t(.0f), (uint16) MNM::Constants::InvalidEdgeIndex); while (currentNode.IsValid()) { if (currentNode.triangleID == toTriangleID) { ReconstructRaycastResult(fromTriangleID, toTriangleID, cameFrom, raycastRequest); return eRayCastResult_NoHit; } if (currentNode.percentageOfTotalDistance > furtherNodeVisited.percentageOfTotalDistance) { furtherNodeVisited = currentNode; } rayHit.triangleID = currentNode.triangleID; RaycastNode nextNode; closedList.insert(currentNode.triangleID); TileID tileID = ComputeTileID(currentNode.triangleID); if (tileID != MNM::Constants::InvalidTileID) { const TileContainer* container = &m_tiles[tileID - 1]; const Tile* tile = &container->tile; vector3_t tileOrigin( real_t(container->x * m_params.tileSize.x), real_t(container->y * m_params.tileSize.y), real_t(container->z * m_params.tileSize.z)); const Tile::Triangle& triangle = tile->triangles[ComputeTriangleIndex(currentNode.triangleID)]; for (uint16 edgeIndex = 0; edgeIndex < 3; ++edgeIndex) { if (currentNode.incidentEdge == edgeIndex) { continue; } const vector3_t a = tileOrigin + vector3_t(tile->vertices[triangle.vertex[edgeIndex]]); const vector3_t b = tileOrigin + vector3_t(tile->vertices[triangle.vertex[ inc_mod3[edgeIndex] ]]); real_t s, t; if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(a), vector2_t(b), s, t)) { if (s < currentNode.percentageOfTotalDistance) { continue; } rayHit.distance = s; rayHit.edge = edgeIndex; for (size_t linkIndex = 0; linkIndex < triangle.linkCount; ++linkIndex) { const Tile::Link& link = tile->links[triangle.firstLink + linkIndex]; if (link.edge != edgeIndex) { continue; } const uint16 side = link.side; if (side == Tile::Link::Internal) { const Tile::Triangle& opposite = tile->triangles[link.triangle]; for (size_t oe = 0; oe < opposite.linkCount; ++oe) { const Tile::Link& reciprocal = tile->links[opposite.firstLink + oe]; const TriangleID possibleNextID = ComputeTriangleID(tileID, link.triangle); if (reciprocal.triangle == ComputeTriangleIndex(currentNode.triangleID)) { if (closedList.find(possibleNextID) != closedList.end()) { break; } cameFrom[possibleNextID] = currentNode.triangleID; if (s > currentNode.percentageOfTotalDistance) { nextNode.incidentEdge = reciprocal.edge; nextNode.percentageOfTotalDistance = s; nextNode.triangleID = possibleNextID; } else { RaycastNode possibleNextEdge(possibleNextID, s, reciprocal.edge); openList.InsertElement(possibleNextEdge); } break; } } } else if (side != Tile::Link::OffMesh) { TileID neighbourTileID = GetNeighbourTileID(container->x, container->y, container->z, link.side); const TileContainer& neighbourContainer = m_tiles[neighbourTileID - 1]; const Tile::Triangle& opposite = neighbourContainer.tile.triangles[link.triangle]; const uint16 currentTriangleIndex = ComputeTriangleIndex(currentNode.triangleID); const uint16 currentOppositeSide = OppositeSide(side); for (size_t reciprocalLinkIndex = 0; reciprocalLinkIndex < opposite.linkCount; ++reciprocalLinkIndex) { const Tile::Link& reciprocal = neighbourContainer.tile.links[opposite.firstLink + reciprocalLinkIndex]; if ((reciprocal.triangle == currentTriangleIndex) && (reciprocal.side == currentOppositeSide)) { const TriangleID possibleNextID = ComputeTriangleID(neighbourTileID, link.triangle); if (closedList.find(possibleNextID) != closedList.end()) { break; } const vector3_t neighbourTileOrigin = vector3_t( real_t(neighbourContainer.x * m_params.tileSize.x), real_t(neighbourContainer.y * m_params.tileSize.y), real_t(neighbourContainer.z * m_params.tileSize.z)); const uint16 i0 = reciprocal.edge; const uint16 i1 = inc_mod3[reciprocal.edge]; assert(i0 < 3); assert(i1 < 3); const vector3_t c = neighbourTileOrigin + vector3_t(neighbourContainer.tile.vertices[opposite.vertex[i0]]); const vector3_t d = neighbourTileOrigin + vector3_t(neighbourContainer.tile.vertices[opposite.vertex[i1]]); real_t p, q; if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(c), vector2_t(d), p, q)) { cameFrom[possibleNextID] = currentNode.triangleID; if (p > currentNode.percentageOfTotalDistance) { nextNode.incidentEdge = reciprocal.edge; nextNode.percentageOfTotalDistance = p; nextNode.triangleID = possibleNextID; } else { RaycastNode possibleNextEdge(possibleNextID, p, reciprocal.edge); openList.InsertElement(possibleNextEdge); } break; // reciprocal link loop } } } } if (nextNode.IsValid()) { break; } } if (nextNode.IsValid()) { break; } } } } if (nextNode.IsValid()) { openList.Reset(); currentNode = nextNode; } else { currentNode = (!openList.IsEmpty()) ? openList.PopBestElement() : RaycastNode(); } } ReconstructRaycastResult(fromTriangleID, furtherNodeVisited.triangleID, cameFrom, raycastRequest); RayHit& requestRayHit = raycastRequest.hit; requestRayHit.distance = rayHit.distance; requestRayHit.triangleID = rayHit.triangleID; requestRayHit.edge = rayHit.edge; return eRayCastResult_Hit; } MeshGrid::ERayCastResult MeshGrid::ReconstructRaycastResult(const TriangleID fromTriangleID, const TriangleID toTriangleID, const RaycastCameFromMap& cameFrom, RaycastRequestBase& raycastRequest) const { TriangleID currentTriangleID = toTriangleID; size_t elementIndex = 0; RaycastCameFromMap::const_iterator elementIterator = cameFrom.find(currentTriangleID); while (elementIterator != cameFrom.end()) { if (elementIndex >= raycastRequest.maxWayTriCount) { return eRayCastResult_RayTooLong; } raycastRequest.way[elementIndex++] = currentTriangleID; currentTriangleID = elementIterator->second; elementIterator = cameFrom.find(currentTriangleID); } raycastRequest.way[elementIndex++] = currentTriangleID; raycastRequest.wayTriCount = elementIndex; return elementIndex < raycastRequest.maxWayTriCount ? eRayCastResult_NoHit : eRayCastResult_RayTooLong; } bool MeshGrid::IsLocationInTriangle(const vector3_t& location, const TriangleID triangleID) const { if (triangleID == MNM::Constants::InvalidTriangleID) { return false; } TileID tileID = ComputeTileID(triangleID); if (tileID == MNM::Constants::InvalidTileID) { return false; } const TileContainer* container = &m_tiles[tileID - 1]; const Tile* tile = &container->tile; vector3_t tileOrigin( real_t(container->x * m_params.tileSize.x), real_t(container->y * m_params.tileSize.y), real_t(container->z * m_params.tileSize.z)); if (triangleID) { const Tile::Triangle& triangle = tile->triangles[ComputeTriangleIndex(triangleID)]; const vector2_t a = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[0]]); const vector2_t b = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[1]]); const vector2_t c = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[2]]); return PointInTriangle(vector2_t(location), a, b, c); } return false; } MeshGrid::ERayCastResult MeshGrid::RayCast_old(const vector3_t& from, TriangleID fromTri, const vector3_t& to, TriangleID toTri, RaycastRequestBase& raycastRequest) const { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); if (TileID tileID = ComputeTileID(fromTri)) { const TileContainer* container = &m_tiles[tileID - 1]; const Tile* tile = &container->tile; vector3_t tileOrigin( real_t(container->x * m_params.tileSize.x), real_t(container->y * m_params.tileSize.y), real_t(container->z * m_params.tileSize.z)); if (fromTri) { const Tile::Triangle& triangle = tile->triangles[ComputeTriangleIndex(fromTri)]; const vector2_t a = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[0]]); const vector2_t b = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[1]]); const vector2_t c = vector2_t(tileOrigin) + vector2_t(tile->vertices[triangle.vertex[2]]); if (!PointInTriangle(vector2_t(from), a, b, c)) { fromTri = 0; } } if (!fromTri) { RayHit& hit = raycastRequest.hit; hit.distance = -real_t::max(); hit.triangleID = 0; hit.edge = 0; return eRayCastResult_Hit; } real_t distance = -1; size_t triCount = 0; TriangleID currentID = fromTri; size_t incidentEdge = MNM::Constants::InvalidEdgeIndex; while (currentID) { if (triCount < raycastRequest.maxWayTriCount) { raycastRequest.way[triCount++] = currentID; } else { // We don't allow rays that pass through more than maxWayTriCount triangles return eRayCastResult_RayTooLong; } if (toTri && currentID == toTri) { raycastRequest.wayTriCount = triCount; return eRayCastResult_NoHit; } const Tile::Triangle& triangle = tile->triangles[ComputeTriangleIndex(currentID)]; TriangleID nextID = 0; real_t possibleDistance = distance; size_t possibleIncidentEdge = MNM::Constants::InvalidEdgeIndex; const TileContainer* possibleContainer = NULL; const Tile* possibleTile = NULL; vector3_t possibleTileOrigin(tileOrigin); TileID possibleTileID = tileID; for (size_t e = 0; e < 3; ++e) { if (incidentEdge == e) { continue; } const vector3_t a = tileOrigin + vector3_t(tile->vertices[triangle.vertex[e]]); const vector3_t b = tileOrigin + vector3_t(tile->vertices[triangle.vertex[next_mod3(e)]]); real_t s, t; if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(a), vector2_t(b), s, t)) { if (s < distance) { continue; } for (size_t l = 0; l < triangle.linkCount; ++l) { const Tile::Link& link = tile->links[triangle.firstLink + l]; if (link.edge != e) { continue; } const size_t side = link.side; if (side == Tile::Link::Internal) { const Tile::Triangle& opposite = tile->triangles[link.triangle]; for (size_t oe = 0; oe < opposite.linkCount; ++oe) { const Tile::Link& reciprocal = tile->links[opposite.firstLink + oe]; const TriangleID possibleNextID = ComputeTriangleID(tileID, link.triangle); if (reciprocal.triangle == ComputeTriangleIndex(currentID)) { distance = s; nextID = possibleNextID; possibleIncidentEdge = reciprocal.edge; possibleTile = tile; possibleTileOrigin = tileOrigin; possibleContainer = container; possibleTileID = tileID; break; // opposite edge loop } } } else if (side != Tile::Link::OffMesh) { TileID neighbourTileID = GetNeighbourTileID(container->x, container->y, container->z, link.side); const TileContainer& neighbourContainer = m_tiles[neighbourTileID - 1]; const Tile::Triangle& opposite = neighbourContainer.tile.triangles[link.triangle]; const uint16 currentTriangleIndex = ComputeTriangleIndex(currentID); const uint16 currentOppositeSide = OppositeSide(side); for (size_t rl = 0; rl < opposite.linkCount; ++rl) { const Tile::Link& reciprocal = neighbourContainer.tile.links[opposite.firstLink + rl]; if ((reciprocal.triangle == currentTriangleIndex) && (reciprocal.side == currentOppositeSide)) { const vector3_t neighbourTileOrigin = vector3_t( real_t(neighbourContainer.x * m_params.tileSize.x), real_t(neighbourContainer.y * m_params.tileSize.y), real_t(neighbourContainer.z * m_params.tileSize.z)); const uint16 i0 = reciprocal.edge; const uint16 i1 = next_mod3(reciprocal.edge); assert(i0 < 3); assert(i1 < 3); const vector3_t c = neighbourTileOrigin + vector3_t( neighbourContainer.tile.vertices[opposite.vertex[i0]]); const vector3_t d = neighbourTileOrigin + vector3_t( neighbourContainer.tile.vertices[opposite.vertex[i1]]); const TriangleID possibleNextID = ComputeTriangleID(neighbourTileID, link.triangle); real_t p, q; if (IntersectSegmentSegment(vector2_t(from), vector2_t(to), vector2_t(c), vector2_t(d), p, q)) { distance = p; nextID = possibleNextID; possibleIncidentEdge = reciprocal.edge; possibleTileID = neighbourTileID; possibleContainer = &neighbourContainer; possibleTile = &neighbourContainer.tile; possibleTileOrigin = neighbourTileOrigin; } break; // reciprocal link loop } } } if (nextID) { break; // link loop } } if (IsTriangleAlreadyInWay(nextID, raycastRequest.way, triCount)) { assert(0); nextID = 0; } // If distance is equals to 0, it means that our starting position is placed // on an edge or on a shared vertex. This means that we need to continue evaluating // the other edges of the triangle to check if we have a better intersection. // This will happen if the ray starts from one edge of the triangle but intersects also // on of the other two edges. If we stop to the first intersection (the one with 0 distance) // we can end up trying to raycast in the wrong direction. // As soon as the distance is more than 0 we can stop the evaluation of the other edges // because it means we are moving towards the direction of the ray. distance = s; const bool shouldStopEvaluationOfOtherEdges = distance > 0; if (shouldStopEvaluationOfOtherEdges) { if (nextID) { break; // edge loop } else { RayHit& hit = raycastRequest.hit; hit.distance = distance; hit.triangleID = currentID; hit.edge = e; raycastRequest.wayTriCount = triCount; return eRayCastResult_Hit; } } } } currentID = nextID; incidentEdge = possibleIncidentEdge; tile = possibleTile ? possibleTile : tile; tileID = possibleTileID; container = possibleContainer ? possibleContainer : container; tileOrigin = possibleTileOrigin; } raycastRequest.wayTriCount = triCount; bool isEndingTriangleAcceptable = IsTriangleAcceptableForLocation(to, raycastRequest.way[triCount - 1]); return isEndingTriangleAcceptable ? eRayCastResult_NoHit : eRayCastResult_Unacceptable; } return eRayCastResult_InvalidStart; } #pragma warning(push) # pragma warning(disable:28285) bool TestEdgeOverlap1D(const real_t& a0, const real_t& a1, const real_t& b0, const real_t& b1, const real_t& dx) { if ((a0 == b0) && (a1 == b1)) { return true; } const real_t amin = a0 < a1 ? a0 : a1; const real_t amax = a0 < a1 ? a1 : a0; const real_t bmin = b0 < b1 ? b0 : b1; const real_t bmax = b0 < b1 ? b1 : b0; const real_t ominx = std::max(amin + dx, bmin + dx); const real_t omaxx = std::min(amax - dx, bmax - dx); if (ominx > omaxx) { return false; } return true; } #pragma warning(pop) bool TestEdgeOverlap2D(const real_t& toleranceSq, const vector2_t& a0, const vector2_t& a1, const vector2_t& b0, const vector2_t& b1, const real_t& dx) { if ((a0.x == b0.x) && (a1.x == b1.x) && (a0.x == a1.x)) { return TestEdgeOverlap1D(a0.y, a1.y, b0.y, b1.y, dx); } const vector2_t amin = a0.x < a1.x ? a0 : a1; const vector2_t amax = a0.x < a1.x ? a1 : a0; const vector2_t bmin = b0.x < b1.x ? b0 : b1; const vector2_t bmax = b0.x < b1.x ? b1 : b0; const real_t ominx = std::max(amin.x, bmin.x) + dx; const real_t omaxx = std::min(amax.x, bmax.x) - dx; if (ominx >= omaxx) { return false; } const real_t aslope = ((amax.x - amin.x) != 0) ? ((amax.y - amin.y) / (amax.x - amin.x)) : 0; const real_t a_cValue = amin.y - aslope * amin.x; const real_t bslope = ((bmax.x - bmin.x) != 0) ? ((bmax.y - bmin.y) / (bmax.x - bmin.x)) : 0; const real_t b_cValue = bmin.y - bslope * bmin.x; const real_t aominy = a_cValue + aslope * ominx; const real_t bominy = b_cValue + bslope * ominx; const real_t aomaxy = a_cValue + aslope * omaxx; const real_t bomaxy = b_cValue + bslope * omaxx; const real_t dminy = bominy - aominy; const real_t dmaxy = bomaxy - aomaxy; if ((square(dminy) > toleranceSq) || (square(dmaxy) > toleranceSq)) { return false; } return true; } bool TestEdgeOverlap(size_t side, const real_t& toleranceSq, const vector3_t& a0, const vector3_t& a1, const vector3_t& b0, const vector3_t& b1) { const int ox = NeighbourOffset_MeshGrid[side][0]; const int oy = NeighbourOffset_MeshGrid[side][1]; const real_t dx = real_t::fraction(1, 1000); if (ox || oy) { const size_t dim = ox ? 0 : 1; const size_t odim = dim ^ 1; if ((a1[dim] - a0[dim] != 0) || (b1[dim] - b0[dim] != 0) || (a0[dim] != b0[dim])) { return false; } return TestEdgeOverlap2D(toleranceSq, vector2_t(a0[odim], a0.z), vector2_t(a1[odim], a1.z), vector2_t(b0[odim], b0.z), vector2_t(b1[odim], b1.z), dx); } else { if ((a1.z - a0.z != 0) || (b1.z - b0.z != 0) || (a0.z != b0.z)) { return false; } return TestEdgeOverlap2D(toleranceSq, vector2_t(a0.x, a0.y), vector2_t(a1.x, a1.y), vector2_t(b0.x, b0.y), vector2_t(b1.x, b1.y), dx); } } TileID MeshGrid::SetTile(size_t x, size_t y, size_t z, Tile& tile) { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); assert((x <= max_x) && (y <= max_y) && (z <= max_z)); const size_t tileName = ComputeTileName(x, y, z); std::pair iresult = m_tileMap.insert(TileMap::value_type((uint32)tileName, 0)); size_t tileID; if (iresult.second) { ++m_tileCount; if (m_frees.empty()) { tileID = m_tileCount; if (m_tileCount > m_tileCapacity) { Grow(std::max(4, m_tileCapacity >> 1)); } } else { tileID = m_frees.back() + 1; m_frees.pop_back(); } iresult.first->second = tileID; m_profiler.AddStat(TileCount, 1); } else { tileID = iresult.first->second; Tile& oldTile = m_tiles[tileID - 1].tile; m_triangleCount -= oldTile.triangleCount; m_profiler.AddStat(VertexCount, -(int)oldTile.vertexCount); m_profiler.AddStat(TriangleCount, -(int)oldTile.triangleCount); m_profiler.AddStat(BVTreeNodeCount, -(int)oldTile.nodeCount); m_profiler.AddStat(LinkCount, -(int)oldTile.linkCount); m_profiler.FreeMemory(VertexMemory, oldTile.vertexCount * sizeof(Tile::Vertex)); m_profiler.FreeMemory(TriangleMemory, oldTile.triangleCount * sizeof(Tile::Triangle)); m_profiler.FreeMemory(BVTreeMemory, oldTile.nodeCount * sizeof(Tile::BVNode)); m_profiler.FreeMemory(LinkMemory, oldTile.linkCount * sizeof(Tile::Link)); oldTile.Destroy(); } m_profiler.AddStat(VertexCount, tile.vertexCount); m_profiler.AddStat(TriangleCount, tile.triangleCount); m_profiler.AddStat(BVTreeNodeCount, tile.nodeCount); m_profiler.AddStat(LinkCount, tile.linkCount); m_profiler.AddMemory(VertexMemory, tile.vertexCount * sizeof(Tile::Vertex)); m_profiler.AddMemory(TriangleMemory, tile.triangleCount * sizeof(Tile::Triangle)); m_profiler.AddMemory(BVTreeMemory, tile.nodeCount * sizeof(Tile::BVNode)); m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link)); m_profiler.FreeMemory(GridMemory, m_profiler[GridMemory].used); m_profiler.AddMemory(GridMemory, m_tileMap.size() * sizeof(TileMap::value_type)); m_profiler.AddMemory(GridMemory, m_tileCapacity * sizeof(TileContainer)); m_profiler.AddMemory(GridMemory, m_frees.capacity() * sizeof(Frees::value_type)); m_triangleCount += tile.triangleCount; TileContainer& container = m_tiles[tileID - 1]; container.x = x; container.y = y; container.z = z; container.tile.Swap(tile); tile.Destroy(); return tileID; } void MeshGrid::ClearTile(TileID tileID, bool clearNetwork) { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); assert((tileID > 0) && (tileID <= m_tileCapacity)); if ((tileID > 0) && (tileID <= m_tileCapacity)) { TileContainer& container = m_tiles[tileID - 1]; m_profiler.AddStat(VertexCount, -(int)container.tile.vertexCount); m_profiler.AddStat(TriangleCount, -(int)container.tile.triangleCount); m_profiler.AddStat(BVTreeNodeCount, -(int)container.tile.nodeCount); m_profiler.AddStat(LinkCount, -(int)container.tile.linkCount); m_profiler.FreeMemory(VertexMemory, container.tile.vertexCount * sizeof(Tile::Vertex)); m_profiler.FreeMemory(TriangleMemory, container.tile.triangleCount * sizeof(Tile::Triangle)); m_profiler.FreeMemory(BVTreeMemory, container.tile.nodeCount * sizeof(Tile::BVNode)); m_profiler.FreeMemory(LinkMemory, container.tile.linkCount * sizeof(Tile::Link)); m_profiler.AddStat(TileCount, -1); m_triangleCount -= container.tile.triangleCount; m_frees.push_back(tileID - 1); --m_tileCount; container.tile.Destroy(); TileMap::iterator it = m_tileMap.find(ComputeTileName(container.x, container.y, container.z)); assert(it != m_tileMap.end()); m_tileMap.erase(it); if (clearNetwork) { for (size_t side = 0; side < SideCount; ++side) { size_t nx = container.x + NeighbourOffset_MeshGrid[side][0]; size_t ny = container.y + NeighbourOffset_MeshGrid[side][1]; size_t nz = container.z + NeighbourOffset_MeshGrid[side][2]; if (TileID neighbourID = GetTileID(nx, ny, nz)) { TileContainer& ncontainer = m_tiles[neighbourID - 1]; ReComputeAdjacency(ncontainer.x, ncontainer.y, ncontainer.z, kAdjecencyCalculationToleranceSq, ncontainer.tile, OppositeSide(side), container.x, container.y, container.z, tileID); } } } m_profiler.FreeMemory(GridMemory, m_profiler[GridMemory].used); m_profiler.AddMemory(GridMemory, m_tileMap.size() * sizeof(TileMap::value_type)); m_profiler.AddMemory(GridMemory, m_tileCapacity * sizeof(TileContainer)); m_profiler.AddMemory(GridMemory, m_frees.capacity() * sizeof(Frees::value_type)); } } void MeshGrid::CreateNetwork() { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); TileMap::iterator it = m_tileMap.begin(); TileMap::iterator end = m_tileMap.end(); const real_t toleranceSq = square(real_t(std::max(m_params.voxelSize.x, m_params.voxelSize.z))); for (; it != end; ++it) { const TileID tileID = it->second; TileContainer& container = m_tiles[tileID - 1]; ComputeAdjacency(container.x, container.y, container.z, toleranceSq, container.tile); } } void MeshGrid::ConnectToNetwork(TileID tileID) { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); assert((tileID > 0) && (tileID <= m_tileCapacity)); if ((tileID > 0) && (tileID <= m_tileCapacity)) { TileContainer& container = m_tiles[tileID - 1]; ComputeAdjacency(container.x, container.y, container.z, kAdjecencyCalculationToleranceSq, container.tile); for (size_t side = 0; side < SideCount; ++side) { size_t nx = container.x + NeighbourOffset_MeshGrid[side][0]; size_t ny = container.y + NeighbourOffset_MeshGrid[side][1]; size_t nz = container.z + NeighbourOffset_MeshGrid[side][2]; if (TileID neighbourID = GetTileID(nx, ny, nz)) { TileContainer& ncontainer = m_tiles[neighbourID - 1]; ReComputeAdjacency(ncontainer.x, ncontainer.y, ncontainer.z, kAdjecencyCalculationToleranceSq, ncontainer.tile, OppositeSide(side), container.x, container.y, container.z, tileID); } } } } TileID MeshGrid::GetTileID(size_t x, size_t y, size_t z) const { const size_t tileName = ComputeTileName(x, y, z); TileMap::const_iterator it = m_tileMap.find(tileName); if (it != m_tileMap.end()) { return it->second; } return 0; } const Tile& MeshGrid::GetTile(TileID tileID) const { assert(tileID > 0); assert(tileID <= m_tileCapacity); return m_tiles[tileID - 1].tile; } Tile& MeshGrid::GetTile(TileID tileID) { assert(tileID > 0); assert(tileID <= m_tileCapacity); return m_tiles[tileID - 1].tile; } const vector3_t MeshGrid::GetTileContainerCoordinates(TileID tileID) const { assert(tileID > 0); assert(tileID <= m_tileCapacity); const TileContainer& container = m_tiles[tileID - 1]; return vector3_t(MNM::real_t(container.x), MNM::real_t(container.y), MNM::real_t(container.z)); } void MeshGrid::Swap(MeshGrid& other) { std::swap(m_tiles, other.m_tiles); std::swap(m_tileCount, other.m_tileCount); std::swap(m_tileCapacity, other.m_tileCapacity); std::swap(m_triangleCount, other.m_triangleCount); m_frees.swap(other.m_frees); m_tileMap.swap(other.m_tileMap); std::swap(m_params, other.m_params); std::swap(m_profiler, other.m_profiler); } void MeshGrid::Draw(size_t drawFlags, TileID excludeID) const { TileMap::const_iterator it = m_tileMap.begin(); TileMap::const_iterator end = m_tileMap.end(); // collect areas // TODO: Clean this up! Temporary to get up and running. std::vector islandAreas(m_islands.size()); for (size_t i = 0; i < m_islands.size(); ++i) { islandAreas[i] = m_islands[i].area; } for (; it != end; ++it) { if (excludeID == it->second) { continue; } const TileContainer& container = m_tiles[it->second - 1]; container.tile.Draw(drawFlags, vector3_t( real_t(m_params.origin.x + container.x * m_params.tileSize.x), real_t(m_params.origin.y + container.y * m_params.tileSize.y), real_t(m_params.origin.z + container.z * m_params.tileSize.z)), it->second, islandAreas); } } const MeshGrid::ProfilerType& MeshGrid::GetProfiler() const { return m_profiler; } struct Edge { uint16 vertex[2]; uint16 triangle[2]; }; void ComputeTileTriangleAdjacency(Tile::Triangle* triangles, size_t triangleCount, size_t vertexCount, Edge* edges, uint16* adjacency) { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); { enum { Unused = 0xffff, }; const size_t MaxLookUp = 4096; uint16 edgeLookUp[MaxLookUp]; assert(MaxLookUp > vertexCount + triangleCount * 3); std::fill(&edgeLookUp[0], &edgeLookUp[0] + vertexCount, static_cast(Unused)); size_t edgeCount = 0; for (size_t i = 0; i < triangleCount; ++i) { const Tile::Triangle& triangle = triangles[i]; for (size_t v = 0; v < 3; ++v) { uint16 i1 = triangle.vertex[v]; uint16 i2 = triangle.vertex[next_mod3(v)]; if (i1 < i2) { size_t edgeIdx = edgeCount++; adjacency[i * 3 + v] = edgeIdx; Edge& edge = edges[edgeIdx]; edge.triangle[0] = i; edge.triangle[1] = i; edge.vertex[0] = i1; edge.vertex[1] = i2; edgeLookUp[vertexCount + edgeIdx] = edgeLookUp[i1]; edgeLookUp[i1] = edgeIdx; } } } for (size_t i = 0; i < triangleCount; ++i) { const Tile::Triangle& triangle = triangles[i]; for (size_t v = 0; v < 3; ++v) { uint16 i1 = triangle.vertex[v]; uint16 i2 = triangle.vertex[next_mod3(v)]; if (i1 > i2) { size_t edgeIndex = edgeLookUp[i2]; for (; edgeIndex != Unused; edgeIndex = edgeLookUp[vertexCount + edgeIndex]) { Edge& edge = edges[edgeIndex]; if ((edge.vertex[1] == i1) && (edge.triangle[0] == edge.triangle[1])) { edge.triangle[1] = i; adjacency[i * 3 + v] = edgeIndex; break; } } if (edgeIndex == Unused) { size_t edgeIdx = edgeCount++; adjacency[i * 3 + v] = edgeIdx; Edge& edge = edges[edgeIdx]; edge.vertex[0] = i1; edge.vertex[1] = i2; edge.triangle[0] = i; edge.triangle[1] = i; } } } } } } void MeshGrid::Grow(size_t amount) { const size_t oldCapacity = m_tileCapacity; m_tileCapacity = m_tileCapacity + amount; TileContainer* tiles = new TileContainer[m_tileCapacity]; if (oldCapacity) { memcpy(tiles, m_tiles, oldCapacity * sizeof(TileContainer)); } std::swap(m_tiles, tiles); delete[] tiles; } TileID MeshGrid::GetNeighbourTileID(size_t x, size_t y, size_t z, size_t side) const { size_t nx = x + NeighbourOffset_MeshGrid[side][0]; size_t ny = y + NeighbourOffset_MeshGrid[side][1]; size_t nz = z + NeighbourOffset_MeshGrid[side][2]; return GetTileID(nx, ny, nz); } static const size_t MaxTriangleCount = 1024; struct SideTileInfo { SideTileInfo() : tile(0) { } Tile* tile; vector3_t offset; }; #pragma warning (push) #pragma warning (disable: 6262) void MeshGrid::ComputeAdjacency(size_t x, size_t y, size_t z, const real_t& toleranceSq, Tile& tile) { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); const size_t vertexCount = tile.vertexCount; const Tile::Vertex* vertices = tile.vertices; const size_t triCount = tile.triangleCount; if (!triCount) { return; } m_profiler.StartTimer(NetworkConstruction); assert(triCount <= MaxTriangleCount); Edge edges[MaxTriangleCount * 3]; uint16 adjacency[MaxTriangleCount * 3]; ComputeTileTriangleAdjacency(tile.triangles, triCount, vertexCount, edges, adjacency); const size_t MaxLinkCount = MaxTriangleCount * 6; Tile::Link links[MaxLinkCount]; size_t linkCount = 0; SideTileInfo sides[SideCount]; for (size_t s = 0; s < SideCount; ++s) { SideTileInfo& side = sides[s]; if (TileID id = GetTileID(x + NeighbourOffset_MeshGrid[s][0], y + NeighbourOffset_MeshGrid[s][1], z + NeighbourOffset_MeshGrid[s][2])) { side.tile = &GetTile(id); side.offset = vector3_t( NeighbourOffset_MeshGrid[s][0] * m_params.tileSize.x, NeighbourOffset_MeshGrid[s][1] * m_params.tileSize.y, NeighbourOffset_MeshGrid[s][2] * m_params.tileSize.z); } } for (size_t i = 0; i < triCount; ++i) { size_t triLinkCount = 0; for (size_t e = 0; e < 3; ++e) { const size_t edgeIndex = adjacency[i * 3 + e]; const Edge& edge = edges[edgeIndex]; if ((edge.triangle[0] != i) && (edge.triangle[1] != i)) { continue; } if (edge.triangle[0] != edge.triangle[1]) { Tile::Link& link = links[linkCount++]; link.side = Tile::Link::Internal; link.edge = e; link.triangle = (edge.triangle[1] == i) ? edge.triangle[0] : edge.triangle[1]; ++triLinkCount; } } if (triLinkCount == 3) { Tile::Triangle& triangle = tile.triangles[i]; triangle.linkCount = triLinkCount; triangle.firstLink = linkCount - triLinkCount; continue; } for (size_t e = 0; e < 3; ++e) { const size_t edgeIndex = adjacency[i * 3 + e]; const Edge& edge = edges[edgeIndex]; if ((edge.triangle[0] != i) && (edge.triangle[1] != i)) { continue; } if (edge.triangle[0] != edge.triangle[1]) { continue; } const vector3_t a0 = vector3_t(vertices[edge.vertex[0]]); const vector3_t a1 = vector3_t(vertices[edge.vertex[1]]); for (size_t s = 0; s < SideCount; ++s) { if (const Tile* stile = sides[s].tile) { const vector3_t& offset = sides[s].offset; const size_t striangleCount = stile->triangleCount; const Tile::Triangle* striangles = stile->triangles; const Tile::Vertex* svertices = stile->vertices; for (size_t k = 0; k < striangleCount; ++k) { const Tile::Triangle& striangle = striangles[k]; for (size_t ne = 0; ne < 3; ++ne) { const vector3_t b0 = offset + vector3_t(svertices[striangle.vertex[ne]]); const vector3_t b1 = offset + vector3_t(svertices[striangle.vertex[next_mod3(ne)]]); if (TestEdgeOverlap(s, toleranceSq, a0, a1, b0, b1)) { Tile::Link& link = links[linkCount++]; link.side = s; link.edge = e; link.triangle = k; #if DEBUG_MNM_DATA_CONSISTENCY_ENABLED const TileID checkId = GetTileID(x + NeighbourOffset_MeshGrid[s][0], y + NeighbourOffset_MeshGrid[s][1], z + NeighbourOffset_MeshGrid[s][2]); BreakOnInvalidTriangle(ComputeTriangleID(checkId, k), m_tileCapacity); #endif ++triLinkCount; break; } } } } } } Tile::Triangle& triangle = tile.triangles[i]; triangle.linkCount = triLinkCount; triangle.firstLink = linkCount - triLinkCount; } m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link)); m_profiler.AddStat(LinkCount, -(int)tile.linkCount); tile.CopyLinks(links, linkCount); m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link)); m_profiler.AddStat(LinkCount, tile.linkCount); m_profiler.StopTimer(NetworkConstruction); } #pragma warning (pop) void MeshGrid::ReComputeAdjacency(size_t x, size_t y, size_t z, const real_t& toleranceSq, Tile& tile, size_t side, size_t tx, size_t ty, size_t tz, TileID targetID) { FUNCTION_PROFILER(gEnv->pSystem, PROFILE_AI); const Tile::Vertex* vertices = tile.vertices; const size_t originTriangleCount = tile.triangleCount; if (!originTriangleCount) { return; } m_profiler.StartTimer(NetworkConstruction); if (!tile.linkCount) { ComputeAdjacency(x, y, z, toleranceSq, tile); } else { const vector3_t noffset = vector3_t( NeighbourOffset_MeshGrid[side][0] * m_params.tileSize.x, NeighbourOffset_MeshGrid[side][1] * m_params.tileSize.y, NeighbourOffset_MeshGrid[side][2] * m_params.tileSize.z); assert(originTriangleCount <= MaxTriangleCount); const size_t MaxLinkCount = MaxTriangleCount * 6; Tile::Link links[MaxLinkCount]; size_t linkCount = 0; const Tile* targetTile = targetID ? &m_tiles[targetID - 1].tile : 0; const Tile::Vertex* targetVertices = targetTile ? targetTile->vertices : 0; const size_t targetTriangleCount = targetTile ? targetTile->triangleCount : 0; const Tile::Triangle* targetTriangles = targetTile ? targetTile->triangles : 0; for (size_t i = 0; i < originTriangleCount; ++i) { size_t triLinkCount = 0; size_t triLinkCountI = 0; Tile::Triangle& originTriangle = tile.triangles[i]; const size_t originLinkCount = originTriangle.linkCount; const size_t originFirstLink = originTriangle.firstLink; for (size_t l = 0; l < originLinkCount; ++l) { const Tile::Link& originLink = tile.links[originFirstLink + l]; if ((originLink.side == Tile::Link::Internal) || (originLink.side != side)) { links[linkCount++] = originLink; ++triLinkCount; triLinkCountI += (originLink.side == Tile::Link::Internal) ? 1 : 0; } } if ((triLinkCountI == 3) || !targetID) { originTriangle.linkCount = triLinkCount; originTriangle.firstLink = linkCount - triLinkCount; continue; } for (size_t e = 0; e < 3; ++e) { const vector3_t a0 = vector3_t(vertices[originTriangle.vertex[e]]); const vector3_t a1 = vector3_t(vertices[originTriangle.vertex[next_mod3(e)]]); for (size_t k = 0; k < targetTriangleCount; ++k) { const Tile::Triangle& ttriangle = targetTriangles[k]; for (size_t ne = 0; ne < 3; ++ne) { const vector3_t b0 = noffset + vector3_t(targetVertices[ttriangle.vertex[ne]]); const vector3_t b1 = noffset + vector3_t(targetVertices[ttriangle.vertex[next_mod3(ne)]]); if (TestEdgeOverlap(side, toleranceSq, a0, a1, b0, b1)) { Tile::Link& link = links[linkCount++]; link.side = side; link.edge = e; link.triangle = k; ++triLinkCount; #if DEBUG_MNM_DATA_CONSISTENCY_ENABLED const TileID checkId = GetTileID(x + NeighbourOffset_MeshGrid[side][0], y + NeighbourOffset_MeshGrid[side][1], z + NeighbourOffset_MeshGrid[side][2]); BreakOnInvalidTriangle(ComputeTriangleID(checkId, k), m_tileCapacity); /* Tile::Link testLink; testLink.side = side; testLink.edge = e; testLink.triangle = k; BreakOnMultipleAdjacencyLinkage(links, links+linkCount, testLink); */ #endif break; } } } } originTriangle.linkCount = triLinkCount; originTriangle.firstLink = linkCount - triLinkCount; } m_profiler.FreeMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link)); m_profiler.AddStat(LinkCount, -(int)tile.linkCount); tile.CopyLinks(links, linkCount); m_profiler.AddMemory(LinkMemory, tile.linkCount * sizeof(Tile::Link)); m_profiler.AddStat(LinkCount, tile.linkCount); } m_profiler.StopTimer(NetworkConstruction); } bool MeshGrid::CalculateMidEdge(const TriangleID triangleID1, const TriangleID triangleID2, Vec3& result) const { if (triangleID1 == triangleID2) { return false; } if (const TileID tileID = ComputeTileID(triangleID1)) { const TileContainer& container = m_tiles[tileID - 1]; const Tile& tile = container.tile; const uint16 triangleIdx = ComputeTriangleIndex(triangleID1); const Tile::Triangle& triangle = tile.triangles[triangleIdx]; uint16 vi0 = 0, vi1 = 0; for (size_t l = 0; l < triangle.linkCount; ++l) { const Tile::Link& link = tile.links[triangle.firstLink + l]; if (link.side == Tile::Link::Internal) { TriangleID linkedTriID = ComputeTriangleID(tileID, link.triangle); if (linkedTriID == triangleID2) { vi0 = link.edge; vi1 = (link.edge + 1) % 3; assert(vi0 < 3); assert(vi1 < 3); break; } } else if (link.side != Tile::Link::OffMesh) { TileID neighbourTileID = GetNeighbourTileID(container.x, container.y, container.z, link.side); TriangleID linkedTriID = ComputeTriangleID(neighbourTileID, link.triangle); if (linkedTriID == triangleID2) { vi0 = link.edge; vi1 = (link.edge + 1) % 3; assert(vi0 < 3); assert(vi1 < 3); break; } } } if (vi0 != vi1) { vector3_t v0, v1, v2; GetVertices(triangleID1, v0, v1, v2); vector3_t vertices[3] = {v0, v1, v2}; PREFAST_ASSUME(vi0 < 3 && vi1 < 3); result = (vertices[vi0] + vertices[vi1]).GetVec3() * 0.5f; return true; } } return false; } ////////////////////////////////////////////////////////////////////////// #if MNM_USE_EXPORT_INFORMATION void MeshGrid::ResetAccessibility(uint8 accessible) { for (TileMap::iterator tileIt = m_tileMap.begin(); tileIt != m_tileMap.end(); ++tileIt) { Tile& tile = m_tiles[tileIt->second - 1].tile; tile.ResetConnectivity(accessible); } } void MeshGrid::ComputeAccessibility(const AccessibilityRequest& inputRequest) { struct Node { Node (TriangleID _id, TriangleID _previousID) : id(_id) , previousId(_previousID) { } TriangleID id; TriangleID previousId; }; std::vector nextTriangles; nextTriangles.reserve(16); std::vector openNodes; openNodes.reserve(m_triangleCount); if (const TileID startTileID = ComputeTileID(inputRequest.fromTriangle)) { const uint16 startTriangleIdx = ComputeTriangleIndex(inputRequest.fromTriangle); Tile& startTile = m_tiles[startTileID - 1].tile; startTile.SetTriangleAccessible(startTriangleIdx); openNodes.push_back(Node(inputRequest.fromTriangle, 0)); while (!openNodes.empty()) { const Node currentNode = openNodes.back(); openNodes.pop_back(); if (const TileID tileID = ComputeTileID(currentNode.id)) { const uint16 triangleIdx = ComputeTriangleIndex(currentNode.id); TileContainer& container = m_tiles[tileID - 1]; Tile& tile = container.tile; const Tile::Triangle& triangle = tile.triangles[triangleIdx]; nextTriangles.clear(); // Collect all accessible nodes from the current one for (size_t l = 0; l < triangle.linkCount; ++l) { const Tile::Link& link = tile.links[triangle.firstLink + l]; TriangleID nextTriangleID; if (link.side == Tile::Link::Internal) { nextTriangleID = ComputeTriangleID(tileID, link.triangle); } else if (link.side != Tile::Link::OffMesh) { TileID neighbourTileID = GetNeighbourTileID(container.x, container.y, container.z, link.side); nextTriangleID = ComputeTriangleID(neighbourTileID, link.triangle); } else { OffMeshNavigation::QueryLinksResult links = inputRequest.offMeshNavigation.GetLinksForTriangle(currentNode.id, link.triangle); while (WayTriangleData nextLink = links.GetNextTriangle()) { nextTriangles.push_back(nextLink.triangleID); } continue; } nextTriangles.push_back(nextTriangleID); } // Add them to the open list if not visited already for (size_t t = 0; t < nextTriangles.size(); ++t) { const TriangleID nextTriangleID = nextTriangles[t]; // Skip if previous triangle if (nextTriangleID == currentNode.previousId) { continue; } // Skip if already visited if (const TileID nextTileID = ComputeTileID(nextTriangleID)) { Tile& nextTile = m_tiles[nextTileID - 1].tile; const uint16 nextTriangleIndex = ComputeTriangleIndex(nextTriangleID); if (nextTile.IsTriangleAccessible(nextTriangleIndex)) { continue; } nextTile.SetTriangleAccessible(nextTriangleIndex); openNodes.push_back(Node(nextTriangleID, currentNode.id)); } } } } } } #endif }