gwnavruntime/queries/utils/rectangleintersector.h Source File

rectangleintersector.h
Go to the documentation of this file.
1 /*
2 * Copyright 2015 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 // Primary contact: JUBA - secondary contact: NOBODY
9 #ifndef Navigation_RectangleIntersector_H
10 #define Navigation_RectangleIntersector_H
11 
17 
18 namespace Kaim
19 {
20 
21 /*
22 class RectangleIntersector
23 */
24 class RectangleIntersector
25 {
26  KY_DEFINE_NEW_DELETE_OPERATORS(MemStat_Query)
27 
28 public:
29  RectangleIntersector(BaseSegmentCastQuery& query) : m_maxDist(query.GetMaxDist())
30  {
31  const DatabaseGenMetrics& databaseGenMetrics = query.m_database->GetDatabaseGenMetrics();
32 
33  m_integerPrecision = databaseGenMetrics.m_integerPrecision;
34 
35  Vec2f ortho = query.GetNormalizedDir2d().PerpCCW();
36  m_startA.Set(query.GetStartPos().x + (ortho.x * query.GetRadius()), query.GetStartPos().y + (ortho.y * query.GetRadius()), query.GetStartPos().z);
37  m_startB.Set(query.GetStartPos().x - (ortho.x * query.GetRadius()), query.GetStartPos().y - (ortho.y * query.GetRadius()), query.GetStartPos().z);
38 
39  m_rectangle = OrientedBox2d(m_startB, query.GetNormalizedDir2d(), m_maxDist, 2.f * query.GetRadius(), 0.f);
40 
41  const Vec2f direction2f(query.GetNormalizedDir2d() * m_maxDist);
42  Vec2f destPos2f(query.GetStartPos().x + direction2f.x, query.GetStartPos().y + direction2f.y);
43  const CoordPos64 destCoordPos(databaseGenMetrics.GetCoordPos64FromVec2f(destPos2f));
44 
45  databaseGenMetrics.GetCoordPos64FromVec3f(m_startB, m_o1);
46  const CoordPos64 startCoordPos = databaseGenMetrics.GetCoordPos64FromWorldIntegerPos(query.GetStartIntegerPos());
47  m_lengthVec = destCoordPos - startCoordPos;
48  m_widthVec = (startCoordPos - m_o1) * 2;
49  }
50 
51  RectangleIntersector(BaseSegmentCanGoQuery& query)
52  {
53  const DatabaseGenMetrics& databaseGenMetrics = query.m_database->GetDatabaseGenMetrics();
54 
55  m_integerPrecision = databaseGenMetrics.m_integerPrecision;
56 
57  Vec2f dir(query.GetDestPos().x - query.GetStartPos().x, query.GetDestPos().y - query.GetStartPos().y);
58  dir.Normalize();
59  dir = dir.PerpCCW();
60  Vec3f normalizedDir3D(dir.x, dir.y, 0.f);
61 
62  const Vec3f o1Floats(query.GetStartPos() - (normalizedDir3D * query.GetRadius()));
63 
64  databaseGenMetrics.GetCoordPos64FromVec3f(o1Floats, m_o1);
65  const CoordPos64 startCoordPos = databaseGenMetrics.GetCoordPos64FromWorldIntegerPos(query.GetStartIntegerPos());
66  m_lengthVec = databaseGenMetrics.GetCoordPos64FromWorldIntegerPos(query.GetDestIntegerPos()) - startCoordPos;
67  m_widthVec = (startCoordPos - m_o1) * 2;
68  }
69 
70  bool DoesIntersectEdge(const CoordPos64& startEdgeCoordPos, const CoordPos64& endEdgeCoordPos)
71  {
72  return Intersections::SegmentVsOrientedBox2d(startEdgeCoordPos, endEdgeCoordPos, m_o1, m_lengthVec, m_widthVec);
73  }
74 
75  void ComputeCollisionPosOnEdge(const CoordPos64& startEdgeCoordPos, const CoordPos64& endEdgeCoordPos, Vec3f& collisionPos, KyFloat32& squareDistToCollisionPos)
76  {
77  const Vec3f startEdgePos(startEdgeCoordPos.x * m_integerPrecision, startEdgeCoordPos.y * m_integerPrecision, 0.f);
78  const Vec3f endEdgePos(endEdgeCoordPos.x * m_integerPrecision, endEdgeCoordPos.y * m_integerPrecision, 0.f);
79 
80  ClosestPoint::OnSegmentVsSegmentCast2d(startEdgePos, endEdgePos, m_startA, m_startB, m_maxDist, collisionPos, squareDistToCollisionPos);
81  }
82 
83  void ComputeTriangleCost(const CoordPos64& v0CoordPos, const CoordPos64& v1CoordPos, const CoordPos64& v2CoordPos, KyFloat32& cost)
84  {
85  const Vec3f v0(v0CoordPos.x * m_integerPrecision, v0CoordPos.y * m_integerPrecision, 0.f);
86  const Vec3f v1(v1CoordPos.x * m_integerPrecision, v1CoordPos.y * m_integerPrecision, 0.f);
87  const Vec3f v2(v2CoordPos.x * m_integerPrecision, v2CoordPos.y * m_integerPrecision, 0.f);
88 
89  Vec3f unused;
90  ClosestPoint::OnTriangleVsSegmentCast2d(v0, v1, v2, m_startA, m_startB, m_maxDist, unused, cost);
91  }
92 
93 private:
94  /*
95  o4 +-----------------------+ o3
96  | |
97  | |
98  | |
99  o1 +---------------------->+
100  m_lengthVec
101  */
102 
103  CoordPos64 m_o1;
104  Vec2LL m_lengthVec;
105  Vec2LL m_widthVec;
106 
107  OrientedBox2d m_rectangle;
108  Vec3f m_startA;
109  Vec3f m_startB;
110  KyFloat32 m_maxDist;
111 
112 
113  KyFloat32 m_integerPrecision;
114 
115 };
116 
117 
118 }
119 
120 
121 #endif //Navigation_RectangleIntersector_H
122 
Vec2LL PerpCCW() const
Rotates this vector 90 degrees counter-clockwise (negating the Y coordinate).
Definition: vec2ll.h:148
Vec2LL CoordPos64
A type that represents the position of a point within the 2D integer grid.
Definition: navmeshtypes.h:19
Definition: gamekitcrowddispersion.h:20
#define KY_DEFINE_NEW_DELETE_OPERATORS(MemStat)
This macro defines new and delete operators.
Definition: memory.h:137
float KyFloat32
Type used internally to represent a 32-bit floating-point number.
Definition: types.h:43