gwnavruntime/pathfollower/circlearcsplinecomputer.h Source File

circlearcsplinecomputer.h
Go to the documentation of this file.
1 /*
2 * Copyright 2016 Autodesk, Inc. All rights reserved.
3 * Use of this software is subject to the terms of the Autodesk license agreement and any attachments or Appendices thereto provided at the time of installation or download,
4 * or which otherwise accompanies this software in either electronic or hard copy form, or which is signed by you and accepted by Autodesk.
5 */
6 
7 #pragma once
8 
9 
17 
19 
20 
21 //#define DBG_CircleArcSplineComputer_PrintStatsOnSmallArcs
22 
23 
24 namespace Kaim
25 {
26 
27 class DisplayListManager;
28 
29 
34 {
35 public:
37  void Clear()
38  {
39  m_spline.Clear();
41  }
42 
43  CircleArcSpline m_spline;
45  KyUInt32 m_warningFlags;
46 };
47 
48 
51 {
52 public:
54  : m_displayListManager(nullptr)
55  , m_visualDebugId(KyUInt32MAXVAL)
56  {}
57 
58  DisplayListManager* m_displayListManager;
59  KyUInt32 m_visualDebugId;
60  String m_visualDebugPrefix;
61 
62  StringPulledBubbleListDisplayConfig m_rawBubbleListDisplayConfig;
63  StringPulledBubbleListDisplayConfig m_adjustedBubbleListDisplayConfig;
64  StringPulledBubbleListDisplayConfig m_simplifiedBubbleListDisplayConfig;
65  CircleArcSplineSectionDisplayConfig m_circleArcSplineSectionDisplayConfig;
66 
67  // Activate this to track down error cases
68  Ptr<SplineInputBlobDumpConfig> m_inputBlobDumpConfig;
69 };
70 
71 
72 // Internal: Class aggregating all necessary information about an edge going from one
73 // turn to the next one. Such edges are namely StringPulledEdge localized into a Channel.
74 class TurnListEdge
75 {
76 public:
77  StringPulledEdge m_edge;
78  KyUInt32 m_startSectionIdx;
79  KyUInt32 m_endSectionIdx;
80 };
81 
82 class SplineInputBlobDumpConfig;
83 class SplineInputBlobDumper;
84 
87 {
88 public:
91 
92  void Initialize(const SplineComputationConfig& splineConfig,
93  const Vec3f& startPosition, const ChannelSectionPtr& startSection,
94  const Vec3f& endPosition, const ChannelSectionPtr& endSection);
95 
96  void Clear();
97  void ClearInternals();
98 
99  void SetStartConstraint(const Vec2f& constraint) { m_startConstraint = constraint; }
100  void SetEndConstraint(const Vec2f& constraint) { m_endConstraint = constraint; }
101 
103 
104  KyUInt32 GetOutputCount() const { return m_profileOutputs.GetCount(); }
105  const RadiusProfileCircleArcSplineComputerOutput& GetOutput(KyUInt32 index) const { return m_profileOutputs[index]; }
106 
107 
108 public: // internal: these are public only to be accessible from unit tests
109  enum NextChannelTurnRangeSearchResult
110  {
111  NextChannelTurnFound,
112  NoChannelTurnUpToEnd
113  };
114 
115  enum ChangeTurnBubbleResult
116  {
117  TurnBubbleChangeRejected,
118  TurnBubbleChangeAccepted,
119  TurnBubbleChangeIgnored
120  };
121 
122  enum NeighborTurnValidationResult
123  {
124  ValidNeighborTurn,
125  ValidNeighborTurn_Small,
126  InvalidNeighborTurn_ContinueSearch,
127  InvalidNeighborTurn_StopSearch
128  };
129 
130  enum NeighborTurnArcValidationMode
131  {
132  UseIsInTurnRangeForSameSideArcsAndCanGoForOppositeOnes,
133  UseCanGoForAllArcs
134  };
135 
136  // ---- Main computation function : calls all the subsequent ones
137  CircleArcSplineComputationResult ComputeSpline(KyUInt32& overallWarningFlags);
138 
139  // ---- First steps: computed once for all RadiusProfiles
140 
141  KyResult CheckInputValidity();
142  KyResult ComputeBubbleArray(BubbleArray& bubbleArray);
143  KyResult ComputeStringPulledBubbleList(const BubbleArray& bubbleArray, StringPulledBubbleList& stringPulledBubbleList);
144 
145  // ---- Last steps: computed for each RadiusProfile
146 
147  // Compute all the last steps: Conversion to turn list, radius adjustment,
148  // simplification & conversion into CircleArcSpline.
149  void ComputeOptimalSpline(const StringPulledBubbleList& stringPulledBubbleList, RadiusProfileCircleArcSplineComputerOutput& output, const RadiusProfile& radiusProfile);
150 
151  // Convert the string pulled Bubble list into a TurnList.
152  CircleArcSplineComputationResult ConvertStringPulledBubbleListIntoTurnList(const StringPulledBubbleList& stringPulledBubbleList, TurnList& turnList);
153 
154  // Enlarge turns to fit preferred turn radii and respecting neighbor turns constraints.
155  CircleArcSplineComputationResult AdjustTurnListToRadiusProfile(const RadiusProfile& radiusProfile, TurnList& turnList, KyUInt32& warningFlags);
156 
157  // Convert the TurnList into a CircleArcSpline.
158  CircleArcSplineComputationResult ConvertTurnListIntoCircleArcSpline(const TurnList& turnList, CircleArcSpline& m_circleArcSpline, KyUInt32& warningFlags);
159 
160 
162  // BubbleArray computation
163 
164  NextChannelTurnRangeSearchResult GetNextChannelTurnRange(const Channel* channel, KyUInt32 currentIdx, KyUInt32 lastIdx, KyUInt32& nextTurnStartIdx, KyUInt32& nextTurnEndIdx);
165  KyResult UpdateBubbleMaxRadius(const Vec3f& center, const Vec3f& constrainingPosition, const Vec3f& oppositeCorner, KyFloat32& maxBubbleRadius) const;
166  KyResult CapBubblesRadiiAccordinglyToStartAndEndPositions(const Channel* channel, KyUInt32 gateIdx, KyFloat32& leftCornerMaxRadius, KyFloat32& rightCornerMaxRadius) const;
167  void AddCornerBubble(const Vec3f& channelCorner, KyFloat32 BubbleRadius, RotationDirection rotationDirection, BubbleArray& bubbleArray);
168  KyResult AddIntermediaryGateBubbles(const Channel* channel, KyUInt32 gateIdx, BubbleArray& bubbleArray);
169  KyResult AddNextChannelTurnBubbles(const Channel* channel, KyUInt32 nextTurnStartIdx, KyUInt32 nextTurnEndIdx, BubbleArray& bubbleArray);
170 
171  // Adjusts bubble radius to neighbors so that the bubble array is string-pullable
172  void EnsureBubbleArrayIsStringPullable(BubbleArray& bubbleArray);
173 
174  void AdjustBubbleRadiusRelativelyToNearbyBubble(const Bubble* nearbyBubble, Bubble& bubble);
175  void EnsureBubbleArrayIsStringPullable_ForwardPass(BubbleArray& bubbleArray, const KyUInt32 firstCornerBubbleIdx, const KyUInt32 lastCornerBubbleIdx);
176  void EnsureBubbleArrayIsStringPullable_BackwardPass(BubbleArray& bubbleArray, const KyUInt32 firstCornerBubbleIdx, const KyUInt32 lastCornerBubbleIdx);
177 
178 
180  // TurnList atomic modifications
181 
182  void ComputeInitialTurnsForStartAndEndConstraints(TurnList& turnList, KyFloat32 startRadius, KyFloat32 endRadius, CircleArcSplineComputationResult& result, KyUInt32& warningFlags);
183  bool AdjustFirstTurnToStartConstraintAndThisRadius(TurnList& turnList, const RadiusProfile& radiusProfile, KyUInt32 radiusIdx, CircleArcSplineComputationResult& result, KyUInt32& warningFlags);
184  bool AdjustLastTurnToEndConstraintAndThisRadius(TurnList& turnList, const RadiusProfile& radiusProfile, KyUInt32 radiusIdx, CircleArcSplineComputationResult& result, KyUInt32& warningFlags);
185 
186  // Check the candidate Bubble is valid if used for the specified turn. If so, it then:
187  // 1. replace the turn Bubble by the candidate one
188  // 2. update turn entry and exit positions
189  // 3. update the neighbor turns:
190  // - remove the ones that are no more in touch with the pulled string
191  // - update the previous turn exit position
192  // - update the next turn entry position
193  ChangeTurnBubbleResult ChangeTurnBubble(TurnList& turnList, TurnList::Iterator turnIt, const Bubble& candidate,
194  const RadiusProfile& radiusProfile, KyUInt32 radiusIndexInProfile, CircleArcSplineComputationResult& result, KyUInt32& warningFlags);
195 
196  KyResult FindNewPrevTurn(TurnList& turnList, const TurnList::Iterator& turnIt, const CircleArcSplineTurn& curTurn, const Bubble& turnBubbleCdt,
197  const RotationDirection rotDir, DisplayList* displayListPtr, TurnList::Iterator& prevTurnIt, TurnListEdge& incomingEdge,
198  const RadiusProfile& radiusProfile, KyUInt32 radiusIndexInProfile, Bubble& newStartBubble, KyUInt32& newStartBubbleRadiusIndexInProfile);
199 
200  KyResult FindNewNextTurn(TurnList& turnList, const TurnList::Iterator& turnIt, const CircleArcSplineTurn& curTurn, const Bubble& turnBubbleCdt, const TurnListEdge& incomingEdge,
201  const RotationDirection rotDir, DisplayList* displayListPtr, TurnList::Iterator& nextTurnIt, TurnListEdge& outgoingEdge,
202  const RadiusProfile& radiusProfile, KyUInt32 radiusIndexInProfile, Bubble& newEndBubble, KyUInt32& newEndBubbleRadiusIndexInProfile);
203 
204  KyResult TryFirstTurnWithRelaxedStartConstraint(
205  Bubble& newStartBubble, KyUInt32& newStartBubbleRadiusIndexInProfile, const CircleArcSplineTurn& curTurn,
206  StringPulledEdge& edge, const Bubble& turnBubbleCdt, const RotationDirection rotDir, DisplayList* displayListPtr,
207  TurnListEdge& incomingEdge, TurnList::Iterator& prevTurnIt, TurnList::Iterator prevTurnCdtIt);
208 
209  KyResult TryLastTurnCandidateWithRelaxedEndConstraint(
210  Bubble& newEndBubble, KyUInt32& newEndBubbleRadiusIndexInProfile, const CircleArcSplineTurn& curTurn,
211  StringPulledEdge& edge, const Bubble& turnBubbleCdt, const RotationDirection rotDir, DisplayList* displayListPtr,
212  const TurnListEdge& incomingEdge, TurnListEdge& outgoingEdge, TurnList::Iterator& nextTurnIt, TurnList::Iterator nextTurnCdtIt);
213 
214  NeighborTurnValidationResult ValidatePrevTurnCandidate(const CircleArcSplineTurn& curTurn, const Bubble& turnBubbleCdt, const CircleArcSplineTurn& prevTurnCdtTurn,
215  const RotationDirection rotDir, DisplayList* displayListPtr, TurnListEdge& incomingEdge,
216  KyFloat32& skippedTurnAngleRadSum, NeighborTurnArcValidationMode neighborTurnArcValidationMode);
217 
218  NeighborTurnValidationResult ValidateNextTurnCandidate(const CircleArcSplineTurn& curTurn, const Bubble& turnBubbleCdt, const CircleArcSplineTurn& nextTurnCdtTurn,
219  const RotationDirection rotDir, DisplayList* displayListPtr, const TurnListEdge& incomingEdge, TurnListEdge& outgoingEdge,
220  KyFloat32& skippedTurnAngleRadSum, NeighborTurnArcValidationMode neighborTurnArcValidationMode);
221 
222  bool DoesSegmentRespectDistanceToChannelBorder(const Channel* channel,
223  const Vec3f& from, KyUInt32 fromSectionIdx, const Vec3f& to, KyUInt32& toSectionIdx) const;
224 
225  bool DoesCircleArcRespectDistanceToChannelBorder(const Channel* channel, const Bubble& circleArcBubble,
226  const Vec3f& from, KyUInt32 fromSectionIdx, const Vec3f& to, KyUInt32 toSectionIdx) const;
227 
228  // ---- to be removed -----
229  bool TestCircleArcCanGoWithSampling(const Bubble &circleArcBubble, const Vec3f &from, const Vec3f &to, const KyFloat32 circleRadius, KyUInt32 fromSectionIdx,
230  const Channel* channel, KyUInt32 toSectionIdx) const;
231 
232 
233 public: // internal - intermediary computation result accessors and other internal functions useful for unit tests
234  const TurnList& GetTurnList() const { return m_turnList; }
235 
236  // Adjusts a channel width accordingly to m_margin.
237  KyFloat32 GetReducedChannelWidth(KyFloat32 channelWidth) const;
238 
239  enum VerboseMode {Verbose, Silent};
240  bool IsSplineValid(const CircleArcSpline& spline, CircleArcSplineComputationResult result, KyUInt32 warningFlags, VerboseMode verboseMode = Silent) const;
241 
242 
244  // Utility internal methods
245 
246  void InitializeVisualDebugColor();
247  void InitializeDisplayList(DisplayList& displayList, const char* groupName, const char* listName) const;
248  void RenderSplineComputationInputs() const;
249  void RenderBubbleData(const CircleArcSplineTurn& turn, const char* groupName) const;
250  void RenderBubbleArray(const BubbleArray& bubbleArray) const;
251  void RenderTurnList(const TurnList& turnList, const char* listBaseName, const StringPulledBubbleListDisplayConfig& displayConfig) const;
252  void RenderResult(const char* displayListBaseName, CircleArcSplineComputationResult result, KyUInt32 warningflags) const;
253 
254  void DumpOrVisualDebugIfNeeded(SplineInputBlobDumper& inputBlobDumper, CircleArcSplineComputationResult overallResult,
255  KyUInt32 overallWarningFlags, KyUInt32 radiusProfileCount);
256  void VisualDebug_BubbleListConvertionError(const StringPulledBubbleList &stringPulledBubbleList, CircleArcSplineComputationResult result);
257  void VisualDebugCircleArcSpline(CircleArcSpline& circleArcSpline);
258 
259 #ifdef DBG_CircleArcSplineComputer_PrintStatsOnSmallArcs
260  static void PrintSmallElementsStatistics();
261 #endif // DBG_CircleArcSplineComputer_PrintStatsOnSmallArcs
262 
263 
264 public: // internal: We let parameters accessible for validation unit tests
265  // Inputs
266  const SplineComputationConfig* m_splineConfig;
267  ChannelSectionPtr m_startSection;
268  ChannelSectionPtr m_endSection;
269  Vec3f m_startPosition;
270  Vec3f m_endPosition;
271  Vec2f m_startConstraint;
272  Vec2f m_endConstraint;
273 
274 public:
275  // This is the minimal distance let between adjacent bubbles to ensure the
276  // string can pass through the gap in-between. Default is 0.01m.
277  KyFloat32 m_margin;
278 
279  // This is the cosine of the angle tolerated to consider start and end
280  // constraints as respected.
281  // Default is 1 degree.
282  KyFloat32 m_maxAngleCosToleranceOnStartAndEndConstraints;
283 
284 private:
285  // Intermediary data structures
286  TurnList::NodePool m_turnPool;
287  TurnList m_turnList;
288 
289  // Intermediary parameter values
290  KyFloat32 m_fullTurnToleranceSin;
291 
292  bool m_checkStartConstraintMaxAngle;
293  Vec2f m_startConstraintCWLimit;
294  Vec2f m_startConstraintCCWLimit;
295 
296  bool m_checkEndConstraintMaxAngle;
297  Vec2f m_endConstraintCWLimit;
298  Vec2f m_endConstraintCCWLimit;
299 
300  // Output
302 
303  // Debug / VisualDebug
304  KyUInt32 m_debug_ChangeTurnBubble_VisualDebugListIdx;
305  String m_debug_RadiusProfileLabel;
306 
307 public: // internal
308  bool m_isInComputingSpline;
309 
310 public:
311  // You can directly set these advanced parameters to toggle and configure VisualDebug.
312  CircleArcSplineComputerVisualDebugConfig m_visualDebugConfig;
313  bool m_debug_CheckSplineValidity;
314 };
315 
316 
317 KY_INLINE KyFloat32 CircleArcSplineComputer::GetReducedChannelWidth(KyFloat32 channelWidth) const
318 {
319  const KyFloat32 reducedChannelWidth = (channelWidth > 2.0f * m_margin) ? (channelWidth - m_margin) : 0.5f * channelWidth;
320  return reducedChannelWidth;
321 }
322 
323 } // namespace Kaim
324 
Class used to configure all VisualDebug components for CircleArcSpline computation.
Definition: circlearcsplinecomputer.h:50
Game side: Manages all DisplayListData, send them to the NavigationLab.
Definition: displaylist.h:375
Class aggregating a CircleArcSpline and the corresponding computation result.
Definition: circlearcsplinecomputer.h:33
std::uint32_t KyUInt32
uint32_t
Definition: types.h:29
RadiusProfile is an array of preferred radii.
Definition: radiusprofile.h:26
This class encapsulate an array of Bubbles.
Definition: bubblearray.h:22
General purpose array for movable objects that require explicit construction/destruction.
Definition: kyarray.h:162
RotationDirection
Defines the 4 possible cases of possibly constrained rotation in the horizontal plane for a given ele...
Definition: rotation.h:15
The class representing a spline compounded of oriented circle arcs and straight line segments...
Definition: circlearcspline.h:27
The spline has not yet been computed.
Definition: circlearcsplinecomputationresult.h:17
This class represents a circle with potential rotation limitation.
Definition: bubble.h:31
2d vector using KyFloat32.
Definition: vec2f.h:18
DisplayList is used to push text, lines or shapes for rendering in the NavigationLab e...
Definition: displaylist.h:128
Navigation return code class.
Definition: types.h:108
The Autodesk Navigation namespace.
Definition: gamekitcrowddispersion.cpp:17
Class used to compute a CircleArcSpline in a Channel.
Definition: circlearcsplinecomputer.h:86
CircleArcSplineComputationResult
Enumerates the CircleArcSpline computation results.
Definition: circlearcsplinecomputationresult.h:15
Channel enrich Path with clearance information on each side of the Path.
Definition: channel.h:169
Iterator on SharedPoolList nodes (SPL stands for SharedPoolList).
Definition: sharedpoollist.h:41
#define KyUInt32MAXVAL
KyUInt32 max value
Definition: types.h:68
float KyFloat32
float
Definition: types.h:32
3d vector using 32bits floating points.
Definition: vec3f.h:16