22 #ifndef __D_INTERSECTION_H__
23 #define __D_INTERSECTION_H__
25 #include "dCoreStdafx.h"
38 typedef dIntersectStatus (*dAaabbIntersectCallback) (
void*
const context,
39 const dFloat32*
const polygon, dInt32 strideInBytes,
40 const dInt32*
const indexArray, dInt32 indexCount, dFloat32 hitDistance);
42 typedef dFloat32 (*dRayIntersectCallback) (
void*
const context,
43 const dFloat32*
const polygon, dInt32 strideInBytes,
44 const dInt32*
const indexArray, dInt32 indexCount);
53 D_CORE_API dFloat32 dRayCastSphere (
const dVector& p0,
const dVector& p1,
const dVector& origin, dFloat32 radius);
60 dInt32 mask = val.GetSignMask() & 0x07;
61 return (mask == 0x07);
66 dVector val(dVector::m_negOne & ((p0 >= q0) & (p1 <= q1)));
67 dInt32 mask = val.GetSignMask() & 0x07;
68 return (mask == 0x07);
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);
77 D_INLINE
void dMovingAABB (
dVector& p0,
dVector& p1,
const dVector& veloc,
const dVector& omega, dFloat32 timestep, dFloat32 maxRadius, dFloat32 minRadius)
79 dVector linearStep (veloc.Scale (timestep));
82 dAssert (omega.m_w == dFloat32 (0.0f));
83 dFloat32 maxAngle = dMin (dSqrt (omega.DotProduct(omega).GetScalar() * timestep * timestep), dFloat32 (45.0f * dDegreeToRad));
85 dFloat32 angularTravel = (maxRadius - minRadius) * maxAngle;
86 dVector angularStep (angularTravel, angularTravel, angularTravel, dFloat32 (0.0f));
92 p0 = r0.GetMin (q0) & dVector::m_triplexMask;
93 p1 = r1.GetMax (q1) & dVector::m_triplexMask;
96 D_INLINE dFloat32 dBoxPenetration (
const dVector& minBox,
const dVector& maxBox)
98 dAssert(maxBox.m_x >= minBox.m_x);
99 dAssert(maxBox.m_y >= minBox.m_y);
100 dAssert(maxBox.m_z >= minBox.m_z);
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();
109 D_INLINE dFloat32 dBoxDistanceToOrigin2 (
const dVector& minBox,
const dVector& maxBox)
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();
119 D_MSV_NEWTON_ALIGN_32
126 ,m_diff((l1 - l0) & dVector::m_triplexMask)
127 ,m_minT(dFloat32(0.0f))
128 ,m_maxT(dFloat32(1.0f))
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));
134 dAssert (m_diff.DotProduct(m_diff).GetScalar() > dFloat32 (0.0f));
135 m_isParallel = (m_diff.Abs() <
dVector(1.0e-8f));
137 m_dpInv = m_diff.Select (
dVector(dFloat32(1.0e-20f)), m_isParallel).Reciproc() & dVector::m_triplexMask;
138 m_unitDir = m_diff.Normalize();
141 D_CORE_API dFloat32 PolygonIntersect(
const dVector& normal, dFloat32 maxT,
const dFloat32*
const polygon, dInt32 strideInBytes,
const dInt32*
const indexArray, dInt32 indexCount)
const;
143 D_INLINE dInt32 BoxTest(
const dVector& minBox,
const dVector& maxBox)
const
146 dVector test(((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
147 if (test.GetSignMask() & 0x07) {
151 dVector tt0(m_dpInv * (minBox - m_p0));
152 dVector tt1(m_dpInv * (maxBox - m_p0));
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);
164 dFloat32 tmin = 0.0f;
165 dFloat32 tmax = 1.0f;
167 for (dInt32 i = 0; i < 3; i++)
171 if (m_p0[i] <= minBox[i] || m_p0[i] >= maxBox[i])
178 dFloat32 t1 = (minBox[i] - m_p0[i]) * m_dpInv[i];
179 dFloat32 t2 = (maxBox[i] - m_p0[i]) * m_dpInv[i];
203 D_INLINE dFloat32 BoxIntersect(
const dVector& minBox,
const dVector& maxBox)
const
205 dVector test(((m_p0 <= minBox) | (m_p0 >= maxBox)) & m_isParallel);
206 if (test.GetSignMask() & 0x07)
208 return dFloat32(1.2f);
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());
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();
233 } D_GCC_NEWTON_ALIGN_32 ;
235 D_MSV_NEWTON_ALIGN_32
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))
251 ,m_separationDistance(dFloat32(1.0e10f))
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;
261 ,m_absDir(dGetIdentityMatrix())
264 ,m_size(dVector::m_half * (p1 - p0))
265 ,m_separationDistance(dFloat32(1.0e10f))
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));
272 D_INLINE
void SetTransposeAbsMatrix (
const dMatrix& matrix)
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();
281 D_INLINE dFloat32 PolygonBoxRayDistance (
const dVector& faceNormal, dInt32 indexCount,
const dInt32*
const indexArray, dInt32 stride,
const dFloat32*
const vertexArray,
const dFastRayTest& ray)
const
285 MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
286 dFloat32 dist0 = ray.BoxIntersect(minBox, maxBox);
287 if (dist0 < dFloat32 (1.0f))
289 dMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
291 MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
292 dVector veloc (faceMatrix.RotateVector(ray.m_diff) & dVector::m_triplexMask);
294 dFloat32 dist1 = localRay.BoxIntersect(minBox, maxBox);
295 dist0 = dMax (dist1, dist0);
300 D_INLINE dFloat32 PolygonBoxDistance (
const dVector& faceNormal, dInt32 indexCount,
const dInt32*
const indexArray, dInt32 stride,
const dFloat32*
const vertexArray)
const
304 MakeBox1 (indexCount, indexArray, stride, vertexArray, minBox, maxBox);
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))
312 dMatrix faceMatrix (MakeFaceMatrix (faceNormal, indexCount, indexArray, stride, vertexArray));
313 MakeBox2 (faceMatrix, indexCount, indexArray, stride, vertexArray, minBox, maxBox);
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))
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();
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();
339 D_INLINE
void MakeBox1 (dInt32 indexCount,
const dInt32*
const indexArray, dInt32 stride,
const dFloat32*
const vertexArray,
dVector& minBox,
dVector& maxBox)
const
341 dVector faceBoxP0 (&vertexArray[indexArray[0] * stride]);
342 faceBoxP0 = faceBoxP0 & dVector::m_triplexMask;
344 for (dInt32 i = 1; i < indexCount; i ++)
346 dVector p (&vertexArray[indexArray[i] * stride]);
347 p = p & dVector::m_triplexMask;
348 faceBoxP0 = faceBoxP0.GetMin(p);
349 faceBoxP1 = faceBoxP1.GetMax(p);
352 minBox = faceBoxP0 - m_p1;
353 maxBox = faceBoxP1 - m_p0;
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
358 dVector faceBoxP0 (faceMatrix.TransformVector (
dVector (&vertexArray[indexArray[0] * stride]) & dVector::m_triplexMask));
360 for (dInt32 i = 1; i < indexCount; i ++)
362 dVector p (faceMatrix.TransformVector (
dVector (&vertexArray[indexArray[i] * stride]) & dVector::m_triplexMask));
363 faceBoxP0 = faceBoxP0.GetMin(p);
364 faceBoxP1 = faceBoxP1.GetMax(p);
366 faceBoxP0 = faceBoxP0 & dVector::m_triplexMask;
367 faceBoxP1 = faceBoxP1 & dVector::m_triplexMask;
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);
374 minBox = faceBoxP0 - boxP1;
375 maxBox = faceBoxP1 - boxP0;
378 D_INLINE
dMatrix MakeFaceMatrix (
const dVector& faceNormal, dInt32 indexCount,
const dInt32*
const indexArray, dInt32 stride,
const dFloat32*
const vertexArray)
const
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;
386 dVector pin1 (&vertexArray[indexArray[1] * stride]);
387 pin1 = pin1 & dVector::m_triplexMask;
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();
402 mutable dVector m_separationDistance;
407 } D_GCC_NEWTON_ALIGN_32 ;