static CFixedVector2D NearestPointOnGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal) { CFixedVector2D g(goal.x, goal.z); switch (goal.type) { case CCmpPathfinder::Goal::POINT: { return g; } case CCmpPathfinder::Goal::CIRCLE: { CFixedVector2D d = pos - g; if (d.IsZero()) d = CFixedVector2D(fixed::FromInt(1), fixed::Zero()); // some arbitrary direction d.Normalize(goal.hw); return g + d; } case CCmpPathfinder::Goal::SQUARE: { CFixedVector2D halfSize(goal.hw, goal.hh); CFixedVector2D d = pos - g; return g + Geometry::NearestPointOnSquare(d, goal.u, goal.v, halfSize); } default: debug_warn(L"invalid type"); return CFixedVector2D(); } }
virtual CFixedVector2D GetPreviousPosition2D() { if (!m_InWorld) { LOGERROR(L"CCmpPosition::GetPreviousPosition2D called on entity when IsInWorld is false"); return CFixedVector2D(); } return CFixedVector2D(m_PrevX, m_PrevZ); }
virtual void UpdateTurretPosition() { if (m_TurretParent == INVALID_ENTITY) return; CmpPtr<ICmpPosition> cmpPosition(GetSimContext(), m_TurretParent); if (!cmpPosition) { LOGERROR("Turret with parent without position component"); return; } if (!cmpPosition->IsInWorld()) MoveOutOfWorld(); else { CFixedVector2D rotatedPosition = CFixedVector2D(m_TurretPosition.X, m_TurretPosition.Z); rotatedPosition = rotatedPosition.Rotate(cmpPosition->GetRotation().Y); CFixedVector2D rootPosition = cmpPosition->GetPosition2D(); entity_pos_t x = rootPosition.X + rotatedPosition.X; entity_pos_t z = rootPosition.Y + rotatedPosition.Y; if (!m_InWorld || m_X != x || m_Z != z) MoveTo(x, z); entity_pos_t y = cmpPosition->GetHeightOffset() + m_TurretPosition.Y; if (!m_InWorld || GetHeightOffset() != y) SetHeightOffset(y); m_InWorld = true; } }
virtual entity_pos_t GetSize() { if (m_Type == UNIT) return m_Size0; else return CFixedVector2D(m_Size0 / 2, m_Size1 / 2).Length(); }
ICmpObstruction::EFoundationCheck CCmpPathfinder::CheckBuildingPlacement(const IObstructionTestFilter& filter, entity_pos_t x, entity_pos_t z, entity_pos_t a, entity_pos_t w, entity_pos_t h, entity_id_t id, pass_class_t passClass, bool onlyCenterPoint) { // Check unit obstruction CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY); if (!cmpObstructionManager) return ICmpObstruction::FOUNDATION_CHECK_FAIL_ERROR; if (cmpObstructionManager->TestStaticShape(filter, x, z, a, w, h, NULL)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_OBSTRUCTS_FOUNDATION; // Test against terrain: UpdateGrid(); ICmpObstructionManager::ObstructionSquare square; CmpPtr<ICmpObstruction> cmpObstruction(GetSimContext(), id); if (!cmpObstruction || !cmpObstruction->GetObstructionSquare(square)) return ICmpObstruction::FOUNDATION_CHECK_FAIL_NO_OBSTRUCTION; if (onlyCenterPoint) { u16 i, j; NearestTile(x, z, i, j); if (IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass)) return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; } // Expand bounds by 1/sqrt(2) tile (multiply by TERRAIN_TILE_SIZE since we want world coordinates) entity_pos_t expand = entity_pos_t::FromInt(2).Sqrt().Multiply(entity_pos_t::FromInt(TERRAIN_TILE_SIZE / 2)); CFixedVector2D halfSize(square.hw + expand, square.hh + expand); CFixedVector2D halfBound = Geometry::GetHalfBoundingBox(square.u, square.v, halfSize); u16 i0, j0, i1, j1; NearestTile(square.x - halfBound.X, square.z - halfBound.Y, i0, j0); NearestTile(square.x + halfBound.X, square.z + halfBound.Y, i1, j1); for (u16 j = j0; j <= j1; ++j) { for (u16 i = i0; i <= i1; ++i) { entity_pos_t x, z; TileCenter(i, j, x, z); if (Geometry::PointIsInSquare(CFixedVector2D(x - square.x, z - square.z), square.u, square.v, halfSize) && !IS_TERRAIN_PASSABLE(m_Grid->get(i,j), passClass)) { return ICmpObstruction::FOUNDATION_CHECK_FAIL_TERRAIN_CLASS; } } } return ICmpObstruction::FOUNDATION_CHECK_SUCCESS; }
virtual fixed GetDistanceTravelled() { if (!m_InWorld) { LOGERROR(L"CCmpPosition::GetDistanceTravelled called on entity when IsInWorld is false"); return fixed::Zero(); } return CFixedVector2D(m_X - m_LastX, m_Z - m_LastZ).Length(); }
static bool AtGoal(u16 i, u16 j, const ICmpPathfinder::Goal& goal) { // Allow tiles slightly more than sqrt(2) from the actual goal, // i.e. adjacent diagonally to the target tile fixed tolerance = entity_pos_t::FromInt(CELL_SIZE*3/2); entity_pos_t x, z; CCmpPathfinder::TileCenter(i, j, x, z); fixed dist = CCmpPathfinder::DistanceToGoal(CFixedVector2D(x, z), goal); return (dist < tolerance); }
fixed CCmpPathfinder::DistanceToGoal(CFixedVector2D pos, const CCmpPathfinder::Goal& goal) { switch (goal.type) { case CCmpPathfinder::Goal::POINT: return (pos - CFixedVector2D(goal.x, goal.z)).Length(); case CCmpPathfinder::Goal::CIRCLE: return ((pos - CFixedVector2D(goal.x, goal.z)).Length() - goal.hw).Absolute(); case CCmpPathfinder::Goal::SQUARE: { CFixedVector2D halfSize(goal.hw, goal.hh); CFixedVector2D d(pos.X - goal.x, pos.Y - goal.z); return Geometry::DistanceToSquare(d, goal.u, goal.v, halfSize); } default: debug_warn(L"invalid type"); return fixed::Zero(); } }
virtual void Init(const CParamNode& paramNode) { // The minimum obstruction size is the navcell size * sqrt(2) // This is enforced in the schema as a minimum of 1.5 fixed minObstruction = (Pathfinding::NAVCELL_SIZE.Square() * 2).Sqrt(); m_TemplateFlags = 0; if (paramNode.GetChild("BlockMovement").ToBool()) m_TemplateFlags |= ICmpObstructionManager::FLAG_BLOCK_MOVEMENT; if (paramNode.GetChild("BlockPathfinding").ToBool()) m_TemplateFlags |= ICmpObstructionManager::FLAG_BLOCK_PATHFINDING; if (paramNode.GetChild("BlockFoundation").ToBool()) m_TemplateFlags |= ICmpObstructionManager::FLAG_BLOCK_FOUNDATION; if (paramNode.GetChild("BlockConstruction").ToBool()) m_TemplateFlags |= ICmpObstructionManager::FLAG_BLOCK_CONSTRUCTION; m_Flags = m_TemplateFlags; if (paramNode.GetChild("DisableBlockMovement").ToBool()) m_Flags &= (flags_t)(~ICmpObstructionManager::FLAG_BLOCK_MOVEMENT); if (paramNode.GetChild("DisableBlockPathfinding").ToBool()) m_Flags &= (flags_t)(~ICmpObstructionManager::FLAG_BLOCK_PATHFINDING); if (paramNode.GetChild("Unit").IsOk()) { m_Type = UNIT; m_Size0 = m_Size1 = paramNode.GetChild("Unit").GetChild("@radius").ToFixed(); } else if (paramNode.GetChild("Static").IsOk()) { m_Type = STATIC; m_Size0 = paramNode.GetChild("Static").GetChild("@width").ToFixed(); m_Size1 = paramNode.GetChild("Static").GetChild("@depth").ToFixed(); ENSURE(m_Size0 > minObstruction); ENSURE(m_Size1 > minObstruction); } else { m_Type = CLUSTER; CFixedVector2D max = CFixedVector2D(fixed::FromInt(0), fixed::FromInt(0)); CFixedVector2D min = CFixedVector2D(fixed::FromInt(0), fixed::FromInt(0)); const CParamNode::ChildrenMap& clusterMap = paramNode.GetChild("Obstructions").GetChildren(); for(CParamNode::ChildrenMap::const_iterator it = clusterMap.begin(); it != clusterMap.end(); ++it) { Shape b; b.size0 = it->second.GetChild("@width").ToFixed(); b.size1 = it->second.GetChild("@depth").ToFixed(); ENSURE(b.size0 > minObstruction); ENSURE(b.size1 > minObstruction); b.dx = it->second.GetChild("@x").ToFixed(); b.dz = it->second.GetChild("@z").ToFixed(); b.da = entity_angle_t::FromInt(0); b.flags = m_Flags; m_Shapes.push_back(b); max.X = MAX(max.X, b.dx + b.size0/2); max.Y = MAX(max.Y, b.dz + b.size1/2); min.X = MIN(min.X, b.dx - b.size0/2); min.Y = MIN(min.Y, b.dz - b.size1/2); } m_Size0 = fixed::FromInt(2).Multiply(MAX(max.X, -min.X)); m_Size1 = fixed::FromInt(2).Multiply(MAX(max.Y, -min.Y)); } m_Active = paramNode.GetChild("Active").ToBool(); m_ControlPersist = paramNode.GetChild("ControlPersist").IsOk(); m_Tag = tag_t(); if (m_Type == CLUSTER) m_ClusterTags.clear(); m_Moving = false; m_ControlGroup = GetEntityId(); m_ControlGroup2 = INVALID_ENTITY; }
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter, entity_pos_t x0, entity_pos_t z0, entity_pos_t r, entity_pos_t range, const Goal& goal, pass_class_t passClass, Path& path) { UpdateGrid(); // TODO: only need to bother updating if the terrain changed PROFILE("ComputeShortPath"); // ScopeTimer UID__(L"ComputeShortPath"); m_DebugOverlayShortPathLines.clear(); if (m_DebugOverlay) { // Render the goal shape m_DebugOverlayShortPathLines.push_back(SOverlayLine()); m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1); switch (goal.type) { case CCmpPathfinder::Goal::POINT: { SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true); break; } case CCmpPathfinder::Goal::CIRCLE: { SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true); break; } case CCmpPathfinder::Goal::SQUARE: { float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat()); SimRender::ConstructSquareOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true); break; } } } // List of collision edges - paths must never cross these. // (Edges are one-sided so intersections are fine in one direction, but not the other direction.) std::vector<Edge> edges; std::vector<Edge> edgesAA; // axis-aligned squares // Create impassable edges at the max-range boundary, so we can't escape the region // where we're meant to be searching fixed rangeXMin = x0 - range; fixed rangeXMax = x0 + range; fixed rangeZMin = z0 - range; fixed rangeZMax = z0 + range; { // (The edges are the opposite direction to usual, so it's an inside-out square) Edge e0 = { CFixedVector2D(rangeXMin, rangeZMin), CFixedVector2D(rangeXMin, rangeZMax) }; Edge e1 = { CFixedVector2D(rangeXMin, rangeZMax), CFixedVector2D(rangeXMax, rangeZMax) }; Edge e2 = { CFixedVector2D(rangeXMax, rangeZMax), CFixedVector2D(rangeXMax, rangeZMin) }; Edge e3 = { CFixedVector2D(rangeXMax, rangeZMin), CFixedVector2D(rangeXMin, rangeZMin) }; edges.push_back(e0); edges.push_back(e1); edges.push_back(e2); edges.push_back(e3); } // List of obstruction vertexes (plus start/end points); we'll try to find paths through // the graph defined by these vertexes std::vector<Vertex> vertexes; // Add the start point to the graph CFixedVector2D posStart(x0, z0); fixed hStart = (posStart - NearestPointOnGoal(posStart, goal)).Length(); Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL }; vertexes.push_back(start); const size_t START_VERTEX_ID = 0; // Add the goal vertex to the graph. // Since the goal isn't always a point, this a special magic virtual vertex which moves around - whenever // we look at it from another vertex, it is moved to be the closest point on the goal shape to that vertex. Vertex end = { CFixedVector2D(goal.x, goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL }; vertexes.push_back(end); const size_t GOAL_VERTEX_ID = 1; // Add terrain obstructions { u16 i0, j0, i1, j1; NearestTile(rangeXMin, rangeZMin, i0, j0); NearestTile(rangeXMax, rangeZMax, i1, j1); AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid); } // Find all the obstruction squares that might affect us CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY); std::vector<ICmpObstructionManager::ObstructionSquare> squares; cmpObstructionManager->GetObstructionsInRange(filter, rangeXMin - r, rangeZMin - r, rangeXMax + r, rangeZMax + r, squares); // Resize arrays to reduce reallocations vertexes.reserve(vertexes.size() + squares.size()*4); edgesAA.reserve(edgesAA.size() + squares.size()); // (assume most squares are AA) // Convert each obstruction square into collision edges and search graph vertexes for (size_t i = 0; i < squares.size(); ++i) { CFixedVector2D center(squares[i].x, squares[i].z); CFixedVector2D u = squares[i].u; CFixedVector2D v = squares[i].v; // Expand the vertexes by the moving unit's collision radius, to find the // closest we can get to it CFixedVector2D hd0(squares[i].hw + r + EDGE_EXPAND_DELTA, squares[i].hh + r + EDGE_EXPAND_DELTA); CFixedVector2D hd1(squares[i].hw + r + EDGE_EXPAND_DELTA, -(squares[i].hh + r + EDGE_EXPAND_DELTA)); // Check whether this is an axis-aligned square bool aa = (u.X == fixed::FromInt(1) && u.Y == fixed::Zero() && v.X == fixed::Zero() && v.Y == fixed::FromInt(1)); Vertex vert; vert.status = Vertex::UNEXPLORED; vert.quadInward = QUADRANT_NONE; vert.quadOutward = QUADRANT_ALL; vert.p.X = center.X - hd0.Dot(u); vert.p.Y = center.Y + hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_BR; vertexes.push_back(vert); vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_TR; vertexes.push_back(vert); vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_TL; vertexes.push_back(vert); vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; vertexes.push_back(vert); // Add the edges: CFixedVector2D h0(squares[i].hw + r, squares[i].hh + r); CFixedVector2D h1(squares[i].hw + r, -(squares[i].hh + r)); CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v)); CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v)); CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v)); CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v)); if (aa) { Edge e = { ev1, ev3 }; edgesAA.push_back(e); } else { Edge e0 = { ev0, ev1 }; Edge e1 = { ev1, ev2 }; Edge e2 = { ev2, ev3 }; Edge e3 = { ev3, ev0 }; edges.push_back(e0); edges.push_back(e1); edges.push_back(e2); edges.push_back(e3); } // TODO: should clip out vertexes and edges that are outside the range, // to reduce the search space } ENSURE(vertexes.size() < 65536); // we store array indexes as u16 if (m_DebugOverlay) { // Render the obstruction edges for (size_t i = 0; i < edges.size(); ++i) { m_DebugOverlayShortPathLines.push_back(SOverlayLine()); m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); std::vector<float> xz; xz.push_back(edges[i].p0.X.ToFloat()); xz.push_back(edges[i].p0.Y.ToFloat()); xz.push_back(edges[i].p1.X.ToFloat()); xz.push_back(edges[i].p1.Y.ToFloat()); SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); } for (size_t i = 0; i < edgesAA.size(); ++i) { m_DebugOverlayShortPathLines.push_back(SOverlayLine()); m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1); std::vector<float> xz; xz.push_back(edgesAA[i].p0.X.ToFloat()); xz.push_back(edgesAA[i].p0.Y.ToFloat()); xz.push_back(edgesAA[i].p0.X.ToFloat()); xz.push_back(edgesAA[i].p1.Y.ToFloat()); xz.push_back(edgesAA[i].p1.X.ToFloat()); xz.push_back(edgesAA[i].p1.Y.ToFloat()); xz.push_back(edgesAA[i].p1.X.ToFloat()); xz.push_back(edgesAA[i].p0.Y.ToFloat()); xz.push_back(edgesAA[i].p0.X.ToFloat()); xz.push_back(edgesAA[i].p0.Y.ToFloat()); SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true); } } // Do an A* search over the vertex/visibility graph: // Since we are just measuring Euclidean distance the heuristic is admissible, // so we never have to re-examine a node once it's been moved to the closed set. // To save time in common cases, we don't precompute a graph of valid edges between vertexes; // we do it lazily instead. When the search algorithm reaches a vertex, we examine every other // vertex and see if we can reach it without hitting any collision edges, and ignore the ones // we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked // as closed), we won't be doing any redundant visibility computations. PROFILE_START("A*"); PriorityQueue open; PriorityQueue::Item qiStart = { START_VERTEX_ID, start.h }; open.push(qiStart); u16 idBest = START_VERTEX_ID; fixed hBest = start.h; while (!open.empty()) { // Move best tile from open to closed PriorityQueue::Item curr = open.pop(); vertexes[curr.id].status = Vertex::CLOSED; // If we've reached the destination, stop if (curr.id == GOAL_VERTEX_ID) { idBest = curr.id; break; } // Sort the edges so ones nearer this vertex are checked first by CheckVisibility, // since they're more likely to block the rays std::sort(edgesAA.begin(), edgesAA.end(), EdgeSort(vertexes[curr.id].p)); std::vector<EdgeAA> edgesLeft; std::vector<EdgeAA> edgesRight; std::vector<EdgeAA> edgesBottom; std::vector<EdgeAA> edgesTop; SplitAAEdges(vertexes[curr.id].p, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop); // Check the lines to every other vertex for (size_t n = 0; n < vertexes.size(); ++n) { if (vertexes[n].status == Vertex::CLOSED) continue; // If this is the magical goal vertex, move it to near the current vertex CFixedVector2D npos; if (n == GOAL_VERTEX_ID) { npos = NearestPointOnGoal(vertexes[curr.id].p, goal); // To prevent integer overflows later on, we need to ensure all vertexes are // 'close' to the source. The goal might be far away (not a good idea but // sometimes it happens), so clamp it to the current search range npos.X = clamp(npos.X, rangeXMin, rangeXMax); npos.Y = clamp(npos.Y, rangeZMin, rangeZMax); } else { npos = vertexes[n].p; } // Work out which quadrant(s) we're approaching the new vertex from u8 quad = 0; if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL; if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR; if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL; if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BR; // Check that the new vertex is in the right quadrant for the old vertex if (!(vertexes[curr.id].quadOutward & quad)) { // Hack: Always head towards the goal if possible, to avoid missing it if it's // inside another unit if (n != GOAL_VERTEX_ID) { continue; } } bool visible = CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) && CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) && CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) && CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) && CheckVisibility(vertexes[curr.id].p, npos, edges); /* // Render the edges that we examine m_DebugOverlayShortPathLines.push_back(SOverlayLine()); m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5); std::vector<float> xz; xz.push_back(vertexes[curr.id].p.X.ToFloat()); xz.push_back(vertexes[curr.id].p.Y.ToFloat()); xz.push_back(npos.X.ToFloat()); xz.push_back(npos.Y.ToFloat()); SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false); //*/ if (visible) { fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length(); // If this is a new tile, compute the heuristic distance if (vertexes[n].status == Vertex::UNEXPLORED) { // Add it to the open list: vertexes[n].status = Vertex::OPEN; vertexes[n].g = g; vertexes[n].h = DistanceToGoal(npos, goal); vertexes[n].pred = curr.id; // If this is an axis-aligned shape, the path must continue in the same quadrant // direction (but not go into the inside of the shape). // Hack: If we started *inside* a shape then perhaps headed to its corner (e.g. the unit // was very near another unit), don't restrict further pathing. if (vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8))) vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF; if (n == GOAL_VERTEX_ID) vertexes[n].p = npos; // remember the new best goal position PriorityQueue::Item t = { (u16)n, g + vertexes[n].h }; open.push(t); // Remember the heuristically best vertex we've seen so far, in case we never actually reach the target if (vertexes[n].h < hBest) { idBest = (u16)n; hBest = vertexes[n].h; } } else // must be OPEN { // If we've already seen this tile, and the new path to this tile does not have a // better cost, then stop now if (g >= vertexes[n].g) continue; // Otherwise, we have a better path, so replace the old one with the new cost/parent vertexes[n].g = g; vertexes[n].pred = curr.id; // If this is an axis-aligned shape, the path must continue in the same quadrant // direction (but not go into the inside of the shape). if (vertexes[n].quadInward) vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF; if (n == GOAL_VERTEX_ID) vertexes[n].p = npos; // remember the new best goal position open.promote((u16)n, g + vertexes[n].h); } } } } // Reconstruct the path (in reverse) for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred) { Waypoint w = { vertexes[id].p.X, vertexes[id].p.Y }; path.m_Waypoints.push_back(w); } PROFILE_END("A*"); }
static void AddTerrainEdges(std::vector<Edge>& edgesAA, std::vector<Vertex>& vertexes, u16 i0, u16 j0, u16 i1, u16 j1, fixed r, ICmpPathfinder::pass_class_t passClass, const Grid<TerrainTile>& terrain) { PROFILE("AddTerrainEdges"); std::vector<TileEdge> tileEdges; // Find all edges between tiles of differently passability statuses for (u16 j = j0; j <= j1; ++j) { for (u16 i = i0; i <= i1; ++i) { if (!IS_TERRAIN_PASSABLE(terrain.get(i, j), passClass)) { bool any = false; // whether we're adding any edges of this tile if (j > 0 && IS_TERRAIN_PASSABLE(terrain.get(i, j-1), passClass)) { TileEdge e = { i, j, TileEdge::BOTTOM }; tileEdges.push_back(e); any = true; } if (j < terrain.m_H-1 && IS_TERRAIN_PASSABLE(terrain.get(i, j+1), passClass)) { TileEdge e = { i, j, TileEdge::TOP }; tileEdges.push_back(e); any = true; } if (i > 0 && IS_TERRAIN_PASSABLE(terrain.get(i-1, j), passClass)) { TileEdge e = { i, j, TileEdge::LEFT }; tileEdges.push_back(e); any = true; } if (i < terrain.m_W-1 && IS_TERRAIN_PASSABLE(terrain.get(i+1, j), passClass)) { TileEdge e = { i, j, TileEdge::RIGHT }; tileEdges.push_back(e); any = true; } // If we want to add any edge, then add the whole square to the axis-aligned-edges list. // (The inner edges are redundant but it's easier than trying to split the squares apart.) if (any) { CFixedVector2D v0 = CFixedVector2D(fixed::FromInt(i * (int)CELL_SIZE) - r, fixed::FromInt(j * (int)CELL_SIZE) - r); CFixedVector2D v1 = CFixedVector2D(fixed::FromInt((i+1) * (int)CELL_SIZE) + r, fixed::FromInt((j+1) * (int)CELL_SIZE) + r); Edge e = { v0, v1 }; edgesAA.push_back(e); } } } } // TODO: maybe we should precompute these terrain edges since they'll rarely change? // TODO: for efficiency (minimising the A* search space), we should coalesce adjoining edges // Add all the tile outer edges to the search vertex lists for (size_t n = 0; n < tileEdges.size(); ++n) { u16 i = tileEdges[n].i; u16 j = tileEdges[n].j; CFixedVector2D v0, v1; Vertex vert; vert.status = Vertex::UNEXPLORED; vert.quadOutward = QUADRANT_ALL; switch (tileEdges[n].dir) { case TileEdge::BOTTOM: { v0 = CFixedVector2D(fixed::FromInt(i * (int)CELL_SIZE) - r, fixed::FromInt(j * (int)CELL_SIZE) - r); v1 = CFixedVector2D(fixed::FromInt((i+1) * (int)CELL_SIZE) + r, fixed::FromInt(j * (int)CELL_SIZE) - r); vert.p.X = v0.X - EDGE_EXPAND_DELTA; vert.p.Y = v0.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TR; vertexes.push_back(vert); vert.p.X = v1.X + EDGE_EXPAND_DELTA; vert.p.Y = v1.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TL; vertexes.push_back(vert); break; } case TileEdge::TOP: { v0 = CFixedVector2D(fixed::FromInt((i+1) * (int)CELL_SIZE) + r, fixed::FromInt((j+1) * (int)CELL_SIZE) + r); v1 = CFixedVector2D(fixed::FromInt(i * (int)CELL_SIZE) - r, fixed::FromInt((j+1) * (int)CELL_SIZE) + r); vert.p.X = v0.X + EDGE_EXPAND_DELTA; vert.p.Y = v0.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BL; vertexes.push_back(vert); vert.p.X = v1.X - EDGE_EXPAND_DELTA; vert.p.Y = v1.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BR; vertexes.push_back(vert); break; } case TileEdge::LEFT: { v0 = CFixedVector2D(fixed::FromInt(i * (int)CELL_SIZE) - r, fixed::FromInt((j+1) * (int)CELL_SIZE) + r); v1 = CFixedVector2D(fixed::FromInt(i * (int)CELL_SIZE) - r, fixed::FromInt(j * (int)CELL_SIZE) - r); vert.p.X = v0.X - EDGE_EXPAND_DELTA; vert.p.Y = v0.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BR; vertexes.push_back(vert); vert.p.X = v1.X - EDGE_EXPAND_DELTA; vert.p.Y = v1.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TR; vertexes.push_back(vert); break; } case TileEdge::RIGHT: { v0 = CFixedVector2D(fixed::FromInt((i+1) * (int)CELL_SIZE) + r, fixed::FromInt(j * (int)CELL_SIZE) - r); v1 = CFixedVector2D(fixed::FromInt((i+1) * (int)CELL_SIZE) + r, fixed::FromInt((j+1) * (int)CELL_SIZE) + r); vert.p.X = v0.X + EDGE_EXPAND_DELTA; vert.p.Y = v0.Y - EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_TL; vertexes.push_back(vert); vert.p.X = v1.X + EDGE_EXPAND_DELTA; vert.p.Y = v1.Y + EDGE_EXPAND_DELTA; vert.quadInward = QUADRANT_BL; vertexes.push_back(vert); break; } } } }
virtual CFixedVector3D PickSpawnPoint(entity_id_t spawned) { // Try to find a free space around the building's footprint. // (Note that we use the footprint, not the obstruction shape - this might be a bit dodgy // because the footprint might be inside the obstruction, but it hopefully gives us a nicer // shape.) const CFixedVector3D error(fixed::FromInt(-1), fixed::FromInt(-1), fixed::FromInt(-1)); CmpPtr<ICmpPosition> cmpPosition(GetSimContext(), GetEntityId()); if (!cmpPosition || !cmpPosition->IsInWorld()) return error; CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY); if (!cmpObstructionManager) return error; entity_pos_t spawnedRadius; ICmpObstructionManager::tag_t spawnedTag; CmpPtr<ICmpObstruction> cmpSpawnedObstruction(GetSimContext(), spawned); if (cmpSpawnedObstruction) { spawnedRadius = cmpSpawnedObstruction->GetUnitRadius(); spawnedTag = cmpSpawnedObstruction->GetObstruction(); } // else use zero radius // Get passability class from UnitMotion CmpPtr<ICmpUnitMotion> cmpUnitMotion(GetSimContext(), spawned); if (!cmpUnitMotion) return error; ICmpPathfinder::pass_class_t spawnedPass = cmpUnitMotion->GetPassabilityClass(); CmpPtr<ICmpPathfinder> cmpPathfinder(GetSimContext(), SYSTEM_ENTITY); if (!cmpPathfinder) return error; CFixedVector2D initialPos = cmpPosition->GetPosition2D(); entity_angle_t initialAngle = cmpPosition->GetRotation().Y; // Max spawning distance in tiles const i32 maxSpawningDistance = 4; if (m_Shape == CIRCLE) { // Expand outwards from foundation for (i32 dist = 0; dist <= maxSpawningDistance; ++dist) { // The spawn point should be far enough from this footprint to fit the unit, plus a little gap entity_pos_t clearance = spawnedRadius + entity_pos_t::FromInt(2 + (int)TERRAIN_TILE_SIZE*dist); entity_pos_t radius = m_Size0 + clearance; // Try equally-spaced points around the circle in alternating directions, starting from the front const i32 numPoints = 31 + 2*dist; for (i32 i = 0; i < (numPoints+1)/2; i = (i > 0 ? -i : 1-i)) // [0, +1, -1, +2, -2, ... (np-1)/2, -(np-1)/2] { entity_angle_t angle = initialAngle + (entity_angle_t::Pi()*2).Multiply(entity_angle_t::FromInt(i)/(int)numPoints); fixed s, c; sincos_approx(angle, s, c); CFixedVector3D pos (initialPos.X + s.Multiply(radius), fixed::Zero(), initialPos.Y + c.Multiply(radius)); SkipTagObstructionFilter filter(spawnedTag); // ignore collisions with the spawned entity if (cmpPathfinder->CheckUnitPlacement(filter, pos.X, pos.Z, spawnedRadius, spawnedPass) == ICmpObstruction::FOUNDATION_CHECK_SUCCESS) return pos; // this position is okay, so return it } } } else { fixed s, c; sincos_approx(initialAngle, s, c); // Expand outwards from foundation for (i32 dist = 0; dist <= maxSpawningDistance; ++dist) { // The spawn point should be far enough from this footprint to fit the unit, plus a little gap entity_pos_t clearance = spawnedRadius + entity_pos_t::FromInt(2 + (int)TERRAIN_TILE_SIZE*dist); for (i32 edge = 0; edge < 4; ++edge) { // Try equally-spaced points along the edge in alternating directions, starting from the middle const i32 numPoints = 9 + 2*dist; // Compute the direction and length of the current edge CFixedVector2D dir; fixed sx, sy; switch (edge) { case 0: dir = CFixedVector2D(c, -s); sx = m_Size0; sy = m_Size1; break; case 1: dir = CFixedVector2D(-s, -c); sx = m_Size1; sy = m_Size0; break; case 2: dir = CFixedVector2D(s, c); sx = m_Size1; sy = m_Size0; break; case 3: dir = CFixedVector2D(-c, s); sx = m_Size0; sy = m_Size1; break; } CFixedVector2D center = initialPos - dir.Perpendicular().Multiply(sy/2 + clearance); dir = dir.Multiply((sx + clearance*2) / (int)(numPoints-1)); for (i32 i = 0; i < (numPoints+1)/2; i = (i > 0 ? -i : 1-i)) // [0, +1, -1, +2, -2, ... (np-1)/2, -(np-1)/2] { CFixedVector2D pos (center + dir*i); SkipTagObstructionFilter filter(spawnedTag); // ignore collisions with the spawned entity if (cmpPathfinder->CheckUnitPlacement(filter, pos.X, pos.Y, spawnedRadius, spawnedPass) == ICmpObstruction::FOUNDATION_CHECK_SUCCESS) return CFixedVector3D(pos.X, fixed::Zero(), pos.Y); // this position is okay, so return it } } } } return error; }