9
3

DetourObstacleAvoidance.h 5.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159
  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. #ifndef DETOUROBSTACLEAVOIDANCE_H
  19. #define DETOUROBSTACLEAVOIDANCE_H
  20. struct dtObstacleCircle
  21. {
  22. float p[3]; ///< Position of the obstacle
  23. float vel[3]; ///< Velocity of the obstacle
  24. float dvel[3]; ///< Velocity of the obstacle
  25. float rad; ///< Radius of the obstacle
  26. float dp[3], np[3]; ///< Use for side selection during sampling.
  27. };
  28. struct dtObstacleSegment
  29. {
  30. float p[3], q[3]; ///< End points of the obstacle segment
  31. bool touch;
  32. };
  33. class dtObstacleAvoidanceDebugData
  34. {
  35. public:
  36. dtObstacleAvoidanceDebugData();
  37. ~dtObstacleAvoidanceDebugData();
  38. bool init(const int maxSamples);
  39. void reset();
  40. void addSample(const float* vel, const float ssize, const float pen,
  41. const float vpen, const float vcpen, const float spen, const float tpen);
  42. void normalizeSamples();
  43. inline int getSampleCount() const { return m_nsamples; }
  44. inline const float* getSampleVelocity(const int i) const { return &m_vel[i*3]; }
  45. inline float getSampleSize(const int i) const { return m_ssize[i]; }
  46. inline float getSamplePenalty(const int i) const { return m_pen[i]; }
  47. inline float getSampleDesiredVelocityPenalty(const int i) const { return m_vpen[i]; }
  48. inline float getSampleCurrentVelocityPenalty(const int i) const { return m_vcpen[i]; }
  49. inline float getSamplePreferredSidePenalty(const int i) const { return m_spen[i]; }
  50. inline float getSampleCollisionTimePenalty(const int i) const { return m_tpen[i]; }
  51. private:
  52. // Explicitly disabled copy constructor and copy assignment operator.
  53. dtObstacleAvoidanceDebugData(const dtObstacleAvoidanceDebugData&);
  54. dtObstacleAvoidanceDebugData& operator=(const dtObstacleAvoidanceDebugData&);
  55. int m_nsamples;
  56. int m_maxSamples;
  57. float* m_vel;
  58. float* m_ssize;
  59. float* m_pen;
  60. float* m_vpen;
  61. float* m_vcpen;
  62. float* m_spen;
  63. float* m_tpen;
  64. };
  65. dtObstacleAvoidanceDebugData* dtAllocObstacleAvoidanceDebugData();
  66. void dtFreeObstacleAvoidanceDebugData(dtObstacleAvoidanceDebugData* ptr);
  67. static const int DT_MAX_PATTERN_DIVS = 32; ///< Max numver of adaptive divs.
  68. static const int DT_MAX_PATTERN_RINGS = 4; ///< Max number of adaptive rings.
  69. struct dtObstacleAvoidanceParams
  70. {
  71. float velBias;
  72. float weightDesVel;
  73. float weightCurVel;
  74. float weightSide;
  75. float weightToi;
  76. float horizTime;
  77. unsigned char gridSize; ///< grid
  78. unsigned char adaptiveDivs; ///< adaptive
  79. unsigned char adaptiveRings; ///< adaptive
  80. unsigned char adaptiveDepth; ///< adaptive
  81. };
  82. class dtObstacleAvoidanceQuery
  83. {
  84. public:
  85. dtObstacleAvoidanceQuery();
  86. ~dtObstacleAvoidanceQuery();
  87. bool init(const int maxCircles, const int maxSegments);
  88. void reset();
  89. void addCircle(const float* pos, const float rad,
  90. const float* vel, const float* dvel);
  91. void addSegment(const float* p, const float* q);
  92. int sampleVelocityGrid(const float* pos, const float rad, const float vmax,
  93. const float* vel, const float* dvel, float* nvel,
  94. const dtObstacleAvoidanceParams* params,
  95. dtObstacleAvoidanceDebugData* debug = 0);
  96. int sampleVelocityAdaptive(const float* pos, const float rad, const float vmax,
  97. const float* vel, const float* dvel, float* nvel,
  98. const dtObstacleAvoidanceParams* params,
  99. dtObstacleAvoidanceDebugData* debug = 0);
  100. inline int getObstacleCircleCount() const { return m_ncircles; }
  101. const dtObstacleCircle* getObstacleCircle(const int i) { return &m_circles[i]; }
  102. inline int getObstacleSegmentCount() const { return m_nsegments; }
  103. const dtObstacleSegment* getObstacleSegment(const int i) { return &m_segments[i]; }
  104. private:
  105. // Explicitly disabled copy constructor and copy assignment operator.
  106. dtObstacleAvoidanceQuery(const dtObstacleAvoidanceQuery&);
  107. dtObstacleAvoidanceQuery& operator=(const dtObstacleAvoidanceQuery&);
  108. void prepare(const float* pos, const float* dvel);
  109. float processSample(const float* vcand, const float cs,
  110. const float* pos, const float rad,
  111. const float* vel, const float* dvel,
  112. const float minPenalty,
  113. dtObstacleAvoidanceDebugData* debug);
  114. dtObstacleAvoidanceParams m_params;
  115. float m_invHorizTime;
  116. float m_vmax;
  117. float m_invVmax;
  118. int m_maxCircles;
  119. dtObstacleCircle* m_circles;
  120. int m_ncircles;
  121. int m_maxSegments;
  122. dtObstacleSegment* m_segments;
  123. int m_nsegments;
  124. };
  125. dtObstacleAvoidanceQuery* dtAllocObstacleAvoidanceQuery();
  126. void dtFreeObstacleAvoidanceQuery(dtObstacleAvoidanceQuery* ptr);
  127. #endif // DETOUROBSTACLEAVOIDANCE_H