From d9f307839693675bcba7ac80a2434b14febafad8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rasmus=20Br=C3=B6nneg=C3=A5rd?= <1162652+rasmusgo@users.noreply.github.com> Date: Mon, 31 Jan 2022 23:25:00 +0100 Subject: [PATCH 1/9] Replace beam search with breadth-first search --- src/object/task/taskgoto.cpp | 274 ++++++++++++++++++++--------------- src/object/task/taskgoto.h | 13 +- 2 files changed, 168 insertions(+), 119 deletions(-) diff --git a/src/object/task/taskgoto.cpp b/src/object/task/taskgoto.cpp index 6cdaed8c..cba8826e 100644 --- a/src/object/task/taskgoto.cpp +++ b/src/object/task/taskgoto.cpp @@ -49,9 +49,10 @@ const float FLY_DEF_HEIGHT = 50.0f; // default flying height // Settings that define goto() accuracy: const float BM_DIM_STEP = 5.0f; // Size of one pixel on the bitmap. Setting 5 means that 5x5 square (in game units) will be represented by 1 px on the bitmap. Decreasing this value will make a bigger bitmap, and may increase accuracy. TODO: Check how it actually impacts goto() accuracy const float BEAM_ACCURACY = 5.0f; // higher value = more accurate, but slower -const float SAFETY_MARGIN = 0.5f; // Smallest distance between two objects. Smaller = less "no route to destination", but higher probability of collisions between objects. +const float SAFETY_MARGIN = 1.5f; // Smallest distance between two objects. Smaller = less "no route to destination", but higher probability of collisions between objects. // Changing SAFETY_MARGIN (old value was 4.0f) seems to have fixed many issues with goto(). TODO: maybe we could make it even smaller? Did changing it introduce any new bugs? +const int NB_ITER = 2000; // Maximum number of iterations you have the right to make before temporarily interrupt in order not to lower the framerate. @@ -126,7 +127,7 @@ bool CTaskGoto::EventProcess(const Event &event) if (a || b) { Gfx::Color c = Gfx::Color(0.0f, 0.0f, 0.0f, 1.0f); - if (b) c = Gfx::Color(0.0f, 0.0f, 1.0f, 1.0f); + if (b) c = Gfx::Color(0.0f, 1.0f, 1.0f, 1.0f); debugImage->SetPixel(Math::IntPoint(x, y), c); } } @@ -1688,6 +1689,8 @@ void CTaskGoto::BeamInit() m_bmIter[i] = -1; } m_bmStep = 0; + m_bfsQueueBegin = 0; + m_bfsQueueEnd = 0; } // Calculates points and passes to go from start to goal. @@ -1701,138 +1704,166 @@ void CTaskGoto::BeamInit() Error CTaskGoto::BeamSearch(const Math::Vector &start, const Math::Vector &goal, float goalRadius) { - float step, len; - int nbIter; - m_bmStep ++; - len = Math::DistanceProjected(start, goal); - step = len/BEAM_ACCURACY; - if ( step < BM_DIM_STEP*2.1f ) step = BM_DIM_STEP*2.1f; - if ( step > 20.0f ) step = 20.0f; - nbIter = 200; // in order not to lower the framerate - m_bmIterCounter = 0; - return BeamExplore(start, start, goal, goalRadius, 165.0f*Math::PI/180.0f, 22, step, 0, nbIter); -} + // Relative postion and distance to neighbors. + static const int dXs[8] = {-1, 0, 1, -1, 1, -1, 0, 1}; + static const int dYs[8] = {-1, -1, -1, 0, 0, 1, 1, 1}; + // static const float dDist[8] = {M_SQRT2, 1.0f, M_SQRT2, 1.0f, 1.0f, M_SQRT2, 1.0f, M_SQRT2}; + static const int32_t dDist[8] = {7, 5, 7, 5, 5, 7, 5, 7}; -// prevPos: previous position -// curPos: current position -// goalPos: position that seeks to achieve -// angle: angle to the goal we explores -// nbDiv: number of subdivisions being done with angle -// step length of a step -// i number of recursions made -// nbIter maximum number of iterations you have the right to make before temporarily interrupt + const int startX = static_cast((start.x+1600.0f)/BM_DIM_STEP); + const int startY = static_cast((start.z+1600.0f)/BM_DIM_STEP); + const int goalX = static_cast((goal.x+1600.0f)/BM_DIM_STEP); + const int goalY = static_cast((goal.z+1600.0f)/BM_DIM_STEP); -Error CTaskGoto::BeamExplore(const Math::Vector &prevPos, const Math::Vector &curPos, - const Math::Vector &goalPos, float goalRadius, - float angle, int nbDiv, float step, - int i, int nbIter) -{ - Math::Vector newPos; - Error ret; - int iDiv, iClear, iLar; - - iLar = 0; - if ( i >= MAXPOINTS ) return ERR_GOTO_ITER; // too many recursions - - m_bmTotal = i; - - if ( m_bmIter[i] == -1 ) + if (m_bfsQueueEnd == 0) // New search { - m_bmIter[i] = 0; - - if ( i == 0 ) + if (startX == goalX && startY == goalY) { - m_bmPoints[i] = curPos; + m_bmPoints[0] = start; + m_bmPoints[1] = goal; + m_bmTotal = 1; + return ERR_OK; } - else + // Enqueue the goal node + if ( goalX >= 0 && goalX < m_bmSize && + goalY >= 0 && goalY < m_bmSize ) { - if ( !BitmapTestLine(prevPos, curPos, angle/nbDiv, true) ) return ERR_GOTO_IMPOSSIBLE; + const int indexInMap = goalY * m_bmSize + goalX; + m_bfsDistances[indexInMap] = 0.0; + m_bfsQueue[m_bfsQueueEnd++] = indexInMap; + BitmapSetDot(1, goalX, goalY); // Mark as enqueued + } - m_bmPoints[i] = curPos; - - if ( Math::DistanceProjected(curPos, goalPos)-goalRadius <= step ) + // Enqueue nodes around the goal + if (goalRadius > 0.0f) + { + const int minX = std::max(0, static_cast((goal.x-goalRadius+1600.0f)/BM_DIM_STEP)); + const int minY = std::max(0, static_cast((goal.z-goalRadius+1600.0f)/BM_DIM_STEP)); + const int maxX = std::min(m_bmSize-1, static_cast((goal.x+goalRadius+1600.0f)/BM_DIM_STEP)); + const int maxY = std::min(m_bmSize-1, static_cast((goal.z+goalRadius+1600.0f)/BM_DIM_STEP)); + for (int y = minY; y <= maxY; ++y) { - if ( goalRadius == 0.0f ) + for (int x = minX; x <= maxX; ++x) { - newPos = goalPos; - } - else - { - newPos = BeamPoint(curPos, goalPos, 0, Math::DistanceProjected(curPos, goalPos)-goalRadius); - } - if ( BitmapTestLine(curPos, newPos, angle/nbDiv, false) ) - { - m_bmPoints[i+1] = newPos; - m_bmTotal = i+1; - return ERR_OK; + float floatX = (x + 0.5f) * BM_DIM_STEP - 1600.0f; + float floatY = (y + 0.5f) * BM_DIM_STEP - 1600.0f; + if (std::hypot(floatX-goal.x, floatY-goal.z) <= goalRadius && + BitmapTestDotIsVisitable(x, y) && + !BitmapTestDot(1, x, y)) + { + const int indexInMap = y * m_bmSize + x; + m_bfsDistances[indexInMap] = 0.0; + m_bfsQueue[m_bfsQueueEnd++] = indexInMap; + BitmapSetDot(1, x, y); // Mark as enqueued + } } } } } - if ( iLar >= m_bmIter[i] ) + m_bmIterCounter = 0; + + while (m_bfsQueueBegin != m_bfsQueueEnd) { - newPos = BeamPoint(curPos, goalPos, 0, step); - ret = BeamExplore(curPos, newPos, goalPos, goalRadius, angle, nbDiv, step, i+1, nbIter); - if ( ret != ERR_GOTO_IMPOSSIBLE ) return ret; - m_bmIter[i] = iLar+1; - for ( iClear=i+1 ; iClear<=MAXPOINTS ; iClear++ ) m_bmIter[iClear] = -1; + // Pop a node from the queue + const uint32_t indexInMap = m_bfsQueue[m_bfsQueueBegin++]; + const int x = indexInMap % m_bmSize; + const int y = indexInMap / m_bmSize; + const int32_t distance = m_bfsDistances[indexInMap]; + + if (x == startX && y == startY) + { + // We have reached the start. + // Follow decreasing distances to find the path. + m_bmPoints[0] = start; + int btX = x; + int btY = y; + for (m_bmTotal = 1; m_bmTotal < MAXPOINTS; ++m_bmTotal) + { + int bestX = -1; + int bestY = -1; + int32_t bestDistance = std::numeric_limits::max(); + for (int i = 0; i < 8; ++i) + { + const int nX = btX + dXs[i]; + const int nY = btY + dYs[i]; + if (!BitmapTestDot(1, nX, nY)) continue; + const int32_t nDistance = m_bfsDistances[nY * m_bmSize + nX]; + if (nDistance < bestDistance) + { + bestX = nX; + bestY = nY; + bestDistance = nDistance; + } + } + if (bestX == -1) + { + GetLogger()->Error("Failed to find node parent\n"); + return ERR_GOTO_ITER; + } + btX = bestX; + btY = bestY; + if (btX == goalX && btY == goalY) + { + m_bmPoints[m_bmTotal] = goal; + break; + } + m_bmPoints[m_bmTotal].x = (btX + 0.5f) * BM_DIM_STEP - 1600.f; + m_bmPoints[m_bmTotal].z = (btY + 0.5f) * BM_DIM_STEP - 1600.f; + + if (bestDistance == 0) + { + break; + } + } + // std::reverse(m_bmPoints, m_bmPoints + m_bmTotal); + + GetLogger()->Info("Found path to goal with %d nodes\n", m_bmTotal + 1); + return ERR_OK; + } + + // Expand the node + for (int i = 0; i < 8; ++i) + { + const int nX = x + dXs[i]; + const int nY = y + dYs[i]; + if (BitmapTestDotIsVisitable(nX, nY)) + { + if (BitmapTestDot(1, nX, nY)) + { + // We have seen this node before. + // Update distance without adding it to the queue. + const int neighborIndexInMap = nY * m_bmSize + nX; + m_bfsDistances[neighborIndexInMap] = std::min( + m_bfsDistances[neighborIndexInMap], + distance + dDist[i]); + } + else + { + // Enqueue this neighbor + const int neighborIndexInMap = nY * m_bmSize + nX; + m_bfsDistances[neighborIndexInMap] = distance + dDist[i]; + m_bfsQueue[m_bfsQueueEnd++] = neighborIndexInMap; + BitmapSetDot(1, nX, nY); // Mark as enqueued + if (m_bfsQueueEnd > m_bmSize * m_bmSize) + { + GetLogger()->Error("Queue is full\n"); + return ERR_GOTO_ITER; + } + } + } + } + + // Limit the number of iterations per frame. m_bmIterCounter ++; - if ( m_bmIterCounter >= nbIter ) return ERR_CONTINUE; - } - iLar ++; - - for ( iDiv=1 ; iDiv<=nbDiv ; iDiv++ ) - { - if ( iLar >= m_bmIter[i] ) - { - newPos = BeamPoint(curPos, goalPos, angle*iDiv/nbDiv, step); - ret = BeamExplore(curPos, newPos, goalPos, goalRadius, angle, nbDiv, step, i+1, nbIter); - if ( ret != ERR_GOTO_IMPOSSIBLE ) return ret; - m_bmIter[i] = iLar+1; - for ( iClear=i+1 ; iClear<=MAXPOINTS ; iClear++ ) m_bmIter[iClear] = -1; - m_bmIterCounter ++; - if ( m_bmIterCounter >= nbIter ) return ERR_CONTINUE; - } - iLar ++; - - if ( iLar >= m_bmIter[i] ) - { - newPos = BeamPoint(curPos, goalPos, -angle*iDiv/nbDiv, step); - ret = BeamExplore(curPos, newPos, goalPos, goalRadius, angle, nbDiv, step, i+1, nbIter); - if ( ret != ERR_GOTO_IMPOSSIBLE ) return ret; - m_bmIter[i] = iLar+1; - for ( iClear=i+1 ; iClear<=MAXPOINTS ; iClear++ ) m_bmIter[iClear] = -1; - m_bmIterCounter ++; - if ( m_bmIterCounter >= nbIter ) return ERR_CONTINUE; - } - iLar ++; + if ( m_bmIterCounter >= NB_ITER ) return ERR_CONTINUE; } return ERR_GOTO_IMPOSSIBLE; } -// Is a right "start-goal". Calculates the point located at the distance "step" -// from the point "start" and an angle "angle" with the right. - -Math::Vector CTaskGoto::BeamPoint(const Math::Vector &startPoint, - const Math::Vector &goalPoint, - float angle, float step) -{ - Math::Vector resPoint; - float goalAngle; - - goalAngle = Math::RotateAngle(goalPoint.x-startPoint.x, goalPoint.z-startPoint.z); - - resPoint.x = startPoint.x + cosf(goalAngle+angle)*step; - resPoint.z = startPoint.z + sinf(goalAngle+angle)*step; - resPoint.y = 0.0f; - - return resPoint; -} - // Tests if a path along a straight line is possible. bool CTaskGoto::BitmapTestLine(const Math::Vector &start, const Math::Vector &goal, @@ -2094,10 +2125,11 @@ void CTaskGoto::BitmapTerrain(int minx, int miny, int maxx, int maxy) bool CTaskGoto::BitmapOpen() { - BitmapClose(); - m_bmSize = static_cast(3200.0f/BM_DIM_STEP); - m_bmArray = MakeUniqueArray(m_bmSize*m_bmSize/8*2); + if (m_bmArray.get() == nullptr) m_bmArray = MakeUniqueArray(m_bmSize*m_bmSize/8*2); + memset(m_bmArray.get(), 0, m_bmSize*m_bmSize/8*2); + if (m_bfsDistances.get() == nullptr) m_bfsDistances = MakeUniqueArray(m_bmSize*m_bmSize); + if (m_bfsQueue.get() == nullptr) m_bfsQueue = MakeUniqueArray(m_bmSize*m_bmSize); m_bmChanged = true; m_bmOffset = m_bmSize/2; @@ -2204,3 +2236,17 @@ bool CTaskGoto::BitmapTestDot(int rank, int x, int y) return m_bmArray[rank*m_bmLine*m_bmSize + m_bmLine*y + x/8] & (1<= m_bmSize || + y < 0 || y >= m_bmSize ) return false; + + if ( x < m_bmMinX || x > m_bmMaxX || + y < m_bmMinY || y > m_bmMaxY ) + { + BitmapTerrain(x-10,y-10, x+10,y+10); // remade a layer + } + + return !(m_bmArray[m_bmLine*y + x/8] & (1< m_bmArray; // bit table + std::unique_ptr m_bmArray; // Bit table + std::unique_ptr m_bfsDistances; // Distances to the goal for breadth-first search. + std::unique_ptr m_bfsQueue; // Nodes in the queue. Stored as indices. + int m_bfsQueueBegin = 0; // Front of the queue. This is the next node to be expanded. + int m_bfsQueueEnd = 0; // Back of the queue. This is where nodes are inserted. int m_bmMinX = 0, m_bmMinY = 0; int m_bmMaxX = 0, m_bmMaxY = 0; - int m_bmTotal = 0; // number of points in m_bmPoints + int m_bmTotal = 0; // index of final point in m_bmPoints int m_bmIndex = 0; // index in m_bmPoints Math::Vector m_bmPoints[MAXPOINTS+2]; signed char m_bmIter[MAXPOINTS+2] = {}; From e2d4dd6995a39f221229bfc56325ab11c8e37a31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rasmus=20Br=C3=B6nneg=C3=A5rd?= <1162652+rasmusgo@users.noreply.github.com> Date: Wed, 2 Feb 2022 20:15:30 +0100 Subject: [PATCH 2/9] Use ray tracing algorithm in BitmapTestLine --- src/object/task/taskgoto.cpp | 91 ++++++++++++++++++++---------------- src/object/task/taskgoto.h | 2 +- 2 files changed, 52 insertions(+), 41 deletions(-) diff --git a/src/object/task/taskgoto.cpp b/src/object/task/taskgoto.cpp index cba8826e..ce891f5d 100644 --- a/src/object/task/taskgoto.cpp +++ b/src/object/task/taskgoto.cpp @@ -1636,7 +1636,7 @@ int CTaskGoto::BeamShortcut() for ( i=m_bmTotal ; i>=m_bmIndex+2 ; i-- ) // tries from the last { - if ( BitmapTestLine(m_bmPoints[m_bmIndex], m_bmPoints[i], 0.0f, false) ) + if ( BitmapTestLine(m_bmPoints[m_bmIndex], m_bmPoints[i]) ) { return i; // bingo, found } @@ -1866,62 +1866,73 @@ Error CTaskGoto::BeamSearch(const Math::Vector &start, const Math::Vector &goal, // Tests if a path along a straight line is possible. -bool CTaskGoto::BitmapTestLine(const Math::Vector &start, const Math::Vector &goal, - float stepAngle, bool bSecond) +bool CTaskGoto::BitmapTestLine(const Math::Vector &start, const Math::Vector &goal) { - Math::Vector pos, inc; - float dist, step; - float distNoB2; - int i, max, x, y; - if ( m_bmArray == nullptr ) return true; - dist = Math::DistanceProjected(start, goal); - if ( dist == 0.0f ) return true; - step = BM_DIM_STEP*0.5f; + const Math::Point startInGrid = Math::Point((start.x+1600.0f)/BM_DIM_STEP, (start.z+1600.0f)/BM_DIM_STEP); + const Math::Point goalInGrid = Math::Point((goal.x+1600.0f)/BM_DIM_STEP, (goal.z+1600.0f)/BM_DIM_STEP); - inc.x = (goal.x-start.x)*step/dist; - inc.z = (goal.z-start.z)*step/dist; + const int startXInt = static_cast(startInGrid.x); + const int startYInt = static_cast(startInGrid.y); + const int goalXInt = static_cast(goalInGrid.x); + const int goalYInt = static_cast(goalInGrid.y); - pos = start; - - if ( bSecond ) + if (startXInt == goalXInt && startYInt == goalYInt) { - x = static_cast((pos.x+1600.0f)/BM_DIM_STEP); - y = static_cast((pos.z+1600.0f)/BM_DIM_STEP); - BitmapSetDot(1, x, y); // puts the flag as the starting point + return true; } - max = static_cast(dist/step); - if ( max == 0 ) max = 1; - distNoB2 = BM_DIM_STEP*sqrtf(2.0f)/sinf(stepAngle); - for ( i=0 ; i 0.0f ? 1 : -1; + const int stepY = dirInGrid.y > 0.0f ? 1 : -1; + + // At what t does the ray enter the next cell? + float tMaxX = + dirInGrid.x > 0.0 ? (std::floor(startInGrid.x) - startInGrid.x + 1) / dirInGrid.x : + dirInGrid.x < 0.0 ? (std::floor(startInGrid.x) - startInGrid.x) / dirInGrid.x : + std::numeric_limits::infinity(); + float tMaxY = + dirInGrid.y > 0.0 ? (std::floor(startInGrid.y) - startInGrid.y + 1) / dirInGrid.y : + dirInGrid.y < 0.0 ? (std::floor(startInGrid.y) - startInGrid.y) / dirInGrid.y : + std::numeric_limits::infinity(); + + // How much t is needed to step from one column/row to another? + // stepX = dir.x * t + // stepX / dir.x = t + const float tDeltaX = static_cast(stepX) / dirInGrid.x; + const float tDeltaY = static_cast(stepY) / dirInGrid.y; + + // Traverse the grid + const int numIntersections = + std::abs(goalXInt - startXInt) + + std::abs(goalYInt - startYInt); + int x = startXInt; + int y = startYInt; + + for ( int i = 0; i < numIntersections; ++i ) { - if ( i == max-1 ) + if ( tMaxX < tMaxY ) { - pos = goal; // tests the point of arrival + tMaxX += tDeltaX; + x += stepX; } else { - pos.x += inc.x; - pos.z += inc.z; + tMaxY += tDeltaY; + y += stepY; } - - x = static_cast((pos.x+1600.0f)/BM_DIM_STEP); - y = static_cast((pos.z+1600.0f)/BM_DIM_STEP); - - if ( bSecond ) + if ( BitmapTestDot(0, x, y) ) { - if ( i > 2 && BitmapTestDot(1, x, y) ) return false; - - if ( step*(i+1) > distNoB2 && i < max-2 ) - { - BitmapSetDot(1, x, y); - } + return false; } - - if ( BitmapTestDot(0, x, y) ) return false; } + return true; } diff --git a/src/object/task/taskgoto.h b/src/object/task/taskgoto.h index 4d18135d..118a372a 100644 --- a/src/object/task/taskgoto.h +++ b/src/object/task/taskgoto.h @@ -104,7 +104,7 @@ protected: void BeamInit(); Error BeamSearch(const Math::Vector &start, const Math::Vector &goal, float goalRadius); - bool BitmapTestLine(const Math::Vector &start, const Math::Vector &goal, float stepAngle, bool bSecond); + bool BitmapTestLine(const Math::Vector &start, const Math::Vector &goal); void BitmapObject(); void BitmapTerrain(const Math::Vector &min, const Math::Vector &max); void BitmapTerrain(int minx, int miny, int maxx, int maxy); From a7b294025e62b6e267c0ae44965fe362551f575a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rasmus=20Br=C3=B6nneg=C3=A5rd?= <1162652+rasmusgo@users.noreply.github.com> Date: Tue, 1 Feb 2022 21:32:13 +0100 Subject: [PATCH 3/9] Rename BeamSearch to PathFindingSearch etc --- src/object/task/taskgoto.cpp | 24 ++++++++++++------------ src/object/task/taskgoto.h | 8 ++++---- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/object/task/taskgoto.cpp b/src/object/task/taskgoto.cpp index ce891f5d..3e9a06f5 100644 --- a/src/object/task/taskgoto.cpp +++ b/src/object/task/taskgoto.cpp @@ -222,7 +222,7 @@ bool CTaskGoto::EventProcess(const Event &event) if ( m_bmCargoObject->GetType() == OBJECT_BASE ) dist = 12.0f; } - ret = BeamSearch(pos, goal, dist); + ret = PathFindingSearch(pos, goal, dist); if ( ret == ERR_OK ) { if ( m_physics->GetLand() ) m_phase = TGP_BEAMWCOLD; @@ -346,7 +346,7 @@ bool CTaskGoto::EventProcess(const Event &event) { m_physics->SetMotorSpeedX(0.0f); // stops the advance m_physics->SetMotorSpeedZ(0.0f); // stops the rotation - BeamStart(); // we start all + PathFindingStart(); // we start all return true; } @@ -790,7 +790,7 @@ Error CTaskGoto::Start(Math::Vector goal, float altitude, } } - BeamStart(); + PathFindingStart(); if ( m_bmCargoObject == nullptr ) { @@ -826,7 +826,7 @@ Error CTaskGoto::IsEnded() { m_physics->SetMotorSpeedX(0.0f); // stops the advance m_physics->SetMotorSpeedZ(0.0f); // stops the rotation - BeamInit(); + PathFindingInit(); m_phase = TGP_BEAMSEARCH; // will seek the path } return ERR_CONTINUE; @@ -888,7 +888,7 @@ Error CTaskGoto::IsEnded() m_physics->SetMotorSpeedX(0.0f); // stops the advance m_physics->SetMotorSpeedZ(0.0f); // stops the rotation - m_bmIndex = BeamShortcut(); + m_bmIndex = PathFindingShortcut(); if ( m_bmIndex > m_bmTotal ) { @@ -1630,7 +1630,7 @@ void CTaskGoto::ComputeFlyingRepulse(float &dir) // Among all of the following, seek if there is one allowing to go directly to the crow flies. // If yes, skip all the unnecessary intermediate points. -int CTaskGoto::BeamShortcut() +int CTaskGoto::PathFindingShortcut() { int i; @@ -1647,7 +1647,7 @@ int CTaskGoto::BeamShortcut() // That's the big start. -void CTaskGoto::BeamStart() +void CTaskGoto::PathFindingStart() { Math::Vector min, max; @@ -1673,14 +1673,14 @@ void CTaskGoto::BeamStart() { m_physics->SetMotorSpeedX(0.0f); // stops the advance m_physics->SetMotorSpeedZ(0.0f); // stops the rotation - BeamInit(); + PathFindingInit(); m_phase = TGP_BEAMSEARCH; // will seek the path } } -// Initialization before the first BeamSearch. +// Initialization before the first PathFindingSearch. -void CTaskGoto::BeamInit() +void CTaskGoto::PathFindingInit() { int i; @@ -1701,8 +1701,8 @@ void CTaskGoto::BeamInit() // ERR_CONTINUE if not done yet // goalRadius: distance at which we must approach the goal -Error CTaskGoto::BeamSearch(const Math::Vector &start, const Math::Vector &goal, - float goalRadius) +Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector &goal, + float goalRadius) { m_bmStep ++; diff --git a/src/object/task/taskgoto.h b/src/object/task/taskgoto.h index 118a372a..26bc52e3 100644 --- a/src/object/task/taskgoto.h +++ b/src/object/task/taskgoto.h @@ -99,10 +99,10 @@ protected: void ComputeRepulse(Math::Point &dir); void ComputeFlyingRepulse(float &dir); - int BeamShortcut(); - void BeamStart(); - void BeamInit(); - Error BeamSearch(const Math::Vector &start, const Math::Vector &goal, float goalRadius); + int PathFindingShortcut(); + void PathFindingStart(); + void PathFindingInit(); + Error PathFindingSearch(const Math::Vector &start, const Math::Vector &goal, float goalRadius); bool BitmapTestLine(const Math::Vector &start, const Math::Vector &goal); void BitmapObject(); From 2b8d580355f0b6745c5f148f7894e28e45bb2161 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rasmus=20Br=C3=B6nneg=C3=A5rd?= <1162652+rasmusgo@users.noreply.github.com> Date: Tue, 8 Feb 2022 21:12:33 +0100 Subject: [PATCH 4/9] goto with Dial's algo (Dijkstra's + bucket sort) --- src/object/task/taskgoto.cpp | 95 ++++++++++++++++++++++++------------ src/object/task/taskgoto.h | 11 +++-- 2 files changed, 73 insertions(+), 33 deletions(-) diff --git a/src/object/task/taskgoto.cpp b/src/object/task/taskgoto.cpp index 3e9a06f5..bb4203a1 100644 --- a/src/object/task/taskgoto.cpp +++ b/src/object/task/taskgoto.cpp @@ -1689,8 +1689,15 @@ void CTaskGoto::PathFindingInit() m_bmIter[i] = -1; } m_bmStep = 0; - m_bfsQueueBegin = 0; - m_bfsQueueEnd = 0; + for (auto& bucket : m_bfsQueue) + { + bucket.clear(); + } + m_bfsQueueMin = 0; + m_bfsQueueCountPushed = 0; + m_bfsQueueCountPopped = 0; + m_bfsQueueCountRepeated = 0; + m_bfsQueueCountSkipped = 0; } // Calculates points and passes to go from start to goal. @@ -1709,7 +1716,7 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector // Relative postion and distance to neighbors. static const int dXs[8] = {-1, 0, 1, -1, 1, -1, 0, 1}; static const int dYs[8] = {-1, -1, -1, 0, 0, 1, 1, 1}; - // static const float dDist[8] = {M_SQRT2, 1.0f, M_SQRT2, 1.0f, 1.0f, M_SQRT2, 1.0f, M_SQRT2}; + // These are the costs of the edges. They must be less than the number of buckets in the queue. static const int32_t dDist[8] = {7, 5, 7, 5, 5, 7, 5, 7}; const int startX = static_cast((start.x+1600.0f)/BM_DIM_STEP); @@ -1717,7 +1724,7 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector const int goalX = static_cast((goal.x+1600.0f)/BM_DIM_STEP); const int goalY = static_cast((goal.z+1600.0f)/BM_DIM_STEP); - if (m_bfsQueueEnd == 0) // New search + if (m_bfsQueueCountPushed == 0) // New search { if (startX == goalX && startY == goalY) { @@ -1731,8 +1738,9 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector goalY >= 0 && goalY < m_bmSize ) { const int indexInMap = goalY * m_bmSize + goalX; - m_bfsDistances[indexInMap] = 0.0; - m_bfsQueue[m_bfsQueueEnd++] = indexInMap; + m_bfsDistances[indexInMap] = 0; + m_bfsQueue[0].push_back(indexInMap); + m_bfsQueueCountPushed += 1; BitmapSetDot(1, goalX, goalY); // Mark as enqueued } @@ -1754,8 +1762,9 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector !BitmapTestDot(1, x, y)) { const int indexInMap = y * m_bmSize + x; - m_bfsDistances[indexInMap] = 0.0; - m_bfsQueue[m_bfsQueueEnd++] = indexInMap; + m_bfsDistances[indexInMap] = 0; + m_bfsQueue[0].push_back(indexInMap); + m_bfsQueueCountPushed += 1; BitmapSetDot(1, x, y); // Mark as enqueued } } @@ -1765,13 +1774,30 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector m_bmIterCounter = 0; - while (m_bfsQueueBegin != m_bfsQueueEnd) + while (m_bfsQueueCountPushed != m_bfsQueueCountPopped) { // Pop a node from the queue - const uint32_t indexInMap = m_bfsQueue[m_bfsQueueBegin++]; + while (m_bfsQueue[m_bfsQueueMin % 8].empty()) + { + m_bfsQueueMin += 1; + } + auto& bucket = m_bfsQueue[m_bfsQueueMin % 8]; + const uint32_t indexInMap = bucket.back(); + bucket.pop_back(); + m_bfsQueueCountPopped += 1; + + const int32_t distance = m_bfsDistances[indexInMap]; + + if (distance != m_bfsQueueMin) + { + m_bfsQueueCountSkipped += 1; + GetLogger()->Debug("Skipping node with mismatched distance, distance: %d, m_bfsQueueMin: %d\n", + distance, m_bfsQueueMin); + continue; + } + const int x = indexInMap % m_bmSize; const int y = indexInMap / m_bmSize; - const int32_t distance = m_bfsDistances[indexInMap]; if (x == startX && y == startY) { @@ -1800,7 +1826,7 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector } if (bestX == -1) { - GetLogger()->Error("Failed to find node parent\n"); + GetLogger()->Debug("Failed to find node parent\n"); return ERR_GOTO_ITER; } btX = bestX; @@ -1820,7 +1846,15 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector } // std::reverse(m_bmPoints, m_bmPoints + m_bmTotal); - GetLogger()->Info("Found path to goal with %d nodes\n", m_bmTotal + 1); + GetLogger()->Debug("Found path to goal with %d nodes\n", m_bmTotal + 1); + GetLogger()->Debug("m_bmStep: %d\n", m_bmStep); + GetLogger()->Debug("m_bfsQueueMin: %d mod 8 = %d\n", m_bfsQueueMin, m_bfsQueueMin % 8); + GetLogger()->Debug("m_bfsQueueCountPushed: %d\n", m_bfsQueueCountPushed); + GetLogger()->Debug("m_bfsQueueCountPopped: %d\n", m_bfsQueueCountPopped); + GetLogger()->Debug("m_bfsQueueCountRepeated: %d\n", m_bfsQueueCountRepeated); + GetLogger()->Debug("m_bfsQueueCountSkipped: %d\n", m_bfsQueueCountSkipped); + GetLogger()->Debug("m_bfsQueue sizes:\n 0: %lu\n 1: %lu\n 2: %lu\n 3: %lu\n 4: %lu\n 5: %lu\n 6: %lu\n 7: %lu\n", + m_bfsQueue[0].size(), m_bfsQueue[1].size(), m_bfsQueue[2].size(), m_bfsQueue[3].size(), m_bfsQueue[4].size(), m_bfsQueue[5].size(), m_bfsQueue[6].size(), m_bfsQueue[7].size()); return ERR_OK; } @@ -1831,28 +1865,26 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector const int nY = y + dYs[i]; if (BitmapTestDotIsVisitable(nX, nY)) { + const int neighborIndexInMap = nY * m_bmSize + nX; + const int32_t newDistance = distance + dDist[i]; if (BitmapTestDot(1, nX, nY)) { // We have seen this node before. - // Update distance without adding it to the queue. - const int neighborIndexInMap = nY * m_bmSize + nX; - m_bfsDistances[neighborIndexInMap] = std::min( - m_bfsDistances[neighborIndexInMap], - distance + dDist[i]); - } - else - { - // Enqueue this neighbor - const int neighborIndexInMap = nY * m_bmSize + nX; - m_bfsDistances[neighborIndexInMap] = distance + dDist[i]; - m_bfsQueue[m_bfsQueueEnd++] = neighborIndexInMap; - BitmapSetDot(1, nX, nY); // Mark as enqueued - if (m_bfsQueueEnd > m_bmSize * m_bmSize) + // Only enqueue previously seen nodes if this is a shorter path. + if (newDistance < m_bfsDistances[neighborIndexInMap]) { - GetLogger()->Error("Queue is full\n"); - return ERR_GOTO_ITER; + m_bfsQueueCountRepeated += 1; + } + else + { + continue; } } + // Enqueue this neighbor + m_bfsDistances[neighborIndexInMap] = newDistance; + m_bfsQueue[newDistance % 8].push_back(neighborIndexInMap); + m_bfsQueueCountPushed += 1; + BitmapSetDot(1, nX, nY); // Mark as enqueued } } @@ -2140,7 +2172,10 @@ bool CTaskGoto::BitmapOpen() if (m_bmArray.get() == nullptr) m_bmArray = MakeUniqueArray(m_bmSize*m_bmSize/8*2); memset(m_bmArray.get(), 0, m_bmSize*m_bmSize/8*2); if (m_bfsDistances.get() == nullptr) m_bfsDistances = MakeUniqueArray(m_bmSize*m_bmSize); - if (m_bfsQueue.get() == nullptr) m_bfsQueue = MakeUniqueArray(m_bmSize*m_bmSize); + for (auto& bucket : m_bfsQueue) + { + bucket.reserve(256); + } m_bmChanged = true; m_bmOffset = m_bmSize/2; diff --git a/src/object/task/taskgoto.h b/src/object/task/taskgoto.h index 26bc52e3..a95b73e4 100644 --- a/src/object/task/taskgoto.h +++ b/src/object/task/taskgoto.h @@ -23,7 +23,9 @@ #include "math/vector.h" +#include #include +#include namespace Math { @@ -142,9 +144,12 @@ protected: int m_bmLine = 0; // increment line m_bmSize/8 std::unique_ptr m_bmArray; // Bit table std::unique_ptr m_bfsDistances; // Distances to the goal for breadth-first search. - std::unique_ptr m_bfsQueue; // Nodes in the queue. Stored as indices. - int m_bfsQueueBegin = 0; // Front of the queue. This is the next node to be expanded. - int m_bfsQueueEnd = 0; // Back of the queue. This is where nodes are inserted. + std::array, 8> m_bfsQueue; // Priority queue with indices to nodes. Nodes are sorted into buckets. + int m_bfsQueueMin = 0; // Front of the queue. This value mod 8 is the index to the bucket with the next node to be expanded. + int m_bfsQueueCountPushed = 0; // Number of nodes inserted into the queue. + int m_bfsQueueCountPopped = 0; // Number of nodes extacted from the queue. + int m_bfsQueueCountRepeated = 0; // Number of nodes re-inserted into the queue. + int m_bfsQueueCountSkipped = 0; // Number of nodes skipped because of unexpected distance (likely re-added). int m_bmMinX = 0, m_bmMinY = 0; int m_bmMaxX = 0, m_bmMaxY = 0; int m_bmTotal = 0; // index of final point in m_bmPoints From e38835cfd467e4e43be35b88e254b97db2e1759a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rasmus=20Br=C3=B6nneg=C3=A5rd?= <1162652+rasmusgo@users.noreply.github.com> Date: Sat, 12 Feb 2022 00:07:52 +0100 Subject: [PATCH 5/9] goto with A-star (with bucket queue) --- src/object/task/taskgoto.cpp | 71 ++++++++++++++++++++++++++---------- src/object/task/taskgoto.h | 4 +- 2 files changed, 53 insertions(+), 22 deletions(-) diff --git a/src/object/task/taskgoto.cpp b/src/object/task/taskgoto.cpp index bb4203a1..038c03fa 100644 --- a/src/object/task/taskgoto.cpp +++ b/src/object/task/taskgoto.cpp @@ -1700,6 +1700,25 @@ void CTaskGoto::PathFindingInit() m_bfsQueueCountSkipped = 0; } +static int HeuristicDistance(int nX, int nY, int startX, int startY) +{ + // 8-way connectivity yields a shortest path that + // consists of a diagonal and a non-diagonal part. + // ...+ + // : | + // :..| + // : /: + // :/ : + // +..: + const int distX = std::abs(nX - startX); + const int distY = std::abs(nY - startY); + const int smaller = std::min(distX, distY); + const int bigger = std::max(distX, distY); + // diagonal number of steps: smaller + // non-diagonal number of steps: bigger - smaller + return smaller * (7 - 5) + bigger * 5; +} + // Calculates points and passes to go from start to goal. // Returns: // ERR_OK if it's good @@ -1738,11 +1757,17 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector goalY >= 0 && goalY < m_bmSize ) { const int indexInMap = goalY * m_bmSize + goalX; + const int totalDistance = HeuristicDistance(goalX, goalY, startX, startY); + m_bfsQueueMin = totalDistance; m_bfsDistances[indexInMap] = 0; - m_bfsQueue[0].push_back(indexInMap); + m_bfsQueue[totalDistance % NUMQUEUEBUCKETS].push_back(indexInMap); m_bfsQueueCountPushed += 1; BitmapSetDot(1, goalX, goalY); // Mark as enqueued } + else + { + m_bfsQueueMin = std::numeric_limits::max(); + } // Enqueue nodes around the goal if (goalRadius > 0.0f) @@ -1762,8 +1787,10 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector !BitmapTestDot(1, x, y)) { const int indexInMap = y * m_bmSize + x; + const int totalDistance = HeuristicDistance(x, y, startX, startY); + m_bfsQueueMin = std::min(m_bfsQueueMin, totalDistance); m_bfsDistances[indexInMap] = 0; - m_bfsQueue[0].push_back(indexInMap); + m_bfsQueue[totalDistance % NUMQUEUEBUCKETS].push_back(indexInMap); m_bfsQueueCountPushed += 1; BitmapSetDot(1, x, y); // Mark as enqueued } @@ -1777,27 +1804,27 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector while (m_bfsQueueCountPushed != m_bfsQueueCountPopped) { // Pop a node from the queue - while (m_bfsQueue[m_bfsQueueMin % 8].empty()) + while (m_bfsQueue[m_bfsQueueMin % NUMQUEUEBUCKETS].empty()) { m_bfsQueueMin += 1; } - auto& bucket = m_bfsQueue[m_bfsQueueMin % 8]; + auto& bucket = m_bfsQueue[m_bfsQueueMin % NUMQUEUEBUCKETS]; const uint32_t indexInMap = bucket.back(); bucket.pop_back(); m_bfsQueueCountPopped += 1; - const int32_t distance = m_bfsDistances[indexInMap]; - - if (distance != m_bfsQueueMin) - { - m_bfsQueueCountSkipped += 1; - GetLogger()->Debug("Skipping node with mismatched distance, distance: %d, m_bfsQueueMin: %d\n", - distance, m_bfsQueueMin); - continue; - } - const int x = indexInMap % m_bmSize; const int y = indexInMap / m_bmSize; + const int32_t distance = m_bfsDistances[indexInMap]; + const int totalDistance = distance + HeuristicDistance(x, y, startX, startY); + + if (totalDistance != m_bfsQueueMin) + { + m_bfsQueueCountSkipped += 1; + GetLogger()->Debug("Skipping node with mismatched distance, distance: %d, totalDistance: %d, m_bfsQueueMin: %d\n", + distance, totalDistance, m_bfsQueueMin); + continue; + } if (x == startX && y == startY) { @@ -1844,17 +1871,19 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector break; } } - // std::reverse(m_bmPoints, m_bmPoints + m_bmTotal); - GetLogger()->Debug("Found path to goal with %d nodes\n", m_bmTotal + 1); + GetLogger()->Debug("Found path to goal with %d nodes and %d cost\n", m_bmTotal + 1, totalDistance); GetLogger()->Debug("m_bmStep: %d\n", m_bmStep); - GetLogger()->Debug("m_bfsQueueMin: %d mod 8 = %d\n", m_bfsQueueMin, m_bfsQueueMin % 8); + GetLogger()->Debug("m_bfsQueueMin: %d mod %d = %d\n", m_bfsQueueMin, NUMQUEUEBUCKETS, m_bfsQueueMin % NUMQUEUEBUCKETS); GetLogger()->Debug("m_bfsQueueCountPushed: %d\n", m_bfsQueueCountPushed); GetLogger()->Debug("m_bfsQueueCountPopped: %d\n", m_bfsQueueCountPopped); GetLogger()->Debug("m_bfsQueueCountRepeated: %d\n", m_bfsQueueCountRepeated); GetLogger()->Debug("m_bfsQueueCountSkipped: %d\n", m_bfsQueueCountSkipped); - GetLogger()->Debug("m_bfsQueue sizes:\n 0: %lu\n 1: %lu\n 2: %lu\n 3: %lu\n 4: %lu\n 5: %lu\n 6: %lu\n 7: %lu\n", - m_bfsQueue[0].size(), m_bfsQueue[1].size(), m_bfsQueue[2].size(), m_bfsQueue[3].size(), m_bfsQueue[4].size(), m_bfsQueue[5].size(), m_bfsQueue[6].size(), m_bfsQueue[7].size()); + GetLogger()->Debug("m_bfsQueue sizes:\n"); + for (size_t i = 0; i < m_bfsQueue.size(); ++i) + { + if (!m_bfsQueue[i].empty()) GetLogger()->Debug(" %lu: %lu\n", i, m_bfsQueue[i].size()); + } return ERR_OK; } @@ -1880,9 +1909,11 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector continue; } } + // Enqueue this neighbor + const int32_t newTotalDistance = newDistance + HeuristicDistance(nX, nY, startX, startY); m_bfsDistances[neighborIndexInMap] = newDistance; - m_bfsQueue[newDistance % 8].push_back(neighborIndexInMap); + m_bfsQueue[newTotalDistance % NUMQUEUEBUCKETS].push_back(neighborIndexInMap); m_bfsQueueCountPushed += 1; BitmapSetDot(1, nX, nY); // Mark as enqueued } diff --git a/src/object/task/taskgoto.h b/src/object/task/taskgoto.h index a95b73e4..f5131fa5 100644 --- a/src/object/task/taskgoto.h +++ b/src/object/task/taskgoto.h @@ -36,7 +36,7 @@ struct Point; class CObject; const int MAXPOINTS = 50000; - +const int NUMQUEUEBUCKETS = 32; enum TaskGotoGoal { @@ -144,7 +144,7 @@ protected: int m_bmLine = 0; // increment line m_bmSize/8 std::unique_ptr m_bmArray; // Bit table std::unique_ptr m_bfsDistances; // Distances to the goal for breadth-first search. - std::array, 8> m_bfsQueue; // Priority queue with indices to nodes. Nodes are sorted into buckets. + std::array, NUMQUEUEBUCKETS> m_bfsQueue; // Priority queue with indices to nodes. Nodes are sorted into buckets. int m_bfsQueueMin = 0; // Front of the queue. This value mod 8 is the index to the bucket with the next node to be expanded. int m_bfsQueueCountPushed = 0; // Number of nodes inserted into the queue. int m_bfsQueueCountPopped = 0; // Number of nodes extacted from the queue. From 2d794d1a32f7d8b8922296d31460c8e66fd75925 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rasmus=20Br=C3=B6nneg=C3=A5rd?= <1162652+rasmusgo@users.noreply.github.com> Date: Sat, 12 Feb 2022 14:27:23 +0100 Subject: [PATCH 6/9] goto: Handle oversized costs with a special bucket This is a bit redundant because the current usage of goto does not trigger it. It can be triggered by: * increasing goalRadius * decreasing NUMQUEUEBUCKETS * decreasing BM_DIM_STEP * increasing edge costs --- src/object/task/taskgoto.cpp | 57 ++++++++++++++++++++++++++++++++++-- src/object/task/taskgoto.h | 2 +- 2 files changed, 55 insertions(+), 4 deletions(-) diff --git a/src/object/task/taskgoto.cpp b/src/object/task/taskgoto.cpp index 038c03fa..4a23c0e0 100644 --- a/src/object/task/taskgoto.cpp +++ b/src/object/task/taskgoto.cpp @@ -1807,6 +1807,34 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector while (m_bfsQueue[m_bfsQueueMin % NUMQUEUEBUCKETS].empty()) { m_bfsQueueMin += 1; + if (m_bfsQueueMin % NUMQUEUEBUCKETS == 0 && !m_bfsQueue[NUMQUEUEBUCKETS].empty()) + { + // Process nodes with oversized costs. + const size_t countBefore = m_bfsQueue[NUMQUEUEBUCKETS].size(); + for (size_t i = 0; i < m_bfsQueue[NUMQUEUEBUCKETS].size();) + { + const uint32_t indexInMap = m_bfsQueue[NUMQUEUEBUCKETS][i]; + const int x = indexInMap % m_bmSize; + const int y = indexInMap / m_bmSize; + const int32_t distance = m_bfsDistances[indexInMap]; + const int totalDistance = distance + HeuristicDistance(x, y, startX, startY); + if (totalDistance < m_bfsQueueMin + NUMQUEUEBUCKETS) + { + // Move node to a regular bucket. + m_bfsQueue[totalDistance % NUMQUEUEBUCKETS].push_back(indexInMap); + m_bfsQueue[NUMQUEUEBUCKETS][i] = m_bfsQueue[NUMQUEUEBUCKETS].back(); + m_bfsQueue[NUMQUEUEBUCKETS].pop_back(); + } + else + { + // Look at next node. + i += 1; + } + } + const size_t countAfter = m_bfsQueue[NUMQUEUEBUCKETS].size(); + GetLogger()->Debug("Redistributed %lu of %lu nodes from the bucket with oversized costs.\n", + countBefore - countAfter, countBefore); + } } auto& bucket = m_bfsQueue[m_bfsQueueMin % NUMQUEUEBUCKETS]; const uint32_t indexInMap = bucket.back(); @@ -1820,9 +1848,32 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector if (totalDistance != m_bfsQueueMin) { - m_bfsQueueCountSkipped += 1; - GetLogger()->Debug("Skipping node with mismatched distance, distance: %d, totalDistance: %d, m_bfsQueueMin: %d\n", - distance, totalDistance, m_bfsQueueMin); + if (totalDistance < m_bfsQueueMin) + { + // This node has been updated to a lower cost and has allready been processed. + m_bfsQueueCountSkipped += 1; + // GetLogger()->Debug("Skipping node with smaller distance, distance: %d, totalDistance: %d, m_bfsQueueMin: %d\n", + // distance, totalDistance, m_bfsQueueMin); + } + else + { + if (totalDistance < m_bfsQueueMin + NUMQUEUEBUCKETS) + { + // Move node to a regular bucket. + m_bfsQueue[totalDistance % NUMQUEUEBUCKETS].push_back(indexInMap); + m_bfsQueueCountPushed += 1; + GetLogger()->Debug("Moving node with bigger distance into regular bucket, distance: %d, totalDistance: %d, m_bfsQueueMin: %d\n", + distance, totalDistance, m_bfsQueueMin); + } + else + { + // Move node to the bucket with oversized costs. + m_bfsQueue[NUMQUEUEBUCKETS].push_back(indexInMap); + m_bfsQueueCountPushed += 1; + GetLogger()->Debug("Moving node with bigger distance into bucket with oversized costs, distance: %d, totalDistance: %d, m_bfsQueueMin: %d\n", + distance, totalDistance, m_bfsQueueMin); + } + } continue; } diff --git a/src/object/task/taskgoto.h b/src/object/task/taskgoto.h index f5131fa5..e502c7c8 100644 --- a/src/object/task/taskgoto.h +++ b/src/object/task/taskgoto.h @@ -144,7 +144,7 @@ protected: int m_bmLine = 0; // increment line m_bmSize/8 std::unique_ptr m_bmArray; // Bit table std::unique_ptr m_bfsDistances; // Distances to the goal for breadth-first search. - std::array, NUMQUEUEBUCKETS> m_bfsQueue; // Priority queue with indices to nodes. Nodes are sorted into buckets. + std::array, NUMQUEUEBUCKETS + 1> m_bfsQueue; // Priority queue with indices to nodes. Nodes are sorted into buckets. The last bucket contains oversized costs. int m_bfsQueueMin = 0; // Front of the queue. This value mod 8 is the index to the bucket with the next node to be expanded. int m_bfsQueueCountPushed = 0; // Number of nodes inserted into the queue. int m_bfsQueueCountPopped = 0; // Number of nodes extacted from the queue. From 3478ee322b9a3b03018562efe7a67edc3a39e45c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rasmus=20Br=C3=B6nneg=C3=A5rd?= <1162652+rasmusgo@users.noreply.github.com> Date: Tue, 15 Feb 2022 01:02:35 +0100 Subject: [PATCH 7/9] goto: Find a more exact end of path wrt goalRadius The last segment of the path is shortened to avoid going too close and risk bumping into the object that it was meant to approach. The same position could also be found by finding the roots of a second order polynomial analytically but this solution is simple and sufficient. --- src/object/task/taskgoto.cpp | 32 ++++++++++++++++++++++++++++---- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/src/object/task/taskgoto.cpp b/src/object/task/taskgoto.cpp index 4a23c0e0..210fe30f 100644 --- a/src/object/task/taskgoto.cpp +++ b/src/object/task/taskgoto.cpp @@ -1912,18 +1912,42 @@ Error CTaskGoto::PathFindingSearch(const Math::Vector &start, const Math::Vector if (btX == goalX && btY == goalY) { m_bmPoints[m_bmTotal] = goal; - break; } - m_bmPoints[m_bmTotal].x = (btX + 0.5f) * BM_DIM_STEP - 1600.f; - m_bmPoints[m_bmTotal].z = (btY + 0.5f) * BM_DIM_STEP - 1600.f; + else + { + m_bmPoints[m_bmTotal].x = (btX + 0.5f) * BM_DIM_STEP - 1600.f; + m_bmPoints[m_bmTotal].z = (btY + 0.5f) * BM_DIM_STEP - 1600.f; + } if (bestDistance == 0) { + if (goalRadius > 0.0f) + { + // Find a more exact position by repeatedly bisecting the interval. + const float r2 = goalRadius * goalRadius; + Math::Vector inside = m_bmPoints[m_bmTotal] - goal; + Math::Vector outside = m_bmPoints[m_bmTotal-1] - goal; + Math::Vector mid = (inside + outside) * 0.5f; + for (int i = 0; i < 10; ++i) + { + if (mid.x*mid.x + mid.z*mid.z < r2) + { + inside = mid; + } + else + { + outside = mid; + } + mid = (inside + outside) * 0.5f; + } + m_bmPoints[m_bmTotal] = mid + goal; + } break; } } - GetLogger()->Debug("Found path to goal with %d nodes and %d cost\n", m_bmTotal + 1, totalDistance); + const float distanceToGoal = DistanceProjected(m_bmPoints[m_bmTotal], goal); + GetLogger()->Debug("Found path to goal with %d nodes and %d cost. Final distance to goal: %f\n", m_bmTotal + 1, totalDistance, distanceToGoal); GetLogger()->Debug("m_bmStep: %d\n", m_bmStep); GetLogger()->Debug("m_bfsQueueMin: %d mod %d = %d\n", m_bfsQueueMin, NUMQUEUEBUCKETS, m_bfsQueueMin % NUMQUEUEBUCKETS); GetLogger()->Debug("m_bfsQueueCountPushed: %d\n", m_bfsQueueCountPushed); From 8af600692e97285de4ccc8f1a01395c3b564568a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rasmus=20Br=C3=B6nneg=C3=A5rd?= <1162652+rasmusgo@users.noreply.github.com> Date: Thu, 17 Feb 2022 23:40:07 +0100 Subject: [PATCH 8/9] goto: Reduce NB_ITER back to 200 --- src/object/task/taskgoto.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/object/task/taskgoto.cpp b/src/object/task/taskgoto.cpp index 210fe30f..2f1b48fe 100644 --- a/src/object/task/taskgoto.cpp +++ b/src/object/task/taskgoto.cpp @@ -52,7 +52,7 @@ const float BEAM_ACCURACY = 5.0f; // higher value = more accurate, but slow const float SAFETY_MARGIN = 1.5f; // Smallest distance between two objects. Smaller = less "no route to destination", but higher probability of collisions between objects. // Changing SAFETY_MARGIN (old value was 4.0f) seems to have fixed many issues with goto(). TODO: maybe we could make it even smaller? Did changing it introduce any new bugs? -const int NB_ITER = 2000; // Maximum number of iterations you have the right to make before temporarily interrupt in order not to lower the framerate. +const int NB_ITER = 200; // Maximum number of iterations you have the right to make before temporarily interrupt in order not to lower the framerate. From 0829cd84feabcbb88c1b640a17cc0f632d68b48b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rasmus=20Br=C3=B6nneg=C3=A5rd?= <1162652+rasmusgo@users.noreply.github.com> Date: Thu, 17 Feb 2022 23:58:32 +0100 Subject: [PATCH 9/9] goto: Remove unused BEAM_ACCURACY --- src/object/task/taskgoto.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/object/task/taskgoto.cpp b/src/object/task/taskgoto.cpp index 2f1b48fe..3f01ea7a 100644 --- a/src/object/task/taskgoto.cpp +++ b/src/object/task/taskgoto.cpp @@ -48,7 +48,6 @@ const float FLY_DEF_HEIGHT = 50.0f; // default flying height // Settings that define goto() accuracy: const float BM_DIM_STEP = 5.0f; // Size of one pixel on the bitmap. Setting 5 means that 5x5 square (in game units) will be represented by 1 px on the bitmap. Decreasing this value will make a bigger bitmap, and may increase accuracy. TODO: Check how it actually impacts goto() accuracy -const float BEAM_ACCURACY = 5.0f; // higher value = more accurate, but slower const float SAFETY_MARGIN = 1.5f; // Smallest distance between two objects. Smaller = less "no route to destination", but higher probability of collisions between objects. // Changing SAFETY_MARGIN (old value was 4.0f) seems to have fixed many issues with goto(). TODO: maybe we could make it even smaller? Did changing it introduce any new bugs?