Newton Dynamics  4.00
dIntersections.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 __D_INTERSECTION_H__
23 #define __D_INTERSECTION_H__
24 
25 #include "dCoreStdafx.h"
26 #include "dDebug.h"
27 #include "dVector.h"
28 #include "dMatrix.h"
29 
30 class dPlane;
31 
32 enum dIntersectStatus
33 {
34  t_StopSearh,
35  t_ContinueSearh
36 };
37 
38 typedef dIntersectStatus (*dAaabbIntersectCallback) (void* const context,
39  const dFloat32* const polygon, dInt32 strideInBytes,
40  const dInt32* const indexArray, dInt32 indexCount, dFloat32 hitDistance);
41 
42 typedef dFloat32 (*dRayIntersectCallback) (void* const context,
43  const dFloat32* const polygon, dInt32 strideInBytes,
44  const dInt32* const indexArray, dInt32 indexCount);
45 
46 D_CORE_API dBigVector dPointToRayDistance (const dBigVector& point, const dBigVector& ray_p0, const dBigVector& ray_p1);
47 D_CORE_API dBigVector dPointToTriangleDistance (const dBigVector& point, const dBigVector& p0, const dBigVector& p1, const dBigVector& p2);
48 D_CORE_API dBigVector dPointToTetrahedrumDistance (const dBigVector& point, const dBigVector& p0, const dBigVector& p1, const dBigVector& p2, const dBigVector& p3);
49 
50 D_CORE_API bool dRayBoxClip (dVector& ray_p0, dVector& ray_p1, const dVector& boxP0, const dVector& boxP1);
51 D_CORE_API void dRayToRayDistance (const dVector& ray_p0, const dVector& ray_p1, const dVector& ray_q0, const dVector& ray_q1, dVector& p0Out, dVector& p1Out);
52 D_CORE_API dFloat32 dRayCastBox (const dVector& p0, const dVector& p1, const dVector& boxP0, const dVector& boxP1, dVector& normalOut);
53 D_CORE_API dFloat32 dRayCastSphere (const dVector& p0, const dVector& p1, const dVector& origin, dFloat32 radius);
54 
55 D_INLINE dInt32 dOverlapTest (const dVector& p0, const dVector& p1, const dVector& q0, const dVector& q1)
56 {
57  dVector r0(p0 - q1);
58  dVector r1(p1 - q0);
59  dVector val(r0 * r1);
60  dInt32 mask = val.GetSignMask() & 0x07;
61  return (mask == 0x07);
62 }
63 
64 D_INLINE dInt32 dBoxInclusionTest (const dVector& p0, const dVector& p1, const dVector& q0, const dVector& q1)
65 {
66  dVector val(dVector::m_negOne & ((p0 >= q0) & (p1 <= q1)));
67  dInt32 mask = val.GetSignMask() & 0x07;
68  return (mask == 0x07);
69 }
70 
71 D_INLINE dInt32 dCompareBox (const dVector& p0, const dVector& p1, const dVector& q0, const dVector& q1)
72 {
73  dAssert(0);
74  return (p0.m_x != q0.m_x) || (p0.m_y != q0.m_y) || (p0.m_z != q0.m_z) || (p1.m_x != q1.m_x) || (p1.m_y != q1.m_y) || (p1.m_z != q1.m_z);
75 }
76 
77 D_INLINE void dMovingAABB (dVector& p0, dVector& p1, const dVector& veloc, const dVector& omega, dFloat32 timestep, dFloat32 maxRadius, dFloat32 minRadius)
78 {
79  dVector linearStep (veloc.Scale (timestep));
80 
81  // estimate the maximum effect of the angular velocity and enlarge that box by that value (use 45 degrees as max angle not 90)
82  dAssert (omega.m_w == dFloat32 (0.0f));
83  dFloat32 maxAngle = dMin (dSqrt (omega.DotProduct(omega).GetScalar() * timestep * timestep), dFloat32 (45.0f * dDegreeToRad));
84 
85  dFloat32 angularTravel = (maxRadius - minRadius) * maxAngle;
86  dVector angularStep (angularTravel, angularTravel, angularTravel, dFloat32 (0.0f));
87 
88  dVector r0 (p0 - angularStep);
89  dVector r1 (p1 + angularStep);
90  dVector q0 (r0 + linearStep);
91  dVector q1 (r1 + linearStep);
92  p0 = r0.GetMin (q0) & dVector::m_triplexMask;
93  p1 = r1.GetMax (q1) & dVector::m_triplexMask;
94 }
95 
96 D_INLINE dFloat32 dBoxPenetration (const dVector& minBox, const dVector& maxBox)
97 {
98  dAssert(maxBox.m_x >= minBox.m_x);
99  dAssert(maxBox.m_y >= minBox.m_y);
100  dAssert(maxBox.m_z >= minBox.m_z);
101 
102  dVector mask ((minBox * maxBox) < dVector::m_zero);
103  dVector dist (maxBox.GetMin (minBox.Abs()) & mask);
104  dist = dist.GetMin(dist.ShiftTripleRight());
105  dist = dist.GetMin(dist.ShiftTripleRight());
106  return dist.GetScalar();
107 }
108 
109 D_INLINE dFloat32 dBoxDistanceToOrigin2 (const dVector& minBox, const dVector& maxBox)
110 {
111  dAssert(maxBox.m_x >= minBox.m_x);
112  dAssert(maxBox.m_y >= minBox.m_y);
113  dAssert(maxBox.m_z >= minBox.m_z);
114  dVector mask ((minBox * maxBox) > dVector::m_zero);
115  dVector dist (maxBox.Abs().GetMin (minBox.Abs()) & mask);
116  return dist.DotProduct(dist).GetScalar();
117 }
118 
119 D_MSV_NEWTON_ALIGN_32
121 {
122  public:
123  D_INLINE dFastRayTest(const dVector& l0, const dVector& l1)
124  :m_p0(l0)
125  ,m_p1(l1)
126  ,m_diff((l1 - l0) & dVector::m_triplexMask)
127  ,m_minT(dFloat32(0.0f))
128  ,m_maxT(dFloat32(1.0f))
129  {
130  dAssert(m_p0.m_w == dFloat32(0.0f));
131  dAssert(m_p1.m_w == dFloat32(0.0f));
132  dAssert(m_diff.m_w == dFloat32(0.0f));
133 
134  dAssert (m_diff.DotProduct(m_diff).GetScalar() > dFloat32 (0.0f));
135  m_isParallel = (m_diff.Abs() < dVector(1.0e-8f));
136  //m_dpInv = (((dVector(dFloat32(1.0e-20)) & m_isParallel) | m_diff.AndNot(m_isParallel)).Reciproc()) & dVector::m_triplexMask;
137  m_dpInv = m_diff.Select (dVector(dFloat32(1.0e-20f)), m_isParallel).Reciproc() & dVector::m_triplexMask;
138  m_unitDir = m_diff.Normalize();
139  }
140 
141  D_CORE_API dFloat32 PolygonIntersect(const dVector& normal, dFloat32 maxT, const dFloat32* const polygon, dInt32 strideInBytes, const dInt32* const indexArray, dInt32 indexCount) const;
142 
143  D_INLINE dInt32 BoxTest(const dVector& minBox, const dVector& maxBox) const
144  {
145 #if 1
146  dVector test(((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
147  if (test.GetSignMask() & 0x07) {
148  return 0;
149  }
150 
151  dVector tt0(m_dpInv * (minBox - m_p0));
152  dVector tt1(m_dpInv * (maxBox - m_p0));
153 
154  dVector t0(m_minT.GetMax(tt0.GetMin(tt1)));
155  dVector t1(m_maxT.GetMin(tt0.GetMax(tt1)));
156  t0 = t0.GetMax(t0.ShiftTripleRight());
157  t1 = t1.GetMin(t1.ShiftTripleRight());
158  t0 = t0.GetMax(t0.ShiftTripleRight());
159  t1 = t1.GetMin(t1.ShiftTripleRight());
160  return ((t0 < t1).GetSignMask() & 1);
161 
162 #else
163 
164  dFloat32 tmin = 0.0f;
165  dFloat32 tmax = 1.0f;
166 
167  for (dInt32 i = 0; i < 3; i++)
168  {
169  if (m_isParallel[i])
170  {
171  if (m_p0[i] <= minBox[i] || m_p0[i] >= maxBox[i])
172  {
173  return 0;
174  }
175  }
176  else
177  {
178  dFloat32 t1 = (minBox[i] - m_p0[i]) * m_dpInv[i];
179  dFloat32 t2 = (maxBox[i] - m_p0[i]) * m_dpInv[i];
180 
181  if (t1 > t2)
182  {
183  dSwap(t1, t2);
184  }
185  if (t1 > tmin)
186  {
187  tmin = t1;
188  }
189  if (t2 < tmax)
190  {
191  tmax = t2;
192  }
193  if (tmin > tmax)
194  {
195  return 0;
196  }
197  }
198  }
199  return 0x1;
200 #endif
201  }
202 
203  D_INLINE dFloat32 BoxIntersect(const dVector& minBox, const dVector& maxBox) const
204  {
205  dVector test(((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
206  if (test.GetSignMask() & 0x07)
207  {
208  return dFloat32(1.2f);
209  }
210  dVector tt0(m_dpInv * (minBox - m_p0));
211  dVector tt1(m_dpInv * (maxBox - m_p0));
212  dVector t0(m_minT.GetMax(tt0.GetMin(tt1)));
213  dVector t1(m_maxT.GetMin(tt0.GetMax(tt1)));
214  t0 = t0.GetMax(t0.ShiftTripleRight());
215  t1 = t1.GetMin(t1.ShiftTripleRight());
216  t0 = t0.GetMax(t0.ShiftTripleRight());
217  t1 = t1.GetMin(t1.ShiftTripleRight());
218  dVector mask(t0 < t1);
219  dVector maxDist(dFloat32(1.2f));
220  t0 = maxDist.Select(t0, mask);
221  dAssert((mask.GetSignMask() & 1) == (t0.m_x < dFloat32(1.0f)));
222  return t0.GetScalar();
223  }
224 
225  const dVector m_p0;
226  const dVector m_p1;
227  const dVector m_diff;
228  dVector m_dpInv;
229  dVector m_minT;
230  dVector m_maxT;
231  dVector m_unitDir;
232  dVector m_isParallel;
233 } D_GCC_NEWTON_ALIGN_32 ;
234 
235 D_MSV_NEWTON_ALIGN_32
236 class dFastAabbInfo : public dMatrix
237 {
238  public:
239  D_INLINE dFastAabbInfo()
240  :dMatrix(dGetIdentityMatrix())
241  ,m_absDir(dGetIdentityMatrix())
242  ,m_p0(dVector::m_zero)
243  ,m_p1(dVector::m_zero)
244  ,m_size(dVector::m_zero)
245  ,m_separationDistance(dFloat32(1.0e10f))
246  {
247  }
248 
249  D_INLINE dFastAabbInfo(const dMatrix& matrix, const dVector& size)
250  :dMatrix(matrix)
251  ,m_separationDistance(dFloat32(1.0e10f))
252  {
253  SetTransposeAbsMatrix (matrix);
254  m_size = dVector(matrix[0].Abs().Scale(size.m_x) + matrix[1].Abs().Scale(size.m_y) + matrix[2].Abs().Scale(size.m_z));
255  m_p0 = (matrix[3] - m_size) & dVector::m_triplexMask;
256  m_p1 = (matrix[3] + m_size) & dVector::m_triplexMask;
257  }
258 
259  D_INLINE dFastAabbInfo(const dVector& p0, const dVector& p1)
260  :dMatrix(dGetIdentityMatrix())
261  ,m_absDir(dGetIdentityMatrix())
262  ,m_p0(p0)
263  ,m_p1(p1)
264  ,m_size(dVector::m_half * (p1 - p0))
265  ,m_separationDistance(dFloat32(1.0e10f))
266  {
267  m_posit = (dVector::m_half * (p1 + p0)) | dVector::m_wOne;
268  dAssert(m_size.m_w == dFloat32(0.0f));
269  dAssert(m_posit.m_w == dFloat32(1.0f));
270  }
271 
272  D_INLINE void SetTransposeAbsMatrix (const dMatrix& matrix)
273  {
274  m_absDir = matrix.Transpose();
275  m_absDir[0] = m_absDir[0].Abs();
276  m_absDir[1] = m_absDir[1].Abs();
277  m_absDir[2] = m_absDir[2].Abs();
278  //m_absDir[3] = dVector::m_wOne;
279  }
280 
281  D_INLINE dFloat32 PolygonBoxRayDistance (const dVector& faceNormal, dInt32 indexCount, const dInt32* const indexArray, dInt32 stride, const dFloat32* const vertexArray, const dFastRayTest& ray) const
282  {
283  dVector minBox;
284  dVector maxBox;
285  MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
286  dFloat32 dist0 = ray.BoxIntersect(minBox, maxBox);
287  if (dist0 < dFloat32 (1.0f))
288  {
289  dMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
290 
291  MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
292  dVector veloc (faceMatrix.RotateVector(ray.m_diff) & dVector::m_triplexMask);
293  dFastRayTest localRay (dVector (dFloat32 (0.0f)), veloc);
294  dFloat32 dist1 = localRay.BoxIntersect(minBox, maxBox);
295  dist0 = dMax (dist1, dist0);
296  }
297  return dist0;
298  }
299 
300  D_INLINE dFloat32 PolygonBoxDistance (const dVector& faceNormal, dInt32 indexCount, const dInt32* const indexArray, dInt32 stride, const dFloat32* const vertexArray) const
301  {
302  dVector minBox;
303  dVector maxBox;
304  MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
305  dVector mask(minBox * maxBox < dVector(dFloat32(0.0f)));
306  dVector dist(maxBox.GetMin(minBox.Abs()) & mask);
307  dist = dist.GetMin(dist.ShiftTripleRight());
308  dist = dist.GetMin(dist.ShiftTripleRight());
309  dFloat32 dist0 = dist.GetScalar();
310  if (dist0 > dFloat32 (0.0f))
311  {
312  dMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
313  MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
314  dVector mask2(minBox * maxBox < dVector(dFloat32(0.0f)));
315  dVector dist2(maxBox.GetMin(minBox.Abs()) & mask2);
316  dist2 = dist2.GetMin(dist2.ShiftTripleRight());
317  dist2 = dist2.GetMin(dist2.ShiftTripleRight());
318  dFloat32 dist1 = dist2.GetScalar();
319  dist0 = (dist1 > dFloat32 (0.0f)) ? dMax (dist0, dist1) : dFloat32 (0.0f);
320  if (dist0 <= dFloat32(0.0f))
321  {
322  dVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask2));
323  dist2 = p1p0.DotProduct(p1p0);
324  dist2 = dist2.Sqrt() * dVector::m_negOne;
325  dist0 = dist2.GetScalar();
326  }
327  }
328  else
329  {
330  dVector p1p0((minBox.Abs()).GetMin(maxBox.Abs()).AndNot(mask));
331  dist = p1p0.DotProduct(p1p0);
332  dist = dist.Sqrt() * dVector::m_negOne;
333  dist0 = dist.GetScalar();
334  }
335  return dist0;
336  }
337 
338  private:
339  D_INLINE void MakeBox1 (dInt32 indexCount, const dInt32* const indexArray, dInt32 stride, const dFloat32* const vertexArray, dVector& minBox, dVector& maxBox) const
340  {
341  dVector faceBoxP0 (&vertexArray[indexArray[0] * stride]);
342  faceBoxP0 = faceBoxP0 & dVector::m_triplexMask;
343  dVector faceBoxP1 (faceBoxP0);
344  for (dInt32 i = 1; i < indexCount; i ++)
345  {
346  dVector p (&vertexArray[indexArray[i] * stride]);
347  p = p & dVector::m_triplexMask;
348  faceBoxP0 = faceBoxP0.GetMin(p);
349  faceBoxP1 = faceBoxP1.GetMax(p);
350  }
351 
352  minBox = faceBoxP0 - m_p1;
353  maxBox = faceBoxP1 - m_p0;
354  }
355 
356  D_INLINE void MakeBox2 (const dMatrix& faceMatrix, dInt32 indexCount, const dInt32* const indexArray, dInt32 stride, const dFloat32* const vertexArray, dVector& minBox, dVector& maxBox) const
357  {
358  dVector faceBoxP0 (faceMatrix.TransformVector (dVector (&vertexArray[indexArray[0] * stride]) & dVector::m_triplexMask));
359  dVector faceBoxP1 (faceBoxP0);
360  for (dInt32 i = 1; i < indexCount; i ++)
361  {
362  dVector p (faceMatrix.TransformVector (dVector (&vertexArray[indexArray[i] * stride]) & dVector::m_triplexMask));
363  faceBoxP0 = faceBoxP0.GetMin(p);
364  faceBoxP1 = faceBoxP1.GetMax(p);
365  }
366  faceBoxP0 = faceBoxP0 & dVector::m_triplexMask;
367  faceBoxP1 = faceBoxP1 & dVector::m_triplexMask;
368 
369  dMatrix matrix = *this * faceMatrix;
370  dVector size (matrix[0].Abs().Scale(m_size.m_x) + matrix[1].Abs().Scale(m_size.m_y) + matrix[2].Abs().Scale(m_size.m_z));
371  dVector boxP0 ((matrix.m_posit - size) & dVector::m_triplexMask);
372  dVector boxP1 ((matrix.m_posit + size) & dVector::m_triplexMask);
373 
374  minBox = faceBoxP0 - boxP1;
375  maxBox = faceBoxP1 - boxP0;
376  }
377 
378  D_INLINE dMatrix MakeFaceMatrix (const dVector& faceNormal, dInt32 indexCount, const dInt32* const indexArray, dInt32 stride, const dFloat32* const vertexArray) const
379  {
380  dMatrix faceMatrix;
381  dVector origin (&vertexArray[indexArray[0] * stride]);
382  dVector pin (&vertexArray[indexArray[0] * stride]);
383  pin = pin & dVector::m_triplexMask;
384  origin = origin & dVector::m_triplexMask;
385 
386  dVector pin1 (&vertexArray[indexArray[1] * stride]);
387  pin1 = pin1 & dVector::m_triplexMask;
388 
389  faceMatrix[0] = faceNormal;
390  faceMatrix[1] = pin1 - origin;
391  faceMatrix[1] = faceMatrix[1].Normalize();
392  faceMatrix[2] = faceMatrix[0].CrossProduct(faceMatrix[1]);
393  faceMatrix[3] = origin | dVector::m_wOne;
394  return faceMatrix.Inverse();
395  }
396 
397  protected:
398  dMatrix m_absDir;
399  dVector m_p0;
400  dVector m_p1;
401  dVector m_size;
402  mutable dVector m_separationDistance;
403 
404  friend class dAabbPolygonSoup;
405 // friend class dCollisionUserMesh;
406 // friend class dCollisionHeightField;
407 } D_GCC_NEWTON_ALIGN_32 ;
408 
409 #endif
410 
dFastRayTest
Definition: dIntersections.h:121
dFastAabbInfo
Definition: dIntersections.h:237
dBigVector
Definition: dVectorArmNeon.h:1521
dVector
Definition: dVectorArmNeon.h:1104
dMatrix
Definition: dMatrix.h:39
dPlane
Definition: dPlane.h:35
dAabbPolygonSoup
Definition: dAabbPolygonSoup.h:33