Index: source/simulation2/components/tests/test_Pathfinder.h =================================================================== --- source/simulation2/components/tests/test_Pathfinder.h +++ source/simulation2/components/tests/test_Pathfinder.h @@ -78,7 +78,8 @@ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*8 + v*6); TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*10); TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*12, i*9)); - TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*8, i*6)); + TS_ASSERT(goal.RectContainsGoal(i*8, i*6, i*10, i*12)); // on the left-edge, collision + TS_ASSERT(!goal.RectContainsGoal(i*4, i*3, i*8, i*6)); // on the right-edge, no collision TS_ASSERT(goal.RectContainsGoal(i*8, i*6, i*12, i*9)); TS_ASSERT(!goal.RectContainsGoal(i*4, i*3, i*7, i*5)); TS_ASSERT(!goal.RectContainsGoal(i*9, i*7, i*13, i*15)); @@ -93,7 +94,8 @@ TS_ASSERT(goal.RectContainsGoal(i*7, i*5, i*9, i*7)); // fully inside TS_ASSERT(goal.RectContainsGoal(i*3, i*1, i*13, i*11)); // fully outside TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*8, i*6)); // partially inside - TS_ASSERT(goal.RectContainsGoal(i*4, i*0, i*12, i*1)); // touching the edge + TS_ASSERT(goal.RectContainsGoal(i*4, i*11, i*12, i*13)); // touching the top edge - collision + TS_ASSERT(!goal.RectContainsGoal(i*4, i*0, i*12, i*1)); // touching the bottom edge - no collision } { @@ -104,8 +106,9 @@ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*0); TS_ASSERT(!goal.RectContainsGoal(i*7, i*5, i*9, i*7)); // fully inside TS_ASSERT(goal.RectContainsGoal(i*3, i*1, i*13, i*11)); // fully outside - TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*8, i*6)); // partially inside - TS_ASSERT(goal.RectContainsGoal(i*4, i*0, i*12, i*1)); // touching the edge + TS_ASSERT(goal.RectContainsGoal(i*2, i*2, i*8, i*6)); // partially inside + TS_ASSERT(!goal.RectContainsGoal(i*4, i*3, i*8, i*6)); // touching the edge from the inside - no collision + TS_ASSERT(goal.RectContainsGoal(i*4, i*0, i*12, i*1)); // touching the edge from the outside } { @@ -117,8 +120,20 @@ TS_ASSERT(goal.RectContainsGoal(i*7, i*5, i*9, i*7)); // fully inside TS_ASSERT(goal.RectContainsGoal(i*3, i*1, i*13, i*11)); // fully outside TS_ASSERT(goal.RectContainsGoal(i*4, i*3, i*8, i*6)); // partially inside - TS_ASSERT(goal.RectContainsGoal(i*4, i*2, i*12, i*3)); // touching the edge - TS_ASSERT(goal.RectContainsGoal(i*3, i*0, i*4, i*10)); // touching the edge + TS_ASSERT(goal.RectContainsGoal(i*4, i*2, i*12, i*3)); // touching the top edge - collision + TS_ASSERT(!goal.RectContainsGoal(i*1, i*9, i*12, i*12)); // touching the bottom edge - no collision + TS_ASSERT(goal.RectContainsGoal(i*3, i*0, i*4, i*10)); // touching the left edge - collision + TS_ASSERT(!goal.RectContainsGoal(i*12, i*2, i*16, i*7)); // touching the right edge - no collision + } + + { + // Vector rotated such that the closest point from the goal to our rectangle isn't straightforward (i.e not a corner). + PathGoal goal = { PathGoal::SQUARE, fixed::FromDouble(167.806610), fixed::FromDouble(674.834106), fixed::FromDouble(19.75), fixed::FromDouble(19.75), u, v }; + goal.u = CFixedVector2D(fixed::FromDouble(-0.384079), fixed::FromDouble(-0.923660)); + goal.v = CFixedVector2D(fixed::FromDouble(0.923660), fixed::FromDouble(-0.384079)); + TS_ASSERT(goal.RectContainsGoal(fixed::FromInt(192),fixed::FromInt(288),fixed::FromInt(672),fixed::FromInt(768))); + int i, j; + TS_ASSERT(goal.NavcellRectContainsGoal(192, 288, 672, 768, &i, &j)); } { Index: source/simulation2/helpers/PathGoal.h =================================================================== --- source/simulation2/helpers/PathGoal.h +++ source/simulation2/helpers/PathGoal.h @@ -64,8 +64,9 @@ bool NavcellRectContainsGoal(int i0, int j0, int i1, int j1, int* i, int* j) const; /** - * Returns true if the rectangle defined by (x0,z0)-(x1,z1) (inclusive) - * contains a part of the goal area. + * Returns true if the rectangle defined by (x0,z0)-(x1,z1) contains a part of the goal area. + * For POINT/SQUARE/CIRCLE goals, inclusive on the left and top edges, exclusive on the bottom and right edges. + * For inverse goals, entirely exclusive. */ bool RectContainsGoal(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1) const; Index: source/simulation2/helpers/PathGoal.cpp =================================================================== --- source/simulation2/helpers/PathGoal.cpp +++ source/simulation2/helpers/PathGoal.cpp @@ -24,74 +24,6 @@ #include "Geometry.h" -static bool NavcellContainsCircle(int i, int j, fixed x, fixed z, fixed r, bool inside) -{ - // Accept any navcell (i,j) that contains a point which is inside[/outside] - // (or on the edge of) the circle - - // Get world-space bounds of navcell - entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); - entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); - entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; - entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; - - if (inside) - { - // Get the point inside the navcell closest to (x,z) - entity_pos_t nx = Clamp(x, x0, x1); - entity_pos_t nz = Clamp(z, z0, z1); - // Check if that point is inside the circle - return (CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(r) <= 0; - } - else - { - // If any corner of the navcell is outside the circle, return true. - // Otherwise, since the circle is convex, there cannot be any other point - // in the navcell that is outside the circle. - return ( - (CFixedVector2D(x0, z0) - CFixedVector2D(x, z)).CompareLength(r) >= 0 - || (CFixedVector2D(x1, z0) - CFixedVector2D(x, z)).CompareLength(r) >= 0 - || (CFixedVector2D(x0, z1) - CFixedVector2D(x, z)).CompareLength(r) >= 0 - || (CFixedVector2D(x1, z1) - CFixedVector2D(x, z)).CompareLength(r) >= 0 - ); - } -} - -static bool NavcellContainsSquare(int i, int j, - fixed x, fixed z, CFixedVector2D u, CFixedVector2D v, fixed hw, fixed hh, - bool inside) -{ - // Accept any navcell (i,j) that contains a point which is inside[/outside] - // (or on the edge of) the square - - // Get world-space bounds of navcell - entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); - entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); - entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; - entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; - - if (inside) - { - // Get the point inside the navcell closest to (x,z) - entity_pos_t nx = Clamp(x, x0, x1); - entity_pos_t nz = Clamp(z, z0, z1); - // Check if that point is inside the circle - return Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh)); - } - else - { - // If any corner of the navcell is outside the square, return true. - // Otherwise, since the square is convex, there cannot be any other point - // in the navcell that is outside the square. - return ( - !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z0 - z), u, v, CFixedVector2D(hw, hh)) - || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z0 - z), u, v, CFixedVector2D(hw, hh)) - || !Geometry::PointIsInSquare(CFixedVector2D(x0 - x, z1 - z), u, v, CFixedVector2D(hw, hh)) - || !Geometry::PointIsInSquare(CFixedVector2D(x1 - x, z1 - z), u, v, CFixedVector2D(hw, hh)) - ); - } -} - bool PathGoal::NavcellContainsGoal(int i, int j) const { switch (type) @@ -104,13 +36,16 @@ return gi == i && gj == j; } case CIRCLE: - return NavcellContainsCircle(i, j, x, z, hw, true); case INVERTED_CIRCLE: - return NavcellContainsCircle(i, j, x, z, hw, false); case SQUARE: - return NavcellContainsSquare(i, j, x, z, u, v, hw, hh, true); case INVERTED_SQUARE: - return NavcellContainsSquare(i, j, x, z, u, v, hw, hh, false); + { + entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + return RectContainsGoal(x0, z0, x1, z1); + } NODEFAULT; } } @@ -147,89 +82,36 @@ } case CIRCLE: - { - // Loop over all navcells in the given range (starting at (i0,j0) since - // this function is meant to find the goal navcell nearest to there - // assuming jmin==jmax || imin==imax), - // and check whether any point in each navcell is within the goal circle. - // (TODO: this is pretty inefficient.) - for (int j = j0; jmin <= j && j <= jmax; j += dj) - { - for (int i = i0; imin <= i && i <= imax; i += di) - { - if (NavcellContainsCircle(i, j, x, z, hw, true)) - { - if (gi) - *gi = i; - if (gj) - *gj = j; - return true; - } - } - } - return false; - } - - case INVERTED_CIRCLE: - { - // Loop over all navcells in the given range (starting at (i0,j0) since - // this function is meant to find the goal navcell nearest to there - // assuming jmin==jmax || imin==imax), - // and check whether any point in each navcell is outside the goal circle. - // (TODO: this is pretty inefficient.) - for (int j = j0; jmin <= j && j <= jmax; j += dj) - { - for (int i = i0; imin <= i && i <= imax; i += di) - { - if (NavcellContainsCircle(i, j, x, z, hw, false)) - { - if (gi) - *gi = i; - if (gj) - *gj = j; - return true; - } - } - } - return false; - } - case SQUARE: { - // Loop over all navcells in the given range (starting at (i0,j0) since - // this function is meant to find the goal navcell nearest to there - // assuming jmin==jmax || imin==imax), - // and check whether any point in each navcell is within the goal square. - // (TODO: this is pretty inefficient.) - for (int j = j0; jmin <= j && j <= jmax; j += dj) - { - for (int i = i0; imin <= i && i <= imax; i += di) - { - if (NavcellContainsSquare(i, j, x, z, u, v, hw, hh, true)) - { - if (gi) - *gi = i; - if (gj) - *gj = j; - return true; - } - } - } - return false; - } + // If the rectangle as a whole doesn't contain the goal, we can safely return. + // TODO: we could do a similar check for INVERTED_[CIRCLE/SQUARE] + entity_pos_t x0 = entity_pos_t::FromInt(imin).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t z0 = entity_pos_t::FromInt(jmin).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t x1 = entity_pos_t::FromInt(imax).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t z1 = entity_pos_t::FromInt(jmax).Multiply(Pathfinding::NAVCELL_SIZE); + if (!RectContainsGoal(x0, z0, x1, z1)) + return false; + } + case INVERTED_CIRCLE: case INVERTED_SQUARE: { // Loop over all navcells in the given range (starting at (i0,j0) since // this function is meant to find the goal navcell nearest to there - // assuming jmin==jmax || imin==imax), - // and check whether any point in each navcell is outside the goal square. + // assuming jmin == jmax || imin == imax), + // and check whether any point in each navcell is within the goal. // (TODO: this is pretty inefficient.) + for (int j = j0; jmin <= j && j <= jmax; j += dj) { for (int i = i0; imin <= i && i <= imax; i += di) { - if (NavcellContainsSquare(i, j, x, z, u, v, hw, hh, false)) + entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + if (RectContainsGoal(x0, z0, x1, z1)) { if (gi) *gi = i; @@ -241,7 +123,6 @@ } return false; } - NODEFAULT; } } @@ -251,30 +132,69 @@ switch (type) { case POINT: - return x0 <= x && x <= x1 && z0 <= z && z <= z1; + return x0 <= x && x < x1 && z0 <= z && z < z1; case CIRCLE: { - entity_pos_t nx = Clamp(x, x0, x1); - entity_pos_t nz = Clamp(z, z0, z1); + entity_pos_t nx = Clamp(x, x0, x1 - fixed::Epsilon()); + entity_pos_t nz = Clamp(z, z0, z1 - fixed::Epsilon()); return (CFixedVector2D(nx, nz) - CFixedVector2D(x, z)).CompareLength(hw) <= 0; } case INVERTED_CIRCLE: { return ( - (CFixedVector2D(x0, z0) - CFixedVector2D(x, z)).CompareLength(hw) >= 0 - || (CFixedVector2D(x1, z0) - CFixedVector2D(x, z)).CompareLength(hw) >= 0 - || (CFixedVector2D(x0, z1) - CFixedVector2D(x, z)).CompareLength(hw) >= 0 - || (CFixedVector2D(x1, z1) - CFixedVector2D(x, z)).CompareLength(hw) >= 0 + (CFixedVector2D(x0, z0) - CFixedVector2D(x, z)).CompareLength(hw) > 0 + || (CFixedVector2D(x1, z0) - CFixedVector2D(x, z)).CompareLength(hw) > 0 + || (CFixedVector2D(x0, z1) - CFixedVector2D(x, z)).CompareLength(hw) > 0 + || (CFixedVector2D(x1, z1) - CFixedVector2D(x, z)).CompareLength(hw) > 0 ); } case SQUARE: { - entity_pos_t nx = Clamp(x, x0, x1); - entity_pos_t nz = Clamp(z, z0, z1); - return Geometry::PointIsInSquare(CFixedVector2D(nx - x, nz - z), u, v, CFixedVector2D(hw, hh)); + // Square-rotated-square collision, apply the Separating Axis Theorem. + CFixedVector2D point(x, z); + + fixed minX = x - u.X.Absolute().Multiply(hw) - v.X.Absolute().Multiply(hw); + if (x1 < minX) + return false; + fixed maxX = x + u.X.Absolute().Multiply(hw) + v.X.Absolute().Multiply(hw); + if (x0 >= maxX) + return false; + fixed minZ = z - u.Y.Absolute().Multiply(hh) - v.Y.Absolute().Multiply(hh); + if (z1 < minZ) + return false; + fixed maxZ = z + u.Y.Absolute().Multiply(hh) + v.Y.Absolute().Multiply(hh); + if (z0 >= maxZ) + return false; + + // Project our AA-square on the rotated square. + fixed minU = CFixedVector2D(x0 - x, z0 - z).Dot(u); + fixed maxU = minU; + minU = std::min(minU, CFixedVector2D(x0 - x, z1 - z).Dot(u)); + maxU = std::max(maxU, CFixedVector2D(x0 - x, z1 - z).Dot(u)); + minU = std::min(minU, CFixedVector2D(x1 - x, z0 - z).Dot(u)); + maxU = std::max(maxU, CFixedVector2D(x1 - x, z0 - z).Dot(u)); + minU = std::min(minU, CFixedVector2D(x1 - x, z1 - z).Dot(u)); + maxU = std::max(maxU, CFixedVector2D(x1 - x, z1 - z).Dot(u)); + + if (maxU < -hw || minU >= hw) + return false; + + fixed minV = CFixedVector2D(x0 - x, z0 - z).Dot(v); + fixed maxV = minV; + minV = std::min(minV, CFixedVector2D(x0 - x, z1 - z).Dot(v)); + maxV = std::max(maxV, CFixedVector2D(x0 - x, z1 - z).Dot(v)); + minV = std::min(minV, CFixedVector2D(x1 - x, z0 - z).Dot(v)); + maxV = std::max(maxV, CFixedVector2D(x1 - x, z0 - z).Dot(v)); + minV = std::min(minV, CFixedVector2D(x1 - x, z1 - z).Dot(v)); + maxV = std::max(maxV, CFixedVector2D(x1 - x, z1 - z).Dot(v)); + + if (maxV < -hh || minV >= hh) + return false; + + return true; } case INVERTED_SQUARE: