Newton Dynamics  4.00
dAabbPolygonSoup.h
1 /* Copyright (c) <2003-2019> <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 __DG_AABB_POLYGONSOUP_H_
23 #define __DG_AABB_POLYGONSOUP_H_
24 
25 #include "dCoreStdafx.h"
26 #include "dTypes.h"
27 #include "dIntersections.h"
28 #include "dPolygonSoupDatabase.h"
29 
31 
33 {
34  public:
35  class dNode
36  {
37  public:
38  enum dNodeType
39  {
40  m_binary = 0,
41  m_leaf,
42  };
43 
45  {
46  #define DG_INDEX_COUNT_BITS 6
47 
48  public:
49  inline dgLeafNodePtr ()
50  {
51  dAssert (0);
52  }
53 
54  inline dgLeafNodePtr (dUnsigned32 node)
55  {
56  m_node = node;
57  dAssert (!IsLeaf());
58  }
59 
60  inline dUnsigned32 IsLeaf () const
61  {
62  return m_node & 0x80000000;
63  }
64 
65  inline dUnsigned32 GetCount() const
66  {
67  dAssert (IsLeaf());
68  return (m_node & (~0x80000000)) >> (32 - DG_INDEX_COUNT_BITS - 1);
69  }
70 
71  inline dUnsigned32 GetIndex() const
72  {
73  dAssert (IsLeaf());
74  return m_node & (~(-(1 << (32 - DG_INDEX_COUNT_BITS - 1))));
75  }
76 
77  inline dgLeafNodePtr (dUnsigned32 faceIndexCount, dUnsigned32 faceIndexStart)
78  {
79  dAssert (faceIndexCount < (1<<DG_INDEX_COUNT_BITS));
80  m_node = 0x80000000 | (faceIndexCount << (32 - DG_INDEX_COUNT_BITS - 1)) | faceIndexStart;
81  }
82 
83  inline dNode* GetNode (const void* const root) const
84  {
85  return ((dNode*) root) + m_node;
86  }
87 
88  union
89  {
90  dUnsigned32 m_node;
91  };
92  };
93 
94  dNode ()
95  :m_indexBox0(0)
96  ,m_indexBox1(0)
97  ,m_left(0)
98  ,m_right(0)
99  {
100  }
101 
102  inline dFloat32 RayDistance (const dFastRayTest& ray, const dTriplex* const vertexArray) const
103  {
104  dVector minBox (&vertexArray[m_indexBox0].m_x);
105  dVector maxBox (&vertexArray[m_indexBox1].m_x);
106  minBox = minBox & dVector::m_triplexMask;
107  maxBox = maxBox & dVector::m_triplexMask;
108  return ray.BoxIntersect(minBox, maxBox);
109  }
110 
111  inline dFloat32 BoxPenetration (const dFastAabbInfo& obb, const dTriplex* const vertexArray) const
112  {
113  dVector p0 (&vertexArray[m_indexBox0].m_x);
114  dVector p1 (&vertexArray[m_indexBox1].m_x);
115  p0 = p0 & dVector::m_triplexMask;
116  p1 = p1 & dVector::m_triplexMask;
117  dVector minBox (p0 - obb.m_p1);
118  dVector maxBox (p1 - obb.m_p0);
119  dAssert(maxBox.m_x >= minBox.m_x);
120  dAssert(maxBox.m_y >= minBox.m_y);
121  dAssert(maxBox.m_z >= minBox.m_z);
122 
123  dVector mask ((minBox * maxBox) < dVector::m_zero);
124  dVector dist (maxBox.GetMin (minBox.Abs()) & mask);
125  dist = dist.GetMin(dist.ShiftTripleRight());
126  dist = dist.GetMin(dist.ShiftTripleRight());
127 
128  if (dist.GetScalar() > dFloat32 (0.0f))
129  {
130  dVector origin (dVector::m_half * (p1 + p0));
131  dVector size (dVector::m_half * (p1 - p0));
132 
133  origin = obb.UntransformVector(origin);
134  size = obb.m_absDir.RotateVector(size);
135  dVector q0 (origin - size);
136  dVector q1 (origin + size);
137  dVector minBox1 (q0 - obb.m_size);
138  dVector maxBox1 (q1 + obb.m_size);
139  dAssert(maxBox1.m_x >= minBox1.m_x);
140  dAssert(maxBox1.m_y >= minBox1.m_y);
141  dAssert(maxBox1.m_z >= minBox1.m_z);
142  dVector mask1 ((minBox1 * maxBox1) < dVector::m_zero);
143  dVector dist1 (maxBox1.GetMin (minBox1.Abs()) & mask1);
144  dist1 = dist1.GetMin(dist1.ShiftTripleRight());
145  dist1 = dist1.GetMin(dist1.ShiftTripleRight());
146  dist = dist.GetMin(dist1);
147  }
148  else
149  {
150  dVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask));
151  dist = p1p0.DotProduct(p1p0);
152  dist = dist.Sqrt() * dVector::m_negOne;
153  }
154  return dist.GetScalar();
155  }
156 
157  inline dFloat32 BoxIntersect (const dFastRayTest& ray, const dFastRayTest& obbRay, const dFastAabbInfo& obb, const dTriplex* const vertexArray) const
158  {
159  dVector p0 (&vertexArray[m_indexBox0].m_x);
160  dVector p1 (&vertexArray[m_indexBox1].m_x);
161  p0 = p0 & dVector::m_triplexMask;
162  p1 = p1 & dVector::m_triplexMask;
163 
164  dVector minBox (p0 - obb.m_p1);
165  dVector maxBox (p1 - obb.m_p0);
166  dFloat32 dist = ray.BoxIntersect(minBox, maxBox);
167  if (dist < dFloat32 (1.0f))
168  {
169  dVector origin (dVector::m_half * (p1 + p0));
170  dVector size (dVector::m_half * (p1 - p0));
171 
172  origin = obb.UntransformVector(origin);
173  size = obb.m_absDir.RotateVector(size);
174  dVector q0 (origin - size);
175  dVector q1 (origin + size);
176 
177  dVector minBox1 (q0 - obb.m_size);
178  dVector maxBox1 (q1 + obb.m_size);
179  dFloat32 dist1 = obbRay.BoxIntersect(minBox1, maxBox1);
180  dist = (dist1 > dFloat32 (1.0f)) ? dist1 : dMax (dist1, dist);
181  }
182  return dist;
183  }
184 
185  dInt32 m_indexBox0;
186  dInt32 m_indexBox1;
187  dgLeafNodePtr m_left;
188  dgLeafNodePtr m_right;
189  };
190 
191  class dgSpliteInfo;
192  class dgNodeBuilder;
193 
194  D_CORE_API virtual void GetAABB (dVector& p0, dVector& p1) const;
195  D_CORE_API virtual void Serialize (const char* const path) const;
196  D_CORE_API virtual void Deserialize (const char* const path);
197 
198  protected:
199  D_CORE_API dAabbPolygonSoup ();
200  D_CORE_API virtual ~dAabbPolygonSoup ();
201 
202  D_CORE_API void Create (const dPolygonSoupBuilder& builder);
203  D_CORE_API void CalculateAdjacendy ();
204  D_CORE_API virtual void ForAllSectorsRayHit (const dFastRayTest& ray, dFloat32 maxT, dRayIntersectCallback callback, void* const context) const;
205  D_CORE_API virtual void ForAllSectors (const dFastAabbInfo& obbAabb, const dVector& boxDistanceTravel, dFloat32 m_maxT, dAaabbIntersectCallback callback, void* const context) const;
206  D_CORE_API virtual dVector ForAllSectorsSupportVectex(const dVector& dir) const;
207 
208  inline void* GetRootNode() const
209  {
210  return m_aabb;
211  }
212 
213  inline void* GetBackNode(const void* const root) const
214  {
215  dNode* const node = (dNode*) root;
216  return node->m_left.IsLeaf() ? NULL : node->m_left.GetNode(m_aabb);
217  }
218 
219  inline void* GetFrontNode(const void* const root) const
220  {
221  dNode* const node = (dNode*) root;
222  return node->m_right.IsLeaf() ? NULL : node->m_right.GetNode(m_aabb);
223  }
224 
225  inline void GetNodeAABB(const void* const root, dVector& p0, dVector& p1) const
226  {
227  const dNode* const node = (dNode*)root;
228  p0 = dVector (&((dTriplex*)m_localVertex)[node->m_indexBox0].m_x);
229  p1 = dVector (&((dTriplex*)m_localVertex)[node->m_indexBox1].m_x);
230  p0 = p0 & dVector::m_triplexMask;
231  p1 = p1 & dVector::m_triplexMask;
232  }
233 
234 
235 
236  private:
237  dgNodeBuilder* BuildTopDown (dgNodeBuilder* const leafArray, dInt32 firstBox, dInt32 lastBox, dgNodeBuilder** const allocator) const;
238  dFloat32 CalculateFaceMaxSize (const dVector* const vertex, dInt32 indexCount, const dInt32* const indexArray) const;
239 // static dIntersectStatus CalculateManifoldFaceEdgeNormals (void* const context, const dFloat32* const polygon, dInt32 strideInBytes, const dInt32* const indexArray, dInt32 indexCount);
240  static dIntersectStatus CalculateDisjointedFaceEdgeNormals (void* const context, const dFloat32* const polygon, dInt32 strideInBytes, const dInt32* const indexArray, dInt32 indexCount, dFloat32 hitDistance);
241  static dIntersectStatus CalculateAllFaceEdgeNormalsOld (void* const context, const dFloat32* const polygon, dInt32 strideInBytes, const dInt32* const indexArray, dInt32 indexCount, dFloat32 hitDistance);
242  static dIntersectStatus CalculateAllFaceEdgeNormals(void* const context, const dFloat32* const polygon, dInt32 strideInBytes, const dInt32* const indexArray, dInt32 indexCount, dFloat32 hitDistance);
243  void ImproveNodeFitness (dgNodeBuilder* const node) const;
244 
245  dInt32 m_nodesCount;
246  dInt32 m_indexCount;
247  dNode* m_aabb;
248  dInt32* m_indices;
249 };
250 
251 
252 #endif
253 
254 
dFastRayTest
Definition: dIntersections.h:121
dPolygonSoupBuilder
Definition: dPolygonSoupBuilder.h:47
dTriplex
Definition: dTypes.h:272
dFastAabbInfo
Definition: dIntersections.h:237
dAabbPolygonSoup::dNode
Definition: dAabbPolygonSoup.h:36
dVector
Definition: dVectorArmNeon.h:1104
dAabbPolygonSoup::dNode::dgLeafNodePtr
Definition: dAabbPolygonSoup.h:45
dAabbPolygonSoup
Definition: dAabbPolygonSoup.h:33
dPolygonSoupDatabase
Definition: dPolygonSoupDatabase.h:30