9
3

DetourPathQueue.cpp 4.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200
  1. //
  2. // Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
  3. //
  4. // This software is provided 'as-is', without any express or implied
  5. // warranty. In no event will the authors be held liable for any damages
  6. // arising from the use of this software.
  7. // Permission is granted to anyone to use this software for any purpose,
  8. // including commercial applications, and to alter it and redistribute it
  9. // freely, subject to the following restrictions:
  10. // 1. The origin of this software must not be misrepresented; you must not
  11. // claim that you wrote the original software. If you use this software
  12. // in a product, an acknowledgment in the product documentation would be
  13. // appreciated but is not required.
  14. // 2. Altered source versions must be plainly marked as such, and must not be
  15. // misrepresented as being the original software.
  16. // 3. This notice may not be removed or altered from any source distribution.
  17. //
  18. #include <string.h>
  19. #include "DetourPathQueue.h"
  20. #include "DetourNavMesh.h"
  21. #include "DetourNavMeshQuery.h"
  22. #include "DetourAlloc.h"
  23. #include "DetourCommon.h"
  24. dtPathQueue::dtPathQueue() :
  25. m_nextHandle(1),
  26. m_maxPathSize(0),
  27. m_queueHead(0),
  28. m_navquery(0)
  29. {
  30. for (int i = 0; i < MAX_QUEUE; ++i)
  31. m_queue[i].path = 0;
  32. }
  33. dtPathQueue::~dtPathQueue()
  34. {
  35. purge();
  36. }
  37. void dtPathQueue::purge()
  38. {
  39. dtFreeNavMeshQuery(m_navquery);
  40. m_navquery = 0;
  41. for (int i = 0; i < MAX_QUEUE; ++i)
  42. {
  43. dtFree(m_queue[i].path);
  44. m_queue[i].path = 0;
  45. }
  46. }
  47. bool dtPathQueue::init(const int maxPathSize, const int maxSearchNodeCount, dtNavMesh* nav)
  48. {
  49. purge();
  50. m_navquery = dtAllocNavMeshQuery();
  51. if (!m_navquery)
  52. return false;
  53. if (dtStatusFailed(m_navquery->init(nav, maxSearchNodeCount)))
  54. return false;
  55. m_maxPathSize = maxPathSize;
  56. for (int i = 0; i < MAX_QUEUE; ++i)
  57. {
  58. m_queue[i].ref = DT_PATHQ_INVALID;
  59. m_queue[i].path = (dtPolyRef*)dtAlloc(sizeof(dtPolyRef)*m_maxPathSize, DT_ALLOC_PERM);
  60. if (!m_queue[i].path)
  61. return false;
  62. }
  63. m_queueHead = 0;
  64. return true;
  65. }
  66. void dtPathQueue::update(const int maxIters)
  67. {
  68. static const int MAX_KEEP_ALIVE = 2; // in update ticks.
  69. // Update path request until there is nothing to update
  70. // or upto maxIters pathfinder iterations has been consumed.
  71. int iterCount = maxIters;
  72. for (int i = 0; i < MAX_QUEUE; ++i)
  73. {
  74. PathQuery& q = m_queue[m_queueHead % MAX_QUEUE];
  75. // Skip inactive requests.
  76. if (q.ref == DT_PATHQ_INVALID)
  77. {
  78. m_queueHead++;
  79. continue;
  80. }
  81. // Handle completed request.
  82. if (dtStatusSucceed(q.status) || dtStatusFailed(q.status))
  83. {
  84. // If the path result has not been read in few frames, free the slot.
  85. q.keepAlive++;
  86. if (q.keepAlive > MAX_KEEP_ALIVE)
  87. {
  88. q.ref = DT_PATHQ_INVALID;
  89. q.status = 0;
  90. }
  91. m_queueHead++;
  92. continue;
  93. }
  94. // Handle query start.
  95. if (q.status == 0)
  96. {
  97. q.status = m_navquery->initSlicedFindPath(q.startRef, q.endRef, q.startPos, q.endPos, q.filter);
  98. }
  99. // Handle query in progress.
  100. if (dtStatusInProgress(q.status))
  101. {
  102. int iters = 0;
  103. q.status = m_navquery->updateSlicedFindPath(iterCount, &iters);
  104. iterCount -= iters;
  105. }
  106. if (dtStatusSucceed(q.status))
  107. {
  108. q.status = m_navquery->finalizeSlicedFindPath(q.path, &q.npath, m_maxPathSize);
  109. }
  110. if (iterCount <= 0)
  111. break;
  112. m_queueHead++;
  113. }
  114. }
  115. dtPathQueueRef dtPathQueue::request(dtPolyRef startRef, dtPolyRef endRef,
  116. const float* startPos, const float* endPos,
  117. const dtQueryFilter* filter)
  118. {
  119. // Find empty slot
  120. int slot = -1;
  121. for (int i = 0; i < MAX_QUEUE; ++i)
  122. {
  123. if (m_queue[i].ref == DT_PATHQ_INVALID)
  124. {
  125. slot = i;
  126. break;
  127. }
  128. }
  129. // Could not find slot.
  130. if (slot == -1)
  131. return DT_PATHQ_INVALID;
  132. dtPathQueueRef ref = m_nextHandle++;
  133. if (m_nextHandle == DT_PATHQ_INVALID) m_nextHandle++;
  134. PathQuery& q = m_queue[slot];
  135. q.ref = ref;
  136. dtVcopy(q.startPos, startPos);
  137. q.startRef = startRef;
  138. dtVcopy(q.endPos, endPos);
  139. q.endRef = endRef;
  140. q.status = 0;
  141. q.npath = 0;
  142. q.filter = filter;
  143. q.keepAlive = 0;
  144. return ref;
  145. }
  146. dtStatus dtPathQueue::getRequestStatus(dtPathQueueRef ref) const
  147. {
  148. for (int i = 0; i < MAX_QUEUE; ++i)
  149. {
  150. if (m_queue[i].ref == ref)
  151. return m_queue[i].status;
  152. }
  153. return DT_FAILURE;
  154. }
  155. dtStatus dtPathQueue::getPathResult(dtPathQueueRef ref, dtPolyRef* path, int* pathSize, const int maxPath)
  156. {
  157. for (int i = 0; i < MAX_QUEUE; ++i)
  158. {
  159. if (m_queue[i].ref == ref)
  160. {
  161. PathQuery& q = m_queue[i];
  162. dtStatus details = q.status & DT_STATUS_DETAIL_MASK;
  163. // Free request for reuse.
  164. q.ref = DT_PATHQ_INVALID;
  165. q.status = 0;
  166. // Copy path
  167. int n = dtMin(q.npath, maxPath);
  168. memcpy(path, q.path, sizeof(dtPolyRef)*n);
  169. *pathSize = n;
  170. return details | DT_SUCCESS;
  171. }
  172. }
  173. return DT_FAILURE;
  174. }