gwnavruntime/pathfollower/avoidancesolver.h Source File

avoidancesolver.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 
12 #include "gwnavruntime/world/bot.h"
17 
18 
19 namespace Kaim
20 {
21 
22 class CosAndSinAngle;
23 
24 class NormalizedUtilityFunctionInfluenceRatios
25 {
26 public:
27  NormalizedUtilityFunctionInfluenceRatios(const AvoidanceConfig* avoidanceConfig)
28  {
29  const Kaim::AvoidanceUtilityFunctionConfig& utilityconfig = avoidanceConfig->m_utilityFunctionConfig;
30  KyFloat32 influenceSum =
31  utilityconfig.m_avoidanceInfluence +
32  utilityconfig.m_desiredVelocityInfluence +
33  utilityconfig.m_previousVelocityInfluence;
34 
35  KyFloat32 influenceSumInv = 1.0f / influenceSum;
36 
37  m_avoidanceInfluence = influenceSumInv * utilityconfig.m_avoidanceInfluence;
38  m_desiredVelocityInfluence = influenceSumInv * utilityconfig.m_desiredVelocityInfluence;
39  m_previousVelocityInfluence = influenceSumInv * utilityconfig.m_previousVelocityInfluence;
40  }
41 
42  KyFloat32 m_avoidanceInfluence;
43  KyFloat32 m_desiredVelocityInfluence;
44  KyFloat32 m_previousVelocityInfluence;
45 };
46 
47 typedef RayCastQueryResult(*PerformRaycastFromBotPositionCallback)
48  (const AvoidanceSolverConfig& config, const Vec2f& direction, KyFloat32 speed, KyFloat32 time,
49  LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData, Vec3f& collisionPos);
50 
51 typedef DiskCollisionQueryResult(*PerformDiskCollisionFromBotPositionCallback)
52  (const AvoidanceSolverConfig& config, KyFloat32 speed, KyFloat32 time, LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData);
53 
54 class VelocitySample
55 {
56 public:
57 
58  VelocitySample()
59  : m_normalizedDirection (0.0f, 0.0f)
60  , m_type (Avoidance_NoAvoidance)
61  , m_avoidanceScore (0.0f)
62  , m_desiredVelocityScore (0.0f)
63  , m_previousVelocityScore (0.0f)
64  , m_collisionTime (0.0f)
65  , m_colliderIndex (-1)
66  {}
67 
68  Vec2f m_normalizedDirection;
69  AvoidanceResult m_type;
70 
71  KyFloat32 m_avoidanceScore;
72  KyFloat32 m_desiredVelocityScore;
73  KyFloat32 m_previousVelocityScore;
74  KyFloat32 m_collisionTime;
75  KyInt32 m_colliderIndex; // if -1 no colliders, or static one (i.e. navmesh or channel borders)
76 
77  KyFloat32 ComputeFinalScore(const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios) const
78  {
79  return ((m_avoidanceScore * utilityFunctionRatios.m_avoidanceInfluence) +
80  (m_desiredVelocityScore * utilityFunctionRatios.m_desiredVelocityInfluence) +
81  (m_previousVelocityScore * utilityFunctionRatios.m_previousVelocityInfluence));
82  }
83 
84  // VelocitySamplesArray::ScoreForDistanceToDirection(desiredDirection, &VelocitySample::ScoreDesiredVelocity);
85  // => (velocitySample.*scoreFunction)(score);
86  // ScoreForDistanceToDirection(previousDirection, &VelocitySample::ScorePreviousVelocity);
87  void ScoreDesiredVelocity (KyFloat32 score) { m_desiredVelocityScore = score; }
88  void ScorePreviousVelocity(KyFloat32 score) { m_previousVelocityScore = score; }
89  typedef void(VelocitySample::*ScoreVelocityFunction)(KyFloat32);
90 
91  // Debugging functions
92  KyFloat32 GetAvoidanceScore (const NormalizedUtilityFunctionInfluenceRatios& ) const { return m_avoidanceScore; }
93  KyFloat32 GetDesiredVelocityScore (const NormalizedUtilityFunctionInfluenceRatios& ) const { return m_desiredVelocityScore; }
94  KyFloat32 GetPreviousVelocityScore (const NormalizedUtilityFunctionInfluenceRatios& ) const { return m_previousVelocityScore; }
95 
96  typedef KyFloat32(VelocitySample::*ScoreAccessorFunction)(const NormalizedUtilityFunctionInfluenceRatios&) const;
97 
98 public: // internal
99  void AdjustScoreAgainstChannel(const AvoidanceSolverConfig& config, KyFloat32 movingSpeed, KyFloat32 timeHorizon, RayCastInChannel& rayCastInChannel);
100  void AdjustScoreAgainstNavMesh(const AvoidanceSolverConfig& config, KyFloat32 movingSpeed, KyFloat32 timeHorizon,
101  LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData, PerformRaycastFromBotPositionCallback raycastFunction);
102 private:
103  void AdjustScoreToStaticObstacleCollision(const AvoidanceSolverConfig& config, KyFloat32 movingSpeed, KyFloat32 timeHorizon, const Vec2f& botPosition, const Vec2f& collisionPos);
104 };
105 
106 typedef KyArrayPOD<VelocitySample, MemStat_PathFollowing, Kaim::ArrayConstPolicy<0, 16, true> > VelocitySampleArrayType;
107 
108 class VelocitySampleArray
109 {
110 public:
111  void Clear();
112  void ComputeTurningVelocitiesSamples(const AvoidanceSolverConfig& config, const Vec2f& movingDirection, KyFloat32 timeHorizon);
113 
114  void ScoreForDistanceToDirection(const Vec2f& direction, VelocitySample::ScoreVelocityFunction scoreFunction);
115  void ScoreForPreviousDirection(const Vec2f& previousDirection, KyFloat32 previousSpeed);
116  KyUInt32 FindClosestSampleToDirection(const Vec2f& direction);
117  KyFloat32 SelectBestVelocitySample(const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, const VelocitySample*& bestSample, const Vec2f& previousDirection);
118 
119  bool ScoreForAvoidance(const AvoidanceSolverConfig& config, KyFloat32 timeHorizon, KyFloat32 movingSpeed,
120  LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData, PerformRaycastFromBotPositionCallback raycastFunction,
121  PerformDiskCollisionFromBotPositionCallback diskCollisionFunction);
122 
123  void DebugDrawSample(DisplayList& displayList_Samples, const VelocitySample& velocitySample, const Vec3f& rootPosition,
124  VelocitySample::ScoreAccessorFunction scoreAccessorFunction, const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, KyFloat32 movingSpeed);
125 
126  void DebugDrawSamples(const char* groupName, const char* displayListName, Kaim::Bot* bot, VelocitySample::ScoreAccessorFunction scoreAccessorFunction,
127  const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, KyFloat32 movingSpeed);
128 
129  void DebugDrawSamplesCurve(const char* groupName, const char* displayListName, Kaim::Bot* bot, VelocitySample::ScoreAccessorFunction scoreAccessorFunction,
130  const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, KyFloat32 movingSpeed);
131 
132  void DebugDraw(AvoidanceSolverConfig& config, const NormalizedUtilityFunctionInfluenceRatios& utilityFunctionRatios, const Vec2f& previousVelocity,
133  const Vec2f& desiredVelocity, const Vec2f& outputVelocity, const VelocitySample& noAvoidanceSample, const VelocitySample& favoriteSample, KyFloat32 movingSpeed);
134 
135 public:
136  VelocitySampleArrayType m_velocitySamples;
137 };
138 
139 
140 // what's is point of this AvoidanceSolverImplementation class ?
141 class AvoidanceSolverImplementation
142 {
143  KY_DEFINE_NEW_DELETE_OPERATORS(MemStat_PathFollowing)
144 
145 public:
146  AvoidanceSolverImplementation() {}
147 
148  AvoidanceResult Solve(AvoidanceSolverConfig& config, const Vec2f& previousVelocity, const Vec2f& desiredVelocity,
149  Vec2f& outputVelocity, Vec2f& outputFrontDirection, bool& collide,
150  PerformRaycastFromBotPositionCallback raycastFunction,
151  PerformDiskCollisionFromBotPositionCallback diskCollisionFunction);
152 
153  AvoidanceResult ComputeAvoidanceVelocity(
154  const Vec2f& previousVelocity, const Vec2f& desiredVelocity, Vec2f& outputVelocity, Vec2f& outputFrontDirection,
155  PerformRaycastFromBotPositionCallback raycastFunction,
156  PerformDiskCollisionFromBotPositionCallback diskCollisionFunction);
157 
158  void ComputeMovingDirection(Vec2f& movingDirection);
159 
160  KyFloat32 ComputeTimeHorizon(KyFloat32 movingSpeed);
161 
162  bool CapDirectionChangeAngle(const Vec2f& normalizedFrom, const Vec2f& normalizedTo, Vec2f& result, CosAndSinAngle& maxAngle);
163 
164  void SolveContact(const KyFloat32 maxSpeed, const Vec2f& avoidanceDirection, bool& hasCollided, Vec2f& collideSolveDirection, KyFloat32& finalSpeed);
165 
166  void CapAvoidanceVelocityChange(
167  const KyFloat32 previousSpeed, const bool hasCollided, const Vec2f& previousDirection, const KyFloat32 movingSpeed,
168  Vec2f& finalVelocityDirection, KyFloat32& finalSpeed, Vec2f& finalFrontDirection);
169 
170  bool IsDirectionValid(const Vec2f& direction, KyFloat32 speed, KyFloat32 time,
171  PerformRaycastFromBotPositionCallback raycastFunction,
172  LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData);
173 
174 private:
175  VelocitySampleArray m_velocitySamplesArray;
176  LocalCostAwareTraverseLogicData m_localCostAwareTraverseLogicData;
177 
178  AvoidanceSolverConfig m_config;
179  Vec2f m_previousVelocityDirection;
180  KyFloat32 m_previousSpeed;
181  Vec2f m_followVelocityDirection;
182  KyFloat32 m_followSpeed;
183 };
184 
187 template<class TraverseLogic>
189 {
190  KY_DEFINE_NEW_DELETE_OPERATORS(MemStat_PathFollowing)
191 
192 public:
193  // ------------------------------ Functions -----------------------------
195  {}
196 
203  virtual AvoidanceResult Solve(
204  AvoidanceSolverConfig& config, const Vec2f& previousVelocity, const Vec2f& desiredVelocity,
205  Vec2f& outputVelocity, Vec2f& outputFrontDirection, bool& collide);
206 
207 protected:
208  static DiskCollisionQueryResult PerformDiskCollisionFromBotPosition(
209  const AvoidanceSolverConfig& config, KyFloat32 speed, KyFloat32 time,
210  LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData);
211 
212  static RayCastQueryResult PerformRaycastFromBotPosition(
213  const AvoidanceSolverConfig& config, const Vec2f& direction, KyFloat32 speed, KyFloat32 time,
214  LocalCostAwareTraverseLogicData& localCostAwareTraverseLogicData, Vec3f& collisionPos);
215 
216  AvoidanceSolverImplementation m_impl;
217 };
218 
219 } // namespace Kaim
220 
222 
This class is the world element that represent an active character in Autodesk Navigation.
Definition: bot.h:128
The trajectory goes in target direction at the desired speed.
Definition: iavoidancecomputer.h:20
std::uint32_t KyUInt32
uint32_t
Definition: types.h:29
AvoidanceSolver computes a velocity compatible with all the colliders in AvoidanceWorkspace and the N...
Definition: avoidancesolver.h:188
DiskCollisionQueryResult
Enumerates the possible results of a DiskCollisionQuery.
Definition: basediskcollisionquery.h:18
AvoidanceResult
Enumerates the possible results of a call to IAvoidanceComputer::Compute().
Definition: iavoidancecomputer.h:18
#define KY_DEFINE_NEW_DELETE_OPERATORS(MemStat)
This macro defines new and delete operators.
Definition: memory.h:132
RayCastQueryResult
Enumerates the possible results of a RayCastQuery.
Definition: baseraycastquery.h:24
2d vector using KyFloat32.
Definition: vec2f.h:18
The Autodesk Navigation namespace.
Definition: gamekitcrowddispersion.cpp:17
std::int32_t KyInt32
int32_t
Definition: types.h:24
virtual AvoidanceResult Solve(AvoidanceSolverConfig &config, const Vec2f &previousVelocity, const Vec2f &desiredVelocity, Vec2f &outputVelocity, Vec2f &outputFrontDirection, bool &collide)
Compute the more appropriate velocity.
Definition: avoidancesolver.inl:11
Interface class for Avoidance solvers.
Definition: iavoidancesolver.h:84
float KyFloat32
float
Definition: types.h:32
3d vector using 32bits floating points.
Definition: vec3f.h:16