9
3

DetourLocalBoundary.cpp 3.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137
  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 <float.h>
  19. #include <string.h>
  20. #include "DetourLocalBoundary.h"
  21. #include "DetourNavMeshQuery.h"
  22. #include "DetourCommon.h"
  23. #include "DetourAssert.h"
  24. dtLocalBoundary::dtLocalBoundary() :
  25. m_nsegs(0),
  26. m_npolys(0)
  27. {
  28. dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
  29. }
  30. dtLocalBoundary::~dtLocalBoundary()
  31. {
  32. }
  33. void dtLocalBoundary::reset()
  34. {
  35. dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
  36. m_npolys = 0;
  37. m_nsegs = 0;
  38. }
  39. void dtLocalBoundary::addSegment(const float dist, const float* s)
  40. {
  41. // Insert neighbour based on the distance.
  42. Segment* seg = 0;
  43. if (!m_nsegs)
  44. {
  45. // First, trivial accept.
  46. seg = &m_segs[0];
  47. }
  48. else if (dist >= m_segs[m_nsegs-1].d)
  49. {
  50. // Further than the last segment, skip.
  51. if (m_nsegs >= MAX_LOCAL_SEGS)
  52. return;
  53. // Last, trivial accept.
  54. seg = &m_segs[m_nsegs];
  55. }
  56. else
  57. {
  58. // Insert inbetween.
  59. int i;
  60. for (i = 0; i < m_nsegs; ++i)
  61. if (dist <= m_segs[i].d)
  62. break;
  63. const int tgt = i+1;
  64. const int n = dtMin(m_nsegs-i, MAX_LOCAL_SEGS-tgt);
  65. dtAssert(tgt+n <= MAX_LOCAL_SEGS);
  66. if (n > 0)
  67. memmove(&m_segs[tgt], &m_segs[i], sizeof(Segment)*n);
  68. seg = &m_segs[i];
  69. }
  70. seg->d = dist;
  71. memcpy(seg->s, s, sizeof(float)*6);
  72. if (m_nsegs < MAX_LOCAL_SEGS)
  73. m_nsegs++;
  74. }
  75. void dtLocalBoundary::update(dtPolyRef ref, const float* pos, const float collisionQueryRange,
  76. dtNavMeshQuery* navquery, const dtQueryFilter* filter)
  77. {
  78. static const int MAX_SEGS_PER_POLY = DT_VERTS_PER_POLYGON*3;
  79. if (!ref)
  80. {
  81. dtVset(m_center, FLT_MAX,FLT_MAX,FLT_MAX);
  82. m_nsegs = 0;
  83. m_npolys = 0;
  84. return;
  85. }
  86. dtVcopy(m_center, pos);
  87. // First query non-overlapping polygons.
  88. navquery->findLocalNeighbourhood(ref, pos, collisionQueryRange,
  89. filter, m_polys, 0, &m_npolys, MAX_LOCAL_POLYS);
  90. // Secondly, store all polygon edges.
  91. m_nsegs = 0;
  92. float segs[MAX_SEGS_PER_POLY*6];
  93. int nsegs = 0;
  94. for (int j = 0; j < m_npolys; ++j)
  95. {
  96. navquery->getPolyWallSegments(m_polys[j], filter, segs, 0, &nsegs, MAX_SEGS_PER_POLY);
  97. for (int k = 0; k < nsegs; ++k)
  98. {
  99. const float* s = &segs[k*6];
  100. // Skip too distant segments.
  101. float tseg;
  102. const float distSqr = dtDistancePtSegSqr2D(pos, s, s+3, tseg);
  103. if (distSqr > dtSqr(collisionQueryRange))
  104. continue;
  105. addSegment(distSqr, s);
  106. }
  107. }
  108. }
  109. bool dtLocalBoundary::isValid(dtNavMeshQuery* navquery, const dtQueryFilter* filter)
  110. {
  111. if (!m_npolys)
  112. return false;
  113. // Check that all polygons still pass query filter.
  114. for (int i = 0; i < m_npolys; ++i)
  115. {
  116. if (!navquery->isValidPolyRef(m_polys[i], filter))
  117. return false;
  118. }
  119. return true;
  120. }