Newton Dynamics  4.00
ndAabbPolygonSoup.h
1 /* Copyright (c) <2003-2022> <Julio Jerez, Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
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 *
11 * 1. The origin of this software must not be misrepresented; you must not
12 * claim that you wrote the original software. If you use this software
13 * in a product, an acknowledgment in the product documentation would be
14 * appreciated but is not required.
15 *
16 * 2. Altered source versions must be plainly marked as such, and must not be
17 * misrepresented as being the original software.
18 *
19 * 3. This notice may not be removed or altered from any source distribution.
20 */
21 
22 #ifndef __NDG_AABB_POLYGONSOUP_H_
23 #define __NDG_AABB_POLYGONSOUP_H_
24 
25 #include "ndCoreStdafx.h"
26 #include "ndTypes.h"
27 #include "ndUtils.h"
28 #include "ndFastRay.h"
29 #include "ndFastAabb.h"
30 #include "ndIntersections.h"
31 #include "ndPolygonSoupDatabase.h"
32 
34 
35 // index format: i0, i1, i2, ... , id, normal, e0Normal, e1Normal, e2Normal, ..., faceSize
36 #define D_CONCAVE_EDGE_MASK (1<<31)
37 #define D_FACE_CLIP_DIAGONAL_SCALE ndFloat32 (0.25f)
38 
39 enum ndIntersectStatus
40 {
41  m_stopSearch,
42  m_continueSearh
43 };
44 
45 typedef ndIntersectStatus(*ndAaabbIntersectCallback) (void* const context,
46  const ndFloat32* const polygon, ndInt32 strideInBytes,
47  const ndInt32* const indexArray, ndInt32 indexCount, ndFloat32 hitDistance);
48 
49 typedef ndFloat32(*ndRayIntersectCallback) (void* const context,
50  const ndFloat32* const polygon, ndInt32 strideInBytes,
51  const ndInt32* const indexArray, ndInt32 indexCount);
52 
55 {
56  public:
57  class ndNode
58  {
59  public:
60  enum ndNodeType
61  {
62  m_binary = 0,
63  m_leaf,
64  };
65 
67  {
68  #define DG_INDEX_COUNT_BITS 6
69 
70  public:
71  inline ndLeafNodePtr ()
72  {
73  ndAssert (0);
74  }
75 
76  inline ndLeafNodePtr (ndUnsigned32 node)
77  {
78  m_node = node;
79  ndAssert (!IsLeaf());
80  }
81 
82  inline ndUnsigned32 IsLeaf () const
83  {
84  return m_node & 0x80000000;
85  }
86 
87  inline ndUnsigned32 GetCount() const
88  {
89  ndAssert (IsLeaf());
90  return (m_node & (~0x80000000)) >> (32 - DG_INDEX_COUNT_BITS - 1);
91  }
92 
93  inline ndUnsigned32 GetIndex() const
94  {
95  ndAssert (IsLeaf());
96  return m_node & (~(-(1 << (32 - DG_INDEX_COUNT_BITS - 1))));
97  }
98 
99  inline ndLeafNodePtr (ndUnsigned32 faceIndexCount, ndUnsigned32 faceIndexStart)
100  {
101  ndAssert (faceIndexCount < (1<<DG_INDEX_COUNT_BITS));
102  m_node = 0x80000000 | (faceIndexCount << (32 - DG_INDEX_COUNT_BITS - 1)) | faceIndexStart;
103  }
104 
105  inline ndNode* GetNode (const void* const root) const
106  {
107  return ((ndNode*) root) + m_node;
108  }
109 
110  ndUnsigned32 m_node;
111  };
112 
113  ndNode ()
114  :m_indexBox0(0)
115  ,m_indexBox1(0)
116  ,m_left(0)
117  ,m_right(0)
118  {
119  }
120 
121  inline ndFloat32 RayDistance (const ndFastRay& ray, const ndTriplex* const vertexArray) const
122  {
123  ndVector minBox (&vertexArray[m_indexBox0].m_x);
124  ndVector maxBox (&vertexArray[m_indexBox1].m_x);
125  minBox = minBox & ndVector::m_triplexMask;
126  maxBox = maxBox & ndVector::m_triplexMask;
127  return ray.BoxIntersect(minBox, maxBox);
128  }
129 
130  inline ndFloat32 BoxPenetration (const ndFastAabb& obb, const ndTriplex* const vertexArray) const
131  {
132  ndVector p0 (&vertexArray[m_indexBox0].m_x);
133  ndVector p1 (&vertexArray[m_indexBox1].m_x);
134  p0 = p0 & ndVector::m_triplexMask;
135  p1 = p1 & ndVector::m_triplexMask;
136  ndVector minBox (p0 - obb.m_p1);
137  ndVector maxBox (p1 - obb.m_p0);
138  ndAssert(maxBox.m_x >= minBox.m_x);
139  ndAssert(maxBox.m_y >= minBox.m_y);
140  ndAssert(maxBox.m_z >= minBox.m_z);
141 
142  ndVector mask ((minBox * maxBox) < ndVector::m_zero);
143  ndVector dist (maxBox.GetMin (minBox.Abs()) & mask);
144  dist = dist.GetMin(dist.ShiftTripleRight());
145  dist = dist.GetMin(dist.ShiftTripleRight());
146 
147  if (dist.GetScalar() > ndFloat32 (0.0f))
148  {
149  ndVector origin (ndVector::m_half * (p1 + p0));
150  ndVector size (ndVector::m_half * (p1 - p0));
151 
152  origin = obb.UntransformVector(origin);
153  size = obb.m_absDir.RotateVector(size);
154  ndVector q0 (origin - size);
155  ndVector q1 (origin + size);
156  ndVector minBox1 (q0 - obb.m_size);
157  ndVector maxBox1 (q1 + obb.m_size);
158  ndAssert(maxBox1.m_x >= minBox1.m_x);
159  ndAssert(maxBox1.m_y >= minBox1.m_y);
160  ndAssert(maxBox1.m_z >= minBox1.m_z);
161  ndVector mask1 ((minBox1 * maxBox1) < ndVector::m_zero);
162  ndVector dist1 (maxBox1.GetMin (minBox1.Abs()) & mask1);
163  dist1 = dist1.GetMin(dist1.ShiftTripleRight());
164  dist1 = dist1.GetMin(dist1.ShiftTripleRight());
165  dist = dist.GetMin(dist1);
166  }
167  else
168  {
169  ndVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask));
170  dist = p1p0.DotProduct(p1p0);
171  dist = dist.Sqrt() * ndVector::m_negOne;
172  }
173  return dist.GetScalar();
174  }
175 
176  inline ndFloat32 BoxIntersect (const ndFastRay& ray, const ndFastRay& obbRay, const ndFastAabb& obb, const ndTriplex* const vertexArray) const
177  {
178  ndVector p0 (&vertexArray[m_indexBox0].m_x);
179  ndVector p1 (&vertexArray[m_indexBox1].m_x);
180  p0 = p0 & ndVector::m_triplexMask;
181  p1 = p1 & ndVector::m_triplexMask;
182 
183  ndVector minBox (p0 - obb.m_p1);
184  ndVector maxBox (p1 - obb.m_p0);
185  ndFloat32 dist = ray.BoxIntersect(minBox, maxBox);
186  if (dist < ndFloat32 (1.0f))
187  {
188  ndVector origin (ndVector::m_half * (p1 + p0));
189  ndVector size (ndVector::m_half * (p1 - p0));
190 
191  origin = obb.UntransformVector(origin);
192  size = obb.m_absDir.RotateVector(size);
193  ndVector q0 (origin - size);
194  ndVector q1 (origin + size);
195 
196  ndVector minBox1 (q0 - obb.m_size);
197  ndVector maxBox1 (q1 + obb.m_size);
198  ndFloat32 dist1 = obbRay.BoxIntersect(minBox1, maxBox1);
199  dist = (dist1 > ndFloat32 (1.0f)) ? dist1 : ndMax (dist1, dist);
200  }
201  return dist;
202  }
203 
204  ndInt32 m_indexBox0;
205  ndInt32 m_indexBox1;
206  ndLeafNodePtr m_left;
207  ndLeafNodePtr m_right;
208  };
209 
210  class ndSplitInfo;
211  class ndNodeBuilder;
212 
214  D_CORE_API virtual void GetAABB (ndVector& p0, ndVector& p1) const;
215 
217  D_CORE_API virtual void Serialize (const char* const path) const;
218 
220  D_CORE_API virtual void Deserialize (const char* const path);
221 
222  protected:
223  D_CORE_API ndAabbPolygonSoup ();
224  D_CORE_API virtual ~ndAabbPolygonSoup ();
225 
226  D_CORE_API void Create (const ndPolygonSoupBuilder& builder);
227  D_CORE_API void CalculateAdjacent ();
228  D_CORE_API virtual ndVector ForAllSectorsSupportVertex(const ndVector& dir) const;
229  D_CORE_API virtual void ForAllSectorsRayHit (const ndFastRay& ray, ndFloat32 maxT, ndRayIntersectCallback callback, void* const context) const;
230  D_CORE_API virtual void ForAllSectors (const ndFastAabb& obbAabb, const ndVector& boxDistanceTravel, ndFloat32 maxT, ndAaabbIntersectCallback callback, void* const context) const;
231  D_CORE_API virtual void ForThisSector(const ndAabbPolygonSoup::ndNode* const node, const ndFastAabb& obbAabb, const ndVector& boxDistanceTravel, ndFloat32 maxT, ndAaabbIntersectCallback callback, void* const context) const;
232 
233  public:
235  inline ndNode* GetRootNode() const
236  {
237  return m_aabb;
238  }
239 
242  inline ndNode* GetBackNode(const ndNode* const node) const
243  {
244  return node->m_left.IsLeaf() ? nullptr : node->m_left.GetNode(m_aabb);
245  }
246 
249  inline ndNode* GetFrontNode(const ndNode* const node) const
250  {
251  return node->m_right.IsLeaf() ? nullptr : node->m_right.GetNode(m_aabb);
252  }
253 
255  inline void GetNodeAabb(const ndNode* const node, ndVector& p0, ndVector& p1) const
256  {
257  p0 = ndVector (&((ndTriplex*)m_localVertex)[node->m_indexBox0].m_x);
258  p1 = ndVector (&((ndTriplex*)m_localVertex)[node->m_indexBox1].m_x);
259  p0 = p0 & ndVector::m_triplexMask;
260  p1 = p1 & ndVector::m_triplexMask;
261  }
262 
263  private:
264  ndNodeBuilder* BuildTopDown (ndNodeBuilder* const leafArray, ndInt32 firstBox, ndInt32 lastBox, ndNodeBuilder** const allocator) const;
265  ndFloat32 CalculateFaceMaxDiagonal (const ndVector* const vertex, ndInt32 indexCount, const ndInt32* const indexArray) const;
266  static ndIntersectStatus CalculateAllFaceEdgeNormals(void* const context, const ndFloat32* const polygon, ndInt32 strideInBytes, const ndInt32* const indexArray, ndInt32 indexCount, ndFloat32 hitDistance);
267 
268  ndNode* m_aabb;
269  ndInt32* m_indices;
270  ndInt32 m_nodesCount;
271  ndInt32 m_indexCount;
272  friend class ndContactSolver;
273 };
274 
275 #endif
276 
277 
ndAabbPolygonSoup::GetFrontNode
ndNode * GetFrontNode(const ndNode *const node) const
Returns the front child node of the hierarchy.
Definition: ndAabbPolygonSoup.h:249
ndAabbPolygonSoup::Serialize
virtual D_CORE_API void Serialize(const char *const path) const
writes the entire database to a binary file named path.
Definition: ndAabbPolygonSoup.cpp:765
ndPolygonSoupDatabase
Definition: ndPolygonSoupDatabase.h:30
ndTriplex
Definition: ndTypes.h:284
ndAabbPolygonSoup::Deserialize
virtual D_CORE_API void Deserialize(const char *const path)
Reads a previously saved database binary file named path.
Definition: ndAabbPolygonSoup.cpp:783
ndContactSolver
Definition: ndContactSolver.h:60
ndPolygonSoupBuilder
Definition: ndPolygonSoupBuilder.h:48
ndAabbPolygonSoup::GetBackNode
ndNode * GetBackNode(const ndNode *const node) const
Returns the back child node of the hierarchy.
Definition: ndAabbPolygonSoup.h:242
ndAabbPolygonSoup::ndNode
Definition: ndAabbPolygonSoup.h:58
ndAabbPolygonSoup::ndNode::ndLeafNodePtr
Definition: ndAabbPolygonSoup.h:67
ndAabbPolygonSoup::GetNodeAabb
void GetNodeAabb(const ndNode *const node, ndVector &p0, ndVector &p1) const
Returns the bounding box of node in point p0 and p1.
Definition: ndAabbPolygonSoup.h:255
ndFastRay
Definition: ndFastRay.h:48
ndFastAabb
Definition: ndFastAabb.h:32
ndAabbPolygonSoup
Base class for creating a leafless bounding box hierarchy for queering a polygon list index list mesh...
Definition: ndAabbPolygonSoup.h:55
ndAabbPolygonSoup::GetAABB
virtual D_CORE_API void GetAABB(ndVector &p0, ndVector &p1) const
get the root node bounding box of the mesh.
Definition: ndAabbPolygonSoup.cpp:282
ndAabbPolygonSoup::GetRootNode
ndNode * GetRootNode() const
Get the root node of the hierarchy.
Definition: ndAabbPolygonSoup.h:235
ndVector
Definition: ndVectorArmNeon.h:41