gwnavruntime/queries/utils/bestfirstsearchedgecollisionvisitor.h Source File

bestfirstsearchedgecollisionvisitor.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 
8 
9 
10 
11 #pragma once
12 
14 
15 namespace Kaim
16 {
17 
18 /*
19 class BestFirstSearchEdgeCollisionVisitor
20 
21 EdgeIntersector must have the following functions :
22 
23  bool DoesIntersectEdge(const Vec3f& startEdgePos, const Vec3f& endEdgePos);
24 
25  void ComputeCollisionPosOnEdge(const Vec3f& startEdgePos, const Vec3f& endEdgePos, Vec3f& collisionPos, KyFloat32& squareDistToCollisionPos)
26 
27  void ComputeTriangleCost(const Vec3f& v0, const Vec3f& v1, const Vec3f& v2, KyFloat32& cost)
28 
29 */
30 template<class TraverseLogic, class EdgeIntersector>
31 class BestFirstSearchEdgeCollisionVisitor
32 {
33  KY_DEFINE_NEW_DELETE_OPERATORS(MemStat_Query)
34 
35 public:
36  BestFirstSearchEdgeCollisionVisitor(void* traverseLogicUserData, const CellBox& cellBox, EdgeIntersector& edgeIntersector);
37 
38  bool IsSearchFinished();
39 
40  bool ShouldVisitNeighborTriangle(const NavTriangleRawPtr& triangleRawPtr, KyUInt32 indexOfNeighborTriangle);
41  bool ShouldVisitTriangle(const NavTriangleRawPtr& triangleRawPtr);
42  void ComputeTriangleCost(const NavTriangleRawPtr& triangleRawPtr, KyFloat32& cost);
43 
44  NavTriangleRawPtr GetNeighborTriangle(const NavTriangleRawPtr& triangleRawPtr, KyUInt32 indexOfNeighborTriangle);
45 
46  void Visit(const NavTriangleRawPtr& triangleRawPtr, KyFloat32 cost, const TriangleStatusInGrid& triangleStatus);
47 
48 public:
49  EdgeIntersector* m_edgeIntersector;
50  void* m_traverseLogicUserData;
51  bool m_collisionFound;
52 
53  KyFloat32 m_squareDistToCollisionPos;
54  Vec3f m_collisionPos;
55  NavHalfEdgeRawPtr m_intersectedHalfEdgeRawPtr;
56 
57 private:
58  CellBox m_cellBox;
59  KyFloat32 m_lastCost;
60  bool m_shouldVisitNeighborTriangle[3];
61  NavTriangleRawPtr m_neighborTriangle[3];
62 };
63 
64 template<class TraverseLogic, class EdgeIntersector>
65 KY_INLINE BestFirstSearchEdgeCollisionVisitor<TraverseLogic, EdgeIntersector>::BestFirstSearchEdgeCollisionVisitor(void* traverseLogicUserData, const CellBox& cellBox, EdgeIntersector& edgeIntersector)
66  : m_edgeIntersector(&edgeIntersector)
67  , m_traverseLogicUserData(traverseLogicUserData)
68  , m_collisionFound(false)
69  ,m_squareDistToCollisionPos(KyFloat32MAXVAL)
70  , m_cellBox(cellBox)
71  , m_lastCost(KyFloat32MAXVAL)
72 {
73  m_shouldVisitNeighborTriangle[0] = true;
74  m_shouldVisitNeighborTriangle[1] = true;
75  m_shouldVisitNeighborTriangle[2] = true;
76 }
77 
78 template<class TraverseLogic, class EdgeIntersector>
79 KY_INLINE bool BestFirstSearchEdgeCollisionVisitor<TraverseLogic, EdgeIntersector>::IsSearchFinished() { return m_lastCost > m_squareDistToCollisionPos; }
80 
81 template<class TraverseLogic, class EdgeIntersector>
82 KY_INLINE bool BestFirstSearchEdgeCollisionVisitor<TraverseLogic, EdgeIntersector>::ShouldVisitNeighborTriangle(const NavTriangleRawPtr& /*triangleRawPtr*/, KyUInt32 indexOfNeighborTriangle)
83 {
84  return m_shouldVisitNeighborTriangle[indexOfNeighborTriangle];
85 }
86 
87 template<class TraverseLogic, class EdgeIntersector>
88 KY_INLINE bool BestFirstSearchEdgeCollisionVisitor<TraverseLogic, EdgeIntersector>::ShouldVisitTriangle(const NavTriangleRawPtr& triangleRawPtr)
89 {
90  return m_cellBox.DoesContain(triangleRawPtr.GetCellPos());
91 }
92 
93 template<class TraverseLogic, class EdgeIntersector>
94 KY_INLINE NavTriangleRawPtr BestFirstSearchEdgeCollisionVisitor<TraverseLogic, EdgeIntersector>::GetNeighborTriangle(const NavTriangleRawPtr& /*triangleRawPtr*/, KyUInt32 indexOfNeighborTriangle)
95 {
96  return m_neighborTriangle[indexOfNeighborTriangle];
97 }
98 
99 template<class TraverseLogic, class EdgeIntersector>
100 void BestFirstSearchEdgeCollisionVisitor<TraverseLogic, EdgeIntersector>::ComputeTriangleCost(const NavTriangleRawPtr& triangleRawPtr, KyFloat32& cost)
101 {
102  CoordPos64 v0, v1, v2;
103  triangleRawPtr.GetVerticesCoordPos64(v0, v1, v2);
104  m_edgeIntersector->ComputeTriangleCost(v0, v1, v2, cost);
105 }
106 
107 template<class TraverseLogic, class EdgeIntersector>
108 void BestFirstSearchEdgeCollisionVisitor<TraverseLogic, EdgeIntersector>::Visit(const NavTriangleRawPtr& triangleRawPtr, const KyFloat32 triangleCost,
109  const TriangleStatusInGrid& triangleStatus)
110 {
111  CoordPos64 v0, v1, v2;
112  CoordPos64* vertex[4] = { &v0, &v1, &v2, &v0};
113  NavHalfEdgeRawPtr halfEdgeRawPtr[3];
114  NavHalfEdge halfEdge[3];
115  bool isACrossableEdges[3];
116  bool neighborTriangleAlreadyVisited[3];
117  bool doesIntersectsEdges[3];
118  bool isNeighborInCellBox[3];
119 
120  const NavHalfEdgeIdx firstNavHalfEdgeIdx = NavFloorBlob::NavTriangleIdxToFirstNavHalfEdgeIdx(triangleRawPtr.GetTriangleIdx());
121  NavFloor* navFloor = triangleRawPtr.GetNavFloor();
122 
123  for(KyUInt32 i = 0; i < 3; ++i)
124  halfEdgeRawPtr[i].Set(navFloor, firstNavHalfEdgeIdx + i);
125 
126  triangleRawPtr.GetVerticesCoordPos64(v0, v1, v2);
127 
128  const NavFloorBlob& navFloorBlob = *navFloor->GetNavFloorBlob();
129  const NavHalfEdge* m_navHalfEdges = navFloorBlob.m_navHalfEdges.GetValues();
130 
131  for(KyUInt32 i = 0; i < 3; ++i)
132  halfEdge[i] = m_navHalfEdges[firstNavHalfEdgeIdx + i];
133 
134  for(KyUInt32 i = 0; i < 3; ++i)
135  {
136  NavHalfEdgeRawPtr pairHalfEdgeRawPtr;
137  isACrossableEdges[i] = halfEdgeRawPtr[i].IsHalfEdgeCrossable<TraverseLogic>(m_traverseLogicUserData, halfEdge[i], &navFloorBlob, pairHalfEdgeRawPtr);
138 
139  if (isACrossableEdges[i])
140  {
141  isNeighborInCellBox[i] = m_cellBox.DoesContain(pairHalfEdgeRawPtr.GetCellPos());
142 
143  bool inSameFloor = halfEdge[i].GetHalfEdgeType() == EDGETYPE_PAIRED || halfEdge[i].GetHalfEdgeType() == EDGETYPE_CONNEXBOUNDARY;
144  m_neighborTriangle[i].Set(pairHalfEdgeRawPtr.GetNavFloor(), NavFloorBlob::NavHalfEdgeIdxToTriangleIdx(pairHalfEdgeRawPtr.GetHalfEdgeIdx()));
145 
146  if (inSameFloor)
147  neighborTriangleAlreadyVisited[i] = triangleStatus.IsTriangleOpen_Unsafe(m_neighborTriangle[i]);
148  else
149  neighborTriangleAlreadyVisited[i] = triangleStatus.IsTriangleOpen(m_neighborTriangle[i]);
150  }
151  else
152  {
153  isNeighborInCellBox[i] = true;
154  neighborTriangleAlreadyVisited[i] = false;
155  }
156  }
157 
158  for(KyUInt32 i = 0; i < 3; ++i)
159  doesIntersectsEdges[i] = neighborTriangleAlreadyVisited[i] || m_edgeIntersector->DoesIntersectEdge(*vertex[i], *vertex[i+1]);
160 
161  m_lastCost = triangleCost;
162 
163  if (isACrossableEdges[0] == false || isACrossableEdges[1] == false || isACrossableEdges[2] == false)
164  {
165  Vec3f collisionPos;
166  KyFloat32 squareDistToCollisionPos;
167 
168  bool aCollisionFoundIsThisTriangle = false;
169 
170  for(KyUInt32 i = 0; i < 3; ++i)
171  {
172  if (isACrossableEdges[i] == false && doesIntersectsEdges[i])
173  {
174  m_edgeIntersector->ComputeCollisionPosOnEdge(*vertex[i], *vertex[i+1], collisionPos, squareDistToCollisionPos);
175  if (squareDistToCollisionPos < m_squareDistToCollisionPos)
176  {
177  m_squareDistToCollisionPos = squareDistToCollisionPos;
178  aCollisionFoundIsThisTriangle = true;
179  m_collisionPos = collisionPos;
180  m_intersectedHalfEdgeRawPtr.Set(triangleRawPtr.m_navFloorRawPtr, NavFloorBlob::NavTriangleIdxToNavHalfEdgeIdx(triangleRawPtr.GetTriangleIdx(), i));
181  }
182  }
183  }
184 
185  m_collisionFound = m_collisionFound || aCollisionFoundIsThisTriangle;
186  }
187 
188  for(KyUInt32 i = 0; i < 3; ++i)
189  m_shouldVisitNeighborTriangle[i] = isACrossableEdges[i] && neighborTriangleAlreadyVisited[i] == false &&
190  doesIntersectsEdges[i] && isNeighborInCellBox[i];
191 }
192 
193 }
194 
195 
196 
#define KyFloat32MAXVAL
KyFloat32 max value
Definition: types.h:71
Box2i CellBox
A type that represents a bounding box around cells in a 2D grid.
Definition: navmeshtypes.h:31
std::uint32_t KyUInt32
uint32_t
Definition: types.h:29
KyUInt32 NavHalfEdgeIdx
An index that uniquely identifies a single edge of a triangle within the set of edges owned by a NavF...
Definition: navmeshtypes.h:84
Indicates that another NavHalfEdge in the same NavFloor but in different Connex lies adjacent to the ...
Definition: navmeshtypes.h:57
static NavHalfEdgeIdx NavTriangleIdxToFirstNavHalfEdgeIdx(NavTriangleIdx idx)
Retrieves the first NavHalfEdgeIdx of NavTriangle specified by the input NavTriangleIdx.
Definition: navfloorblob.inl:23
#define KY_DEFINE_NEW_DELETE_OPERATORS(MemStat)
This macro defines new and delete operators.
Definition: memory.h:132
Vec2LL CoordPos64
A type that represents the position of a point within the 2D integer grid.
Definition: navmeshtypes.h:16
The Autodesk Navigation namespace.
Definition: gamekitcrowddispersion.cpp:17
static NavTriangleIdx NavHalfEdgeIdxToTriangleIdx(NavHalfEdgeIdx idx)
Retrieves the index of the triangle that contains the edge specified the input NavHalfEdgeIdx.
Definition: navfloorblob.inl:22
Indicates that another NavHalfEdge in the same Connex lies adjacent to the NavHalfEdge.
Definition: navmeshtypes.h:58
static NavHalfEdgeIdx NavTriangleIdxToNavHalfEdgeIdx(NavTriangleIdx idx, KyInt32 halfEdgeNumber)
Retrieves one edge from the specified triangle.
Definition: navfloorblob.inl:25
float KyFloat32
float
Definition: types.h:32