From e0ed8a1629661bcc7b529f745ce44404d7be41e8 Mon Sep 17 00:00:00 2001 From: Ykkrosh Date: Sat, 30 Jan 2010 13:11:58 +0000 Subject: [PATCH] # Pathfinder updates More hacks so units follow paths relatively smoothly, and to avoid pathfinding in simple situations This was SVN commit r7296. --- source/maths/Fixed.h | 9 ++ source/maths/FixedVector2D.h | 121 +++++++++++++++++ source/maths/tests/test_Fixed.h | 9 ++ .../simulation2/components/CCmpPathfinder.cpp | 67 +++++++++- .../simulation2/components/CCmpUnitMotion.cpp | 124 ++++++++++++++---- .../simulation2/components/ICmpPathfinder.h | 18 +++ 6 files changed, 317 insertions(+), 31 deletions(-) create mode 100644 source/maths/FixedVector2D.h diff --git a/source/maths/Fixed.h b/source/maths/Fixed.h index 694a655845..6dfb4970ed 100644 --- a/source/maths/Fixed.h +++ b/source/maths/Fixed.h @@ -19,6 +19,7 @@ #define INCLUDED_FIXED #include "lib/types.h" +#include "maths/Sqrt.h" template 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)); + } }; /** diff --git a/source/maths/FixedVector2D.h b/source/maths/FixedVector2D.h new file mode 100644 index 0000000000..929f8382ed --- /dev/null +++ b/source/maths/FixedVector2D.h @@ -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 . + */ + +#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 diff --git a/source/maths/tests/test_Fixed.h b/source/maths/tests/test_Fixed.h index fd1bd93b7c..135a3b9445 100644 --- a/source/maths/tests/test_Fixed.h +++ b/source/maths/tests/test_Fixed.h @@ -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; diff --git a/source/simulation2/components/CCmpPathfinder.cpp b/source/simulation2/components/CCmpPathfinder.cpp index d56e2bd17f..0fcd2d4f73 100644 --- a/source/simulation2/components/CCmpPathfinder.cpp +++ b/source/simulation2/components/CCmpPathfinder.cpp @@ -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::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::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; diff --git a/source/simulation2/components/CCmpUnitMotion.cpp b/source/simulation2/components/CCmpUnitMotion.cpp index 3cead36ad9..5dce1c2008 100644 --- a/source/simulation2/components/CCmpUnitMotion.cpp +++ b/source/simulation2/components/CCmpUnitMotion.cpp @@ -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 cmpPathfinder (*m_Context, SYSTEM_ENTITY); + if (cmpPathfinder.null()) + return; + + CmpPtr 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 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 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); } diff --git a/source/simulation2/components/ICmpPathfinder.h b/source/simulation2/components/ICmpPathfinder.h index 1de9924002..93c25c1875 100644 --- a/source/simulation2/components/ICmpPathfinder.h +++ b/source/simulation2/components/ICmpPathfinder.h @@ -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 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. */