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; } }
bool Geometry::TestSquareSquare( const CFixedVector2D& c0, const CFixedVector2D& u0, const CFixedVector2D& v0, const CFixedVector2D& halfSize0, const CFixedVector2D& c1, const CFixedVector2D& u1, const CFixedVector2D& v1, const CFixedVector2D& halfSize1) { // TODO: need to test this carefully CFixedVector2D corner0a = c0 + u0.Multiply(halfSize0.X) + v0.Multiply(halfSize0.Y); CFixedVector2D corner0b = c0 - u0.Multiply(halfSize0.X) - v0.Multiply(halfSize0.Y); CFixedVector2D corner1a = c1 + u1.Multiply(halfSize1.X) + v1.Multiply(halfSize1.Y); CFixedVector2D corner1b = c1 - u1.Multiply(halfSize1.X) - v1.Multiply(halfSize1.Y); // Do a SAT test for each square vs each edge of the other square if (!SquareSAT(corner0a - c1, -u0, u1, v1, halfSize1)) return false; if (!SquareSAT(corner0a - c1, v0, u1, v1, halfSize1)) return false; if (!SquareSAT(corner0b - c1, u0, u1, v1, halfSize1)) return false; if (!SquareSAT(corner0b - c1, -v0, u1, v1, halfSize1)) return false; if (!SquareSAT(corner1a - c0, -u1, u0, v0, halfSize0)) return false; if (!SquareSAT(corner1a - c0, v1, u0, v0, halfSize0)) return false; if (!SquareSAT(corner1b - c0, u1, u0, v0, halfSize0)) return false; if (!SquareSAT(corner1b - c0, -v1, u0, v0, halfSize0)) return false; return true; }
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(); } }
// Exactly like TestRaySquare with u=(1,0), v=(0,1) bool Geometry::TestRayAASquare(const CFixedVector2D& a, const CFixedVector2D& b, const CFixedVector2D& halfSize) { fixed hw = halfSize.X; fixed hh = halfSize.Y; if (-hw <= a.X && a.X <= hw && -hh <= a.Y && a.Y <= hh) return false; // a is inside if (-hw <= b.X && b.X <= hw && -hh <= b.Y && b.Y <= hh) // TODO: isn't this subsumed by the next checks? return true; // a is outside, b is inside if ((a.X < -hw && b.X < -hw) || (a.X > hw && b.X > hw) || (a.Y < -hh && b.Y < -hh) || (a.Y > hh && b.Y > hh)) return false; // ab is entirely above/below/side the square CFixedVector2D abp = (b - a).Perpendicular(); fixed s0 = abp.Dot(CFixedVector2D(hw, hh) - a); fixed s1 = abp.Dot(CFixedVector2D(hw, -hh) - a); fixed s2 = abp.Dot(CFixedVector2D(-hw, -hh) - a); fixed s3 = abp.Dot(CFixedVector2D(-hw, hh) - a); if (s0.IsZero() || s1.IsZero() || s2.IsZero() || s3.IsZero()) return true; // ray intersects the corner bool sign = (s0 < fixed::Zero()); if ((s1 < fixed::Zero()) != sign || (s2 < fixed::Zero()) != sign || (s3 < fixed::Zero()) != sign) return true; // ray cuts through the square return false; }
bool Geometry::TestRaySquare(const CFixedVector2D& a, const CFixedVector2D& b, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize) { /* * We only consider collisions to be when the ray goes from outside to inside the shape (and possibly out again). * Various cases to consider: * 'a' inside, 'b' inside -> no collision * 'a' inside, 'b' outside -> no collision * 'a' outside, 'b' inside -> collision * 'a' outside, 'b' outside -> depends; use separating axis theorem: * if the ray's bounding box is outside the square -> no collision * if the whole square is on the same side of the ray -> no collision * otherwise -> collision * (Points on the edge are considered 'inside'.) */ fixed hw = halfSize.X; fixed hh = halfSize.Y; fixed au = a.Dot(u); fixed av = a.Dot(v); if (-hw <= au && au <= hw && -hh <= av && av <= hh) return false; // a is inside fixed bu = b.Dot(u); fixed bv = b.Dot(v); if (-hw <= bu && bu <= hw && -hh <= bv && bv <= hh) // TODO: isn't this subsumed by the next checks? return true; // a is outside, b is inside if ((au < -hw && bu < -hw) || (au > hw && bu > hw) || (av < -hh && bv < -hh) || (av > hh && bv > hh)) return false; // ab is entirely above/below/side the square CFixedVector2D abp = (b - a).Perpendicular(); fixed s0 = abp.Dot((u.Multiply(hw) + v.Multiply(hh)) - a); fixed s1 = abp.Dot((u.Multiply(hw) - v.Multiply(hh)) - a); fixed s2 = abp.Dot((-u.Multiply(hw) - v.Multiply(hh)) - a); fixed s3 = abp.Dot((-u.Multiply(hw) + v.Multiply(hh)) - a); if (s0.IsZero() || s1.IsZero() || s2.IsZero() || s3.IsZero()) return true; // ray intersects the corner bool sign = (s0 < fixed::Zero()); if ((s1 < fixed::Zero()) != sign || (s2 < fixed::Zero()) != sign || (s3 < fixed::Zero()) != sign) return true; // ray cuts through the square return false; }
fixed Geometry::DistanceToSquare(const CFixedVector2D& point, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize, bool countInsideAsZero) { /* * Relative to its own coordinate system, we have a square like: * * A : B : C * : : * - - ########### - - * # # * # I # * D # 0 # E v * # # ^ * # # | * - - ########### - - -->u * : : * F : G : H * * where 0 is the center, u and v are unit axes, * and the square is hw*2 by hh*2 units in size. * * Points in the BIG regions should check distance to horizontal edges. * Points in the DIE regions should check distance to vertical edges. * Points in the ACFH regions should check distance to the corresponding corner. * * So we just need to check all of the regions to work out which calculations to apply. * */ // By symmetry (taking absolute values), we work only in the 0-B-C-E quadrant // du, dv are the location of the point in the square's coordinate system fixed du = point.Dot(u).Absolute(); fixed dv = point.Dot(v).Absolute(); fixed hw = halfSize.X; fixed hh = halfSize.Y; if (du < hw) // regions B, I, G { if (dv < hh) // region I return countInsideAsZero ? fixed::Zero() : std::min(hw - du, hh - dv); else return dv - hh; } else if (dv < hh) // regions D, E { return du - hw; // vertical edges } else // regions A, C, F, H { CFixedVector2D distance(du - hw, dv - hh); return distance.Length(); } }
/** * Debug visualisation of graph edges between regions. */ void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass) { const EdgesMap& edges = m_Edges[passClass]; const std::vector<Chunk>& chunks = m_Chunks[passClass]; for (auto& edge : edges) { for (const RegionID& region: edge.second) { // Draw a line between the two regions' centers int i0, j0, i1, j1; chunks[edge.first.cj * m_ChunksW + edge.first.ci].RegionCenter(edge.first.r, i0, j0); chunks[region.cj * m_ChunksW + region.ci].RegionCenter(region.r, i1, j1); CFixedVector2D a, b; Pathfinding::NavcellCenter(i0, j0, a.X, a.Y); Pathfinding::NavcellCenter(i1, j1, b.X, b.Y); // Push the endpoints inwards a little to avoid overlaps CFixedVector2D d = b - a; d.Normalize(entity_pos_t::FromInt(1)); a += d; b -= d; std::vector<float> xz; xz.push_back(a.X.ToFloat()); xz.push_back(a.Y.ToFloat()); xz.push_back(b.X.ToFloat()); xz.push_back(b.Y.ToFloat()); m_DebugOverlayLines.emplace_back(); m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0); SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true); } } }
// Same as above except it does not use Length // For explanations refer to DistanceToSquare fixed Geometry::DistanceToSquareSquared(const CFixedVector2D& point, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize, bool countInsideAsZero) { fixed du = point.Dot(u).Absolute(); fixed dv = point.Dot(v).Absolute(); fixed hw = halfSize.X; fixed hh = halfSize.Y; if (du < hw) // regions B, I, G { if (dv < hh) // region I return countInsideAsZero ? fixed::Zero() : std::min((hw - du).Square(), (hh - dv).Square()); else return (dv - hh).Square(); // horizontal edges } else if (dv < hh) // regions D, E { return (du - hw).Square(); // vertical edges } else // regions A, C, F, H { return (du - hw).Square() + (dv - hh).Square(); } }
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; }
/** * Separating axis test; returns true if the square defined by u/v/halfSize at the origin * is not entirely on the clockwise side of a line in direction 'axis' passing through 'a' */ static bool SquareSAT(const CFixedVector2D& a, const CFixedVector2D& axis, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize) { fixed hw = halfSize.X; fixed hh = halfSize.Y; CFixedVector2D p = axis.Perpendicular(); if (p.Dot((u.Multiply(hw) + v.Multiply(hh)) - a) <= fixed::Zero()) return true; if (p.Dot((u.Multiply(hw) - v.Multiply(hh)) - a) <= fixed::Zero()) return true; if (p.Dot((-u.Multiply(hw) - v.Multiply(hh)) - a) <= fixed::Zero()) return true; if (p.Dot((-u.Multiply(hw) + v.Multiply(hh)) - a) <= fixed::Zero()) return true; return false; }
CFixedVector2D Geometry::NearestPointOnSquare(const CFixedVector2D& point, const CFixedVector2D& u, const CFixedVector2D& v, const CFixedVector2D& halfSize) { /* * Relative to its own coordinate system, we have a square like: * * A : : C * : : * - - #### B #### - - * #\ /# * # \ / # * D --0-- E v * # / \ # ^ * #/ \# | * - - #### G #### - - -->u * : : * F : : H * * where 0 is the center, u and v are unit axes, * and the square is hw*2 by hh*2 units in size. * * Points in the BDEG regions are nearest to the corresponding edge. * Points in the ACFH regions are nearest to the corresponding corner. * * So we just need to check all of the regions to work out which calculations to apply. * */ // du, dv are the location of the point in the square's coordinate system fixed du = point.Dot(u); fixed dv = point.Dot(v); fixed hw = halfSize.X; fixed hh = halfSize.Y; if (-hw < du && du < hw) // regions B, G; or regions D, E inside the square { if (-hh < dv && dv < hh && (du.Absolute() - hw).Absolute() < (dv.Absolute() - hh).Absolute()) // regions D, E { if (du >= fixed::Zero()) // E return u.Multiply(hw) + v.Multiply(dv); else // D return -u.Multiply(hw) + v.Multiply(dv); } else // B, G { if (dv >= fixed::Zero()) // B return v.Multiply(hh) + u.Multiply(du); else // G return -v.Multiply(hh) + u.Multiply(du); } } else if (-hh < dv && dv < hh) // regions D, E outside the square { if (du >= fixed::Zero()) // E return u.Multiply(hw) + v.Multiply(dv); else // D return -u.Multiply(hw) + v.Multiply(dv); } else // regions A, C, F, H { CFixedVector2D corner; if (du < fixed::Zero()) // A, F corner -= u.Multiply(hw); else // C, H corner += u.Multiply(hw); if (dv < fixed::Zero()) // F, H corner -= v.Multiply(hh); else // A, C corner += v.Multiply(hh); return corner; } }