Index: ps/trunk/source/simulation2/components/tests/test_Pathfinder.h
===================================================================
--- ps/trunk/source/simulation2/components/tests/test_Pathfinder.h (revision 20877)
+++ ps/trunk/source/simulation2/components/tests/test_Pathfinder.h (revision 20878)
@@ -1,353 +1,400 @@
-/* Copyright (C) 2017 Wildfire Games.
+/* Copyright (C) 2018 Wildfire Games.
* This file is part of 0 A.D.
*
* 0 A.D. is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* 0 A.D. is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with 0 A.D. If not, see .
*/
#include "simulation2/system/ComponentTest.h"
#include "simulation2/components/ICmpObstructionManager.h"
#include "simulation2/components/ICmpPathfinder.h"
#include "graphics/MapReader.h"
#include "graphics/Terrain.h"
#include "graphics/TerrainTextureManager.h"
#include "lib/timer.h"
#include "lib/tex/tex.h"
#include "ps/Loader.h"
#include "ps/Pyrogenesis.h"
#include "simulation2/Simulation2.h"
class TestCmpPathfinder : public CxxTest::TestSuite
{
public:
void setUp()
{
g_VFS = CreateVfs();
g_VFS->Mount(L"", DataDir()/"mods"/"mod", VFS_MOUNT_MUST_EXIST);
g_VFS->Mount(L"", DataDir()/"mods"/"public", VFS_MOUNT_MUST_EXIST, 1); // ignore directory-not-found errors
TS_ASSERT_OK(g_VFS->Mount(L"cache", DataDir()/"_testcache"));
CXeromyces::Startup();
// Need some stuff for terrain movement costs:
// (TODO: this ought to be independent of any graphics code)
new CTerrainTextureManager;
g_TexMan.LoadTerrainTextures();
}
void tearDown()
{
delete &g_TexMan;
CXeromyces::Terminate();
g_VFS.reset();
DeleteDirectory(DataDir()/"_testcache");
}
void test_namespace()
{
// Check that Pathfinding::NAVCELL_SIZE is actually an integer and that the definitions
// of Pathfinding::NAVCELL_SIZE_INT and Pathfinding::NAVCELL_SIZE_LOG2 match
TS_ASSERT_EQUALS(Pathfinding::NAVCELL_SIZE.ToInt_RoundToNegInfinity(), Pathfinding::NAVCELL_SIZE.ToInt_RoundToInfinity());
TS_ASSERT_EQUALS(Pathfinding::NAVCELL_SIZE.ToInt_RoundToNearest(), Pathfinding::NAVCELL_SIZE_INT);
TS_ASSERT_EQUALS((Pathfinding::NAVCELL_SIZE >> 1).ToInt_RoundToZero(), Pathfinding::NAVCELL_SIZE_LOG2);
}
+ void test_pathgoal()
+ {
+ entity_pos_t i = Pathfinding::NAVCELL_SIZE;
+ CFixedVector2D u(i*1, i*0);
+ CFixedVector2D v(i*0, i*1);
+
+ {
+ PathGoal goal = { PathGoal::POINT, i*8, i*6 };
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*6);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*2);
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*8 + v*6);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*10);
+ }
+
+ {
+ PathGoal goal = { PathGoal::CIRCLE, i*8, i*6, i*5 };
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*4);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*0);
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*4 + v*3);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*5);
+ }
+
+ {
+ PathGoal goal = { PathGoal::INVERTED_CIRCLE, i*8, i*6, i*5 };
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*1);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*3);
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*0 + v*0);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*0);
+ }
+
+ {
+ PathGoal goal = { PathGoal::SQUARE, i*8, i*6, i*4, i*3, u, v };
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*4);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*0);
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*4 + v*3);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*5);
+ }
+
+ {
+ PathGoal goal = { PathGoal::INVERTED_SQUARE, i*8, i*6, i*4, i*3, u, v };
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*8 + v*4), u*8 + v*3);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*8 + v*4), i*1);
+ TS_ASSERT_EQUALS(goal.NearestPointOnGoal(u*0 + v*0), u*0 + v*0);
+ TS_ASSERT_EQUALS(goal.DistanceToPoint(u*0 + v*0), i*0);
+ }
+ }
+
void test_performance_DISABLED()
{
CTerrain terrain;
CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain);
sim2.LoadDefaultScripts();
sim2.ResetState();
std::unique_ptr mapReader(new CMapReader());
LDR_BeginRegistering();
mapReader->LoadMap(L"maps/skirmishes/Median Oasis (2).pmp",
sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue,
&terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
&sim2, &sim2.GetSimContext(), -1, false);
LDR_EndRegistering();
TS_ASSERT_OK(LDR_NonprogressiveLoad());
sim2.Update(0);
CmpPtr cmp(sim2, SYSTEM_ENTITY);
#if 0
entity_pos_t x0 = entity_pos_t::FromInt(10);
entity_pos_t z0 = entity_pos_t::FromInt(495);
entity_pos_t x1 = entity_pos_t::FromInt(500);
entity_pos_t z1 = entity_pos_t::FromInt(495);
ICmpPathfinder::Goal goal = { ICmpPathfinder::Goal::POINT, x1, z1 };
WaypointPath path;
cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), path);
for (size_t i = 0; i < path.m_Waypoints.size(); ++i)
printf("%d: %f %f\n", (int)i, path.m_Waypoints[i].x.ToDouble(), path.m_Waypoints[i].z.ToDouble());
#endif
double t = timer_Time();
srand(1234);
for (size_t j = 0; j < 1024*2; ++j)
{
entity_pos_t x0 = entity_pos_t::FromInt(rand() % 512);
entity_pos_t z0 = entity_pos_t::FromInt(rand() % 512);
entity_pos_t x1 = x0 + entity_pos_t::FromInt(rand() % 64);
entity_pos_t z1 = z0 + entity_pos_t::FromInt(rand() % 64);
PathGoal goal = { PathGoal::POINT, x1, z1 };
WaypointPath path;
cmp->ComputePath(x0, z0, goal, cmp->GetPassabilityClass("default"), path);
}
t = timer_Time() - t;
printf("[%f]", t);
}
void test_performance_short_DISABLED()
{
CTerrain terrain;
terrain.Initialize(5, NULL);
CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain);
sim2.LoadDefaultScripts();
sim2.ResetState();
const entity_pos_t range = entity_pos_t::FromInt(TERRAIN_TILE_SIZE*12);
CmpPtr cmpObstructionMan(sim2, SYSTEM_ENTITY);
CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY);
srand(0);
for (size_t i = 0; i < 200; ++i)
{
fixed x = fixed::FromFloat(1.5f*range.ToFloat() * rand()/(float)RAND_MAX);
fixed z = fixed::FromFloat(1.5f*range.ToFloat() * rand()/(float)RAND_MAX);
// printf("# %f %f\n", x.ToFloat(), z.ToFloat());
cmpObstructionMan->AddUnitShape(INVALID_ENTITY, x, z, fixed::FromInt(2), 0, INVALID_ENTITY);
}
NullObstructionFilter filter;
PathGoal goal = { PathGoal::POINT, range, range };
WaypointPath path;
cmpPathfinder->ComputeShortPath(filter, range/3, range/3, fixed::FromInt(2), range, goal, 0, path);
for (size_t i = 0; i < path.m_Waypoints.size(); ++i)
printf("# %d: %f %f\n", (int)i, path.m_Waypoints[i].x.ToFloat(), path.m_Waypoints[i].z.ToFloat());
}
template
void DumpGrid(std::ostream& stream, const Grid& grid, int mask)
{
for (u16 j = 0; j < grid.m_H; ++j)
{
for (u16 i = 0; i < grid.m_W; )
{
if (!(grid.get(i, j) & mask))
{
i++;
continue;
}
u16 i0 = i;
for (i = i0+1; ; ++i)
{
if (i >= grid.m_W || !(grid.get(i, j) & mask))
{
stream << " \n";
break;
}
}
}
}
}
void test_perf2_DISABLED()
{
CTerrain terrain;
CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain);
sim2.LoadDefaultScripts();
sim2.ResetState();
std::unique_ptr mapReader(new CMapReader());
LDR_BeginRegistering();
mapReader->LoadMap(L"maps/scenarios/Peloponnese.pmp",
sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue,
&terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
&sim2, &sim2.GetSimContext(), -1, false);
LDR_EndRegistering();
TS_ASSERT_OK(LDR_NonprogressiveLoad());
sim2.Update(0);
std::ofstream stream(OsString("perf2.html").c_str(), std::ofstream::out | std::ofstream::trunc);
CmpPtr cmpObstructionManager(sim2, SYSTEM_ENTITY);
CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY);
pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default");
const Grid& obstructions = cmpPathfinder->GetPassabilityGrid();
int scale = 1;
stream << "\n";
stream << "\n";
stream << "\n";
}
void test_perf3_DISABLED()
{
CTerrain terrain;
CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain);
sim2.LoadDefaultScripts();
sim2.ResetState();
std::unique_ptr mapReader(new CMapReader());
LDR_BeginRegistering();
mapReader->LoadMap(L"maps/scenarios/Peloponnese.pmp",
sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue,
&terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
&sim2, &sim2.GetSimContext(), -1, false);
LDR_EndRegistering();
TS_ASSERT_OK(LDR_NonprogressiveLoad());
sim2.Update(0);
std::ofstream stream(OsString("perf3.html").c_str(), std::ofstream::out | std::ofstream::trunc);
CmpPtr cmpObstructionManager(sim2, SYSTEM_ENTITY);
CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY);
pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default");
const Grid& obstructions = cmpPathfinder->GetPassabilityGrid();
int scale = 31;
stream << "\n";
stream << "\n";
stream << "\n";
}
void DumpPath(std::ostream& stream, int i0, int j0, int i1, int j1, CmpPtr& cmpPathfinder)
{
entity_pos_t x0 = entity_pos_t::FromInt(i0);
entity_pos_t z0 = entity_pos_t::FromInt(j0);
entity_pos_t x1 = entity_pos_t::FromInt(i1);
entity_pos_t z1 = entity_pos_t::FromInt(j1);
PathGoal goal = { PathGoal::POINT, x1, z1 };
WaypointPath path;
cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path);
u32 debugSteps;
double debugTime;
Grid debugGrid;
cmpPathfinder->GetDebugData(debugSteps, debugTime, debugGrid);
// stream << " \n";
stream << " \n";
// stream << " \n";
// DumpGrid(stream, debugGrid, 1);
// stream << " \n";
// stream << " \n";
// DumpGrid(stream, debugGrid, 2);
// stream << " \n";
// stream << " \n";
// DumpGrid(stream, debugGrid, 3);
// stream << " \n";
stream << " \n";
stream << " \n";
}
void RepeatPath(int n, int i0, int j0, int i1, int j1, CmpPtr& cmpPathfinder)
{
entity_pos_t x0 = entity_pos_t::FromInt(i0);
entity_pos_t z0 = entity_pos_t::FromInt(j0);
entity_pos_t x1 = entity_pos_t::FromInt(i1);
entity_pos_t z1 = entity_pos_t::FromInt(j1);
PathGoal goal = { PathGoal::POINT, x1, z1 };
double t = timer_Time();
for (int i = 0; i < n; ++i)
{
WaypointPath path;
cmpPathfinder->ComputePath(x0, z0, goal, cmpPathfinder->GetPassabilityClass("default"), path);
}
t = timer_Time() - t;
debug_printf("### RepeatPath %fms each (%fs total)\n", 1000*t / n, t);
}
};
Index: ps/trunk/source/simulation2/helpers/PathGoal.cpp
===================================================================
--- ps/trunk/source/simulation2/helpers/PathGoal.cpp (revision 20877)
+++ ps/trunk/source/simulation2/helpers/PathGoal.cpp (revision 20878)
@@ -1,346 +1,376 @@
-/* Copyright (C) 2015 Wildfire Games.
+/* Copyright (C) 2018 Wildfire Games.
* This file is part of 0 A.D.
*
* 0 A.D. is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* 0 A.D. is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with 0 A.D. If not, see .
*/
#include "precompiled.h"
#include "PathGoal.h"
#include "graphics/Terrain.h"
#include "Pathfinding.h"
#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)
{
case POINT:
{
// Only accept a single navcell
int gi = (x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
int gj = (z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
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);
NODEFAULT;
}
}
bool PathGoal::NavcellRectContainsGoal(int i0, int j0, int i1, int j1, int* gi, int* gj) const
{
// Get min/max to simplify range checks
int imin = std::min(i0, i1);
int imax = std::max(i0, i1);
int jmin = std::min(j0, j1);
int jmax = std::max(j0, j1);
// Direction to iterate from (i0,j0) towards (i1,j1)
int di = i1 < i0 ? -1 : +1;
int dj = j1 < j0 ? -1 : +1;
switch (type)
{
case POINT:
{
// Calculate the navcell that contains the point goal
int i = (x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
int j = (z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
// If that goal navcell is in the given range, return it
if (imin <= i && i <= imax && jmin <= j && j <= jmax)
{
if (gi)
*gi = i;
if (gj)
*gj = j;
return true;
}
return false;
}
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;
}
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.
// (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))
{
if (gi)
*gi = i;
if (gj)
*gj = j;
return true;
}
}
}
return false;
}
NODEFAULT;
}
}
bool PathGoal::RectContainsGoal(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1) const
{
switch (type)
{
case POINT:
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);
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
);
}
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));
}
case INVERTED_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))
);
}
NODEFAULT;
}
}
fixed PathGoal::DistanceToPoint(CFixedVector2D pos) const
{
+ CFixedVector2D d(pos.X - x, pos.Y - z);
+
switch (type)
{
case POINT:
- return (pos - CFixedVector2D(x, z)).Length();
+ return d.Length();
case CIRCLE:
+ return d.CompareLength(hw) <= 0 ? fixed::Zero() : d.Length() - hw;
+
case INVERTED_CIRCLE:
- return ((pos - CFixedVector2D(x, z)).Length() - hw).Absolute();
+ return d.CompareLength(hw) >= 0 ? fixed::Zero() : hw - d.Length();
case SQUARE:
+ {
+ CFixedVector2D halfSize(hw, hh);
+ return Geometry::PointIsInSquare(d, u, v, halfSize) ?
+ fixed::Zero() : Geometry::DistanceToSquare(d, u, v, halfSize);
+ }
+
case INVERTED_SQUARE:
{
CFixedVector2D halfSize(hw, hh);
- CFixedVector2D d(pos.X - x, pos.Y - z);
- return Geometry::DistanceToSquare(d, u, v, halfSize);
+ return !Geometry::PointIsInSquare(d, u, v, halfSize) ?
+ fixed::Zero() : Geometry::DistanceToSquare(d, u, v, halfSize);
}
NODEFAULT;
}
}
CFixedVector2D PathGoal::NearestPointOnGoal(CFixedVector2D pos) const
{
CFixedVector2D g(x, z);
switch (type)
{
case POINT:
return g;
case CIRCLE:
+ {
+ CFixedVector2D d(pos.X - x, pos.Y - z);
+ if (d.CompareLength(hw) <= 0)
+ return pos;
+
+ d.Normalize(hw);
+ return g + d;
+ }
+
case INVERTED_CIRCLE:
{
- CFixedVector2D d = pos - g;
+ CFixedVector2D d(pos.X - x, pos.Y - z);
+ if (d.CompareLength(hw) >= 0)
+ return pos;
+
if (d.IsZero())
d = CFixedVector2D(fixed::FromInt(1), fixed::Zero()); // some arbitrary direction
d.Normalize(hw);
return g + d;
}
case SQUARE:
+ {
+ CFixedVector2D halfSize(hw, hh);
+ CFixedVector2D d(pos.X - x, pos.Y - z);
+ return Geometry::PointIsInSquare(d, u, v, halfSize) ?
+ pos : g + Geometry::NearestPointOnSquare(d, u, v, halfSize);
+ }
+
case INVERTED_SQUARE:
{
CFixedVector2D halfSize(hw, hh);
- CFixedVector2D d = pos - g;
- return g + Geometry::NearestPointOnSquare(d, u, v, halfSize);
+ CFixedVector2D d(pos.X - x, pos.Y - z);
+ return !Geometry::PointIsInSquare(d, u, v, halfSize) ?
+ pos : g + Geometry::NearestPointOnSquare(d, u, v, halfSize);
}
NODEFAULT;
}
}
Index: ps/trunk/source/simulation2/helpers/PathGoal.h
===================================================================
--- ps/trunk/source/simulation2/helpers/PathGoal.h (revision 20877)
+++ ps/trunk/source/simulation2/helpers/PathGoal.h (revision 20878)
@@ -1,84 +1,83 @@
-/* Copyright (C) 2015 Wildfire Games.
+/* Copyright (C) 2018 Wildfire Games.
* This file is part of 0 A.D.
*
* 0 A.D. is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* 0 A.D. is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with 0 A.D. If not, see .
*/
#ifndef INCLUDED_PATHGOAL
#define INCLUDED_PATHGOAL
#include "maths/FixedVector2D.h"
#include "simulation2/helpers/Position.h"
/**
* Pathfinder goal.
* The goal can be either a point, a circle, or a square (rectangle).
* For circles/squares, any point inside the shape is considered to be
* part of the goal.
* Also, it can be an 'inverted' circle/square, where any point outside
* the shape is part of the goal.
*/
class PathGoal
{
public:
enum Type {
POINT, // single point
CIRCLE, // the area inside a circle
INVERTED_CIRCLE, // the area outside a circle
SQUARE, // the area inside a square
INVERTED_SQUARE // the area outside a square
} type;
entity_pos_t x, z; // position of center
- CFixedVector2D u, v; // if [INVERTED_]SQUARE, then orthogonal unit axes
-
entity_pos_t hw, hh; // if [INVERTED_]SQUARE, then half width & height; if [INVERTED_]CIRCLE, then hw is radius
+ CFixedVector2D u, v; // if [INVERTED_]SQUARE, then orthogonal unit axes
+
entity_pos_t maxdist; // maximum distance wanted between two path waypoints
/**
* Returns true if the given navcell contains a part of the goal area.
*/
bool NavcellContainsGoal(int i, int j) const;
/**
* Returns true if any navcell (i, j) where
* min(i0,i1) <= i <= max(i0,i1)
* min(j0,j1) <= j <= max(j0,j1),
* contains a part of the goal area.
* If so, arguments i and j (if not NULL) are set to the goal navcell nearest
* to (i0, j0), assuming the rect has either width or height = 1.
*/
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.
*/
bool RectContainsGoal(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1) const;
/**
- * Returns the minimum distance from the point with the given @p pos
- * to any point on the outline of the goal shape.
+ * Returns the minimum distance from the point pos to any point on the goal shape.
*/
fixed DistanceToPoint(CFixedVector2D pos) const;
/**
- * Returns the coordinates of the point on the goal that is closest to pos in a straight line.
+ * Returns the coordinates of the point on the goal that is closest to the point pos.
*/
CFixedVector2D NearestPointOnGoal(CFixedVector2D pos) const;
};
#endif // INCLUDED_PATHGOAL