paczek/0ad
1
0
Fork 0
forked from 0ad/0ad
Code Pull Requests Activity

# Pathfinder updates

Browse Source 
More hacks so units follow paths relatively smoothly, and to avoid
pathfinding in simple situations

This was SVN commit r7296.
This commit is contained in:
Ykkrosh 2010-01-30 13:11:58 +00:00
parent fa1fd65a3e
commit e0ed8a1629
6 changed files with 317 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
source/maths/Fixed.h
View File

@ -19,6 +19,7 @@
#define INCLUDED_FIXED
#include "lib/types.h"
#include "maths/Sqrt.h"
template <typename T>
inline T round_away_from_zero(float value)
@ -140,6 +141,14 @@ public:
i64 t = (i64)value * (i64)n.value;
return CFixed((T)(t >> fract_bits));
}
CFixed Sqrt() const
{
if (value <= 0)
return CFixed(0);
u32 s = isqrt64(value);
return CFixed((u64)s << (fract_bits / 2));
}
};
/**

121
source/maths/FixedVector2D.h Normal file
View File

@ -0,0 +1,121 @@
/* Copyright (C) 2010 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 <http://www.gnu.org/licenses/>.
*/
#ifndef INCLUDED_FIXED_VECTOR2D
#define INCLUDED_FIXED_VECTOR2D
#include "maths/Fixed.h"
#include "maths/Sqrt.h"
class CFixedVector2D
{
private:
typedef CFixed_23_8 fixed;
public:
fixed X, Y;
CFixedVector2D() { }
CFixedVector2D(fixed X, fixed Y) : X(X), Y(Y) { }
/// Vector equality
bool operator==(const CFixedVector2D& v) const
{
return (X == v.X && Y == v.Y);
}
/// Vector addition
CFixedVector2D operator+(const CFixedVector2D& v) const
{
return CFixedVector2D(X + v.X, Y + v.Y);
}
/// Vector subtraction
CFixedVector2D operator-(const CFixedVector2D& v) const
{
return CFixedVector2D(X - v.X, Y - v.Y);
}
/// Negation
CFixedVector2D operator-() const
{
return CFixedVector2D(-X, -Y);
}
/// Vector addition
CFixedVector2D& operator+=(const CFixedVector2D& v)
{
*this = *this + v;
return *this;
}
/// Vector subtraction
CFixedVector2D& operator-=(const CFixedVector2D& v)
{
*this = *this - v;
return *this;
}
/**
* Returns the length of the vector.
* Will not overflow if the result can be represented as type 'fixed'.
*/
fixed Length() const
{
// Do intermediate calculations with 64-bit ints to avoid overflows
i64 x = (i64)X.GetInternalValue();
i64 y = (i64)Y.GetInternalValue();
u64 d2 = (u64)(x * x + y * y);
u32 d = isqrt64(d2);
fixed r;
r.SetInternalValue((i32)d);
return r;
}
/**
* Normalize the vector so that length is close to 1.
* If length is 0, does nothing.
* WARNING: The fixed-point numbers only have 8-bit fractional parts, so
* a normalized vector will be very imprecise.
*/
void Normalize()
{
fixed l = Length();
if (!l.IsZero())
{
X = X / l;
Y = Y / l;
}
}
/**
* Compute the dot product of this vector with another.
*/
fixed Dot(const CFixedVector2D& v)
{
i64 x = (i64)X.GetInternalValue() * (i64)v.X.GetInternalValue();
i64 y = (i64)Y.GetInternalValue() * (i64)v.Y.GetInternalValue();
i64 sum = x + y;
fixed ret;
ret.SetInternalValue((i32)(sum >> fixed::fract_bits));
return ret;
}
};
#endif // INCLUDED_FIXED_VECTOR2D

9
source/maths/tests/test_Fixed.h
View File

@ -113,6 +113,15 @@ public:
// TODO: test all the arithmetic operators
void test_Sqrt()
{
TS_ASSERT_EQUALS(CFixed_23_8::FromDouble(1.0).Sqrt().ToDouble(), 1.0);
TS_ASSERT_EQUALS(CFixed_23_8::FromDouble(1000000.0).Sqrt().ToDouble(), 1000.0);
TS_ASSERT_EQUALS(CFixed_23_8::FromDouble(0.0625).Sqrt().ToDouble(), 0.25);
TS_ASSERT_EQUALS(CFixed_23_8::FromDouble(-1.0).Sqrt().ToDouble(), 0.0);
TS_ASSERT_EQUALS(CFixed_23_8::FromDouble(0.0).Sqrt().ToDouble(), 0.0);
}
void test_Atan2()
{
typedef CFixed_23_8 f;

67
source/simulation2/components/CCmpPathfinder.cpp
View File

@ -23,6 +23,7 @@
#include "simulation2/MessageTypes.h"
#include "graphics/Terrain.h"
#include "maths/FixedVector2D.h"
#include "maths/MathUtil.h"
#include "ps/Overlay.h"
#include "ps/Profile.h"
@ -223,6 +224,10 @@ public:
m_GridDirty = true;
}
virtual bool CanMoveStraight(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, u32& cost);
virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, Path& ret);
virtual void SetDebugPath(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1)
{
delete m_DebugGrid;
@ -302,13 +307,69 @@ public:
m_GridDirty = false;
}
}
void ComputePath(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, Path& path);
};
REGISTER_COMPONENT_TYPE(Pathfinder)
u32 g_CostPerTile = 256; // base cost to move between adjacent tiles
// Detect intersection between ray (0,0)-L and circle with center M radius r
// (Only counts intersections from the outside to the inside)
static bool IntersectRayCircle(CFixedVector2D l, CFixedVector2D m, entity_pos_t r)
{
// TODO: this should all be checked and tested etc, it's just a rough first attempt for now...
// Intersections at (t * l.X - m.X)^2 * (t * l.Y - m.Y) = r^2
// so solve the quadratic for t:
#define DOT(u, v) ( ((i64)u.X.GetInternalValue()*(i64)v.X.GetInternalValue()) + ((i64)u.Y.GetInternalValue()*(i64)v.Y.GetInternalValue()) )
i64 a = DOT(l, l);
if (a == 0)
return false; // avoid divide-by-zero later
i64 b = DOT(l, m)*-2;
i64 c = DOT(m, m) - r.GetInternalValue()*r.GetInternalValue();
i64 d = b*b - 4*a*c; // TODO: overflow breaks stuff here
if (d < 0) // no solutions
return false;
// Find the time of first intersection (entering the circle)
i64 t2a = (-b - isqrt64(d)); // don't divide by 2a explicitly, to avoid rounding errors
if ((a > 0 && t2a < 0) || (a < 0 && t2a > 0)) // if t2a/2a < 0 then intersection was before the ray
return false;
if (t2a >= 2*a) // intersection was after the ray
return false;
// printf("isct (%f,%f) (%f,%f) %f a=%lld b=%lld c=%lld d=%lld t2a=%lld\n", l.X.ToDouble(), l.Y.ToDouble(), m.X.ToDouble(), m.Y.ToDouble(), r.ToDouble(), a, b, c, d, t2a);
return true;
}
bool CCmpPathfinder::CanMoveStraight(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, u32& cost)
{
PROFILE("CanMoveStraight");
// TODO: this is all very inefficient, it should use kind of spatial data structures
// Ray-circle intersections
for (std::map<tag_t, Circle>::iterator it = m_Circles.begin(); it != m_Circles.end(); ++it)
{
if (IntersectRayCircle(CFixedVector2D(x1 - x0, z1 - z0), CFixedVector2D(it->second.x - x0, it->second.z - z0), it->second.r + r))
return false;
}
// Ray-square intersections
for (std::map<tag_t, Square>::iterator it = m_Squares.begin(); it != m_Squares.end(); ++it)
{
// XXX need some kind of square intersection code
if (IntersectRayCircle(CFixedVector2D(x1 - x0, z1 - z0), CFixedVector2D(it->second.x - x0, it->second.z - z0), it->second.w/2 + r))
return false;
}
// Calculate the exact movement cost
// (TODO: this needs to care about terrain costs etc)
cost = (CFixedVector2D(x1 - x0, z1 - z0).Length() * g_CostPerTile).ToInt_RoundToZero();
return true;
}
/**
* Tile data for A* computation
*/
@ -364,8 +425,6 @@ void PathfinderOverlay::ProcessTile(ssize_t i, ssize_t j)
* to the implementation shouldn't affect that interface much.
*/
u32 g_CostPerTile = 256; // base cost to move between adjacent tiles
struct QueueItem
{
u16 i, j;

124
source/simulation2/components/CCmpUnitMotion.cpp
View File

@ -34,15 +34,19 @@ public:
DEFAULT_COMPONENT_ALLOCATOR(UnitMotion)
const CSimContext* m_Context;
// Template state:
CFixed_23_8 m_Speed; // in units per second
// Dynamic state:
bool m_HasTarget;
ICmpPathfinder::Path m_Path;
entity_pos_t m_TargetX, m_TargetZ; // these values contain undefined junk if !HasTarget
virtual void Init(const CSimContext& UNUSED(context), const CParamNode& UNUSED(paramNode))
virtual void Init(const CSimContext& context, const CParamNode& UNUSED(paramNode))
{
m_Context = &context;
m_Speed = CFixed_23_8::FromInt(4);
m_HasTarget = false;
}
@ -56,6 +60,7 @@ public:
serialize.Bool("has target", m_HasTarget);
if (m_HasTarget)
{
// TODO: m_Path
serialize.NumberFixed_Unbounded("target x", m_TargetX);
serialize.NumberFixed_Unbounded("target z", m_TargetZ);
}
@ -88,12 +93,38 @@ public:
virtual void MoveToPoint(entity_pos_t x, entity_pos_t z)
{
m_HasTarget = true;
m_TargetX = x;
m_TargetZ = z;
CmpPtr<ICmpPathfinder> cmpPathfinder (*m_Context, SYSTEM_ENTITY);
if (cmpPathfinder.null())
return;
CmpPtr<ICmpPosition> cmpPosition(*m_Context, GetEntityId());
if (cmpPosition.null())
return;
CFixedVector3D pos = cmpPosition->GetPosition();
m_Path.m_Waypoints.clear();
u32 cost;
entity_pos_t r = entity_pos_t::FromInt(0); // TODO: should get this from the entity's size
if (cmpPathfinder->CanMoveStraight(pos.X, pos.Z, x, z, r, cost))
{
m_TargetX = x;
m_TargetZ = z;
m_HasTarget = true;
}
else
{
cmpPathfinder->SetDebugPath(pos.X, pos.Z, x, z);
cmpPathfinder->ComputePath(pos.X, pos.Z, x, z, m_Path);
if (!m_Path.m_Waypoints.empty())
PickNextWaypoint(pos);
}
}
void Move(const CSimContext& context, CFixed_23_8 dt);
void PickNextWaypoint(const CFixedVector3D& pos);
};
REGISTER_COMPONENT_TYPE(UnitMotion)
@ -113,30 +144,69 @@ void CCmpUnitMotion::Move(const CSimContext& context, CFixed_23_8 dt)
CFixedVector3D pos = cmpPosition->GetPosition();
pos.Y = CFixed_23_8::FromInt(0); // remove Y so it doesn't influence our distance calculations
// We want to move (at most) m_Speed*dt units from pos towards m_Target[XZ]
// We want to move (at most) m_Speed*dt units from pos towards the next waypoint
CFixedVector3D target(m_TargetX, CFixed_23_8::FromInt(0), m_TargetZ);
CFixedVector3D offset = target - pos;
// Face towards the target
entity_angle_t angle = atan2_approx(offset.X, offset.Z);
cmpPosition->SetYRotation(angle);
// If it's close, we can move there directly
if (offset.Length() <= maxdist)
while (dt > CFixed_23_8::FromInt(0))
{
cmpPosition->MoveTo(m_TargetX, m_TargetZ);
m_HasTarget = false;
return;
CFixedVector3D target(m_TargetX, CFixed_23_8::FromInt(0), m_TargetZ);
CFixedVector3D offset = target - pos;
// Face towards the target
entity_angle_t angle = atan2_approx(offset.X, offset.Z);
cmpPosition->SetYRotation(angle);
// If it's close, we can move there directly
if (offset.Length() <= maxdist)
{
// If we've reached the last waypoint, stop
if (m_Path.m_Waypoints.empty())
{
cmpPosition->MoveTo(target.X, target.Z);
m_HasTarget = false;
return;
}
// Otherwise, spend the rest of the time heading towards the next waypoint
dt = dt - dt.Multiply(offset.Length() / maxdist);
pos = target;
PickNextWaypoint(pos);
continue;
}
else
{
// Not close enough, so just move in the right direction
offset.Normalize(maxdist);
pos += offset;
cmpPosition->MoveTo(pos.X, pos.Z);
return;
}
}
}
void CCmpUnitMotion::PickNextWaypoint(const CFixedVector3D& pos)
{
// We can always pick the immediate next waypoint
debug_assert(!m_Path.m_Waypoints.empty());
m_TargetX = m_Path.m_Waypoints.back().x;
m_TargetZ = m_Path.m_Waypoints.back().z;
m_Path.m_Waypoints.pop_back();
m_HasTarget = true;
// To smooth the motion and avoid grid-constrained motion, we could try picking some
// subsequent waypoints instead, if we can reach them without hitting any obstacles
CmpPtr<ICmpPathfinder> cmpPathfinder (*m_Context, SYSTEM_ENTITY);
if (cmpPathfinder.null())
return;
for (size_t i = 0; i < 3 && !m_Path.m_Waypoints.empty(); ++i)
{
u32 cost;
entity_pos_t r = entity_pos_t::FromInt(0); // TODO: should get this from the entity's size
if (!cmpPathfinder->CanMoveStraight(pos.X, pos.Z, m_Path.m_Waypoints.back().x, m_Path.m_Waypoints.back().z, r, cost))
break;
m_TargetX = m_Path.m_Waypoints.back().x;
m_TargetZ = m_Path.m_Waypoints.back().z;
m_Path.m_Waypoints.pop_back();
}
// Visualise the path to the destination
CmpPtr<ICmpPathfinder> cmpPathfinder(context, SYSTEM_ENTITY);
if (!cmpPathfinder.null())
cmpPathfinder->SetDebugPath(pos.X, pos.Z, target.X, target.Z);
// Otherwise move in the right direction
offset.Normalize(maxdist);
pos += offset;
cmpPosition->MoveTo(pos.X, pos.Z);
}

18
source/simulation2/components/ICmpPathfinder.h
View File

@ -46,6 +46,10 @@ public:
u32 cost; // currently a meaningless number
};
/**
* Returned path.
* Waypoints are in *reverse* order (the earliest is at the back of the list)
*/
struct Path
{
std::vector<Waypoint> m_Waypoints;
@ -66,6 +70,20 @@ public:
virtual void RemoveShape(tag_t tag) = 0;
/**
* Determine whether a unit (of radius r) can move between the given points in a straight line,
* without hitting any obstacles.
* This is based on the exact list of shapes, not the grid approximation.
* This should be used as a shortcut to avoid using the pathfinding algorithm in simple cases,
* and for more refined movement along the found paths.
*/
virtual bool CanMoveStraight(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, entity_pos_t r, u32& cost) = 0;
/**
* Compute a path between the given points, and return the set of waypoints.
*/
virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, entity_pos_t x1, entity_pos_t z1, Path& ret) = 0;
/**
* Compute a path between the given points, and draw the latest such path as a terrain overlay.
*/