19 template <
typename Real>
35 Real radius, Result&
result);
38 template <
typename Real>
61 Real GetVertexIntersection(Real dx, Real dy, Real dz, Real vx,
62 Real vy, Real vz, Real rsqr);
64 Real GetEdgeIntersection(Real dx, Real dz, Real vx, Real vz,
65 Real vsqr, Real rsqr);
67 int FindFaceRegionIntersection(Real ex, Real ey, Real ez, Real cx,
68 Real cy, Real cz, Real vx, Real vy, Real vz, Real& ix, Real& iy,
69 Real& iz,
bool aboveFace, Real radius, Real& contactTime);
71 int FindJustEdgeIntersection(Real cy, Real ex, Real ey, Real ez, Real dx,
72 Real dz, Real vx, Real vy, Real vz, Real& ix, Real& iy, Real& iz,
73 Real radius, Real& contactTime);
75 int FindEdgeRegionIntersection(Real ex, Real ey, Real ez, Real cx,
76 Real cy, Real cz, Real vx, Real vy, Real vz, Real& ix, Real& iy,
77 Real& iz,
bool aboveEdge, Real radius, Real& contactTime);
79 int FindVertexRegionIntersection(Real ex, Real ey, Real ez, Real cx,
80 Real cy, Real cz, Real vx, Real vy, Real vz, Real& ix, Real& iy,
81 Real& iz, Real radius, Real& contactTime);
85 template <
typename Real>
97 DoQuery(boxExtent, cdiff, sphere.radius, result);
101 template <
typename Real>
105 for (
int i = 0; i < 3; ++i)
107 cdiff[i] = fabs(cdiff[i]);
112 if (cdiff[0] <= boxExtent[0])
114 if (cdiff[1] <= boxExtent[1])
116 if (cdiff[2] <= boxExtent[2])
119 result.intersect =
true;
124 result.intersect = (delta[2] <= radius);
129 if (cdiff[2] <= boxExtent[2])
132 result.intersect = (delta[1] <= radius);
138 Real rsqr = radius * radius;
139 Real lsqr = delta[1] * delta[1] + delta[2] * delta[2];
140 result.intersect = (lsqr <= rsqr);
146 if (cdiff[1] <= boxExtent[1])
148 if (cdiff[2] <= boxExtent[2])
151 result.intersect = (delta[0] <= radius);
157 Real rsqr = radius * radius;
158 Real lsqr = delta[0] * delta[0] + delta[2] * delta[2];
159 result.intersect = (lsqr <= rsqr);
164 if (cdiff[2] <= boxExtent[2])
168 Real rsqr = radius * radius;
169 Real lsqr = delta[0] * delta[0] + delta[1] * delta[1];
170 result.intersect = (lsqr <= rsqr);
176 Real rsqr = radius * radius;
177 Real lsqr =
Dot(delta, delta);
178 result.intersect = (lsqr <= rsqr);
185 template <
typename Real>
200 DoQuery(maxTime, boxCenter, boxExtent, sphere, cdiff, relvel, result);
204 template <
typename Real>
212 if (cdiff[0] < (Real)0)
214 cdiff[0] = -cdiff[0];
215 relvel[0] = -relvel[0];
220 if (cdiff[1] < (Real)0)
222 cdiff[1] = -cdiff[1];
223 relvel[1] = -relvel[1];
228 if (cdiff[2] < (Real)0)
230 cdiff[2] = -cdiff[2];
231 relvel[2] = -relvel[2];
239 if (cdiff[0] <= boxExtent[0])
241 if (cdiff[1] <= boxExtent[1])
243 if (cdiff[2] <= boxExtent[2])
247 result.intersect =
true;
248 result.contactTime = (Real)0;
249 result.contactPoint = sphere.
center;
255 retVal = FindFaceRegionIntersection(
256 boxExtent[0], boxExtent[1], boxExtent[2],
257 cdiff[0], cdiff[1], cdiff[2],
258 relvel[0], relvel[1], relvel[2],
259 ix, iy, iz,
true, sphere.
radius, result.contactTime);
264 if (cdiff[2] <= boxExtent[2])
267 retVal = FindFaceRegionIntersection(
268 boxExtent[0], boxExtent[2], boxExtent[1],
269 cdiff[0], cdiff[2], cdiff[1],
270 relvel[0], relvel[2], relvel[1],
271 ix, iz, iy,
true, sphere.
radius, result.contactTime);
276 retVal = FindEdgeRegionIntersection(
277 boxExtent[1], boxExtent[0], boxExtent[2],
278 cdiff[1], cdiff[0], cdiff[2],
279 relvel[1], relvel[0], relvel[2],
280 iy, ix, iz,
true, sphere.
radius, result.contactTime);
286 if (cdiff[1] <= boxExtent[1])
288 if (cdiff[2] <= boxExtent[2])
291 retVal = FindFaceRegionIntersection(
292 boxExtent[1], boxExtent[2], boxExtent[0],
293 cdiff[1], cdiff[2], cdiff[0],
294 relvel[1], relvel[2], relvel[0],
295 iy, iz, ix,
true, sphere.
radius, result.contactTime);
300 retVal = FindEdgeRegionIntersection(
301 boxExtent[0], boxExtent[1], boxExtent[2],
302 cdiff[0], cdiff[1], cdiff[2],
303 relvel[0], relvel[1], relvel[2],
304 ix, iy, iz,
true, sphere.
radius, result.contactTime);
309 if (cdiff[2] <= boxExtent[2])
312 retVal = FindEdgeRegionIntersection(
313 boxExtent[0], boxExtent[2], boxExtent[1],
314 cdiff[0], cdiff[2], cdiff[1],
315 relvel[0], relvel[2], relvel[1],
316 ix, iz, iy,
true, sphere.
radius, result.contactTime);
321 retVal = FindVertexRegionIntersection(
322 boxExtent[0], boxExtent[1], boxExtent[2],
323 cdiff[0], cdiff[1], cdiff[2],
324 relvel[0], relvel[1], relvel[2],
325 ix, iy, iz, sphere.
radius, result.contactTime);
330 if (retVal == 0 || result.contactTime > maxTime)
332 result.intersect =
false;
337 result.intersect =
true;
338 result.contactPoint = boxCenter +
Vector3<Real>{signX * ix, signY * iy, signZ * iz};
342 template <
typename Real>
343 Real FIQuery<Real, AlignedBox3<Real>,
Sphere3<Real>>::GetVertexIntersection(
344 Real dx, Real dy, Real dz, Real vx, Real vy, Real vz, Real rsqr)
350 Real vsqr = vx * vx + vy * vy + vz * vz;
351 Real dot = dx * vx + dy * vy + dz * vz;
352 Real diff = dx * dx + dy * dy + dz * dz - rsqr;
353 Real inv = ((Real)1) / sqrt(fabs(dot * dot - vsqr * diff));
354 return diff * inv / ((Real)1 - dot * inv);
357 template <
typename Real>
359 Real dx, Real dz, Real vx, Real vz, Real vsqr, Real rsqr)
365 Real dot = vx * dx + vz * dz;
366 Real diff = dx * dx + dz * dz - rsqr;
367 Real inv = ((Real)1) / sqrt(fabs(dot * dot - vsqr * diff));
368 return diff * inv / ((Real)1 - dot * inv);
371 template <
typename Real>
373 FindFaceRegionIntersection(Real ex, Real ey, Real ez, Real cx, Real cy,
374 Real cz, Real vx, Real vy, Real vz, Real& ix, Real& iy, Real& iz,
375 bool aboveFace, Real radius, Real& contactTime)
386 if (cz <= ez + radius && aboveFace)
388 contactTime = (Real)0;
398 Real rsqr = radius * radius;
399 Real vzSqr = vz * vz;
400 Real vsqrX = vzSqr + vx * vx;
401 Real vsqrY = vzSqr + vy * vy;
402 Real dx, dy, dz = cz - ez;
415 crossX = vx * dz - vz * dx;
421 crossX = vz * dx - vx * dz;
428 crossY = vy * dz - vz * dy;
434 crossY = vz * dy - vy * dz;
438 if (crossX > radius * vx * signX)
440 if (crossX * crossX > rsqr * vsqrX)
447 if (crossY > radius * vy * signY)
450 if (crossY * crossY > rsqr * vsqrY)
459 Real crossSqrLen =
Dot(cross, cross);
460 Real relvelSqrLen =
Dot(relVelocity, relVelocity);
461 if (crossSqrLen > rsqr * relvelSqrLen)
467 contactTime = GetVertexIntersection(dx, dy, dz, vx, vy, vz, rsqr);
474 contactTime = GetEdgeIntersection(dx, dz, vx, vz, vsqrX, rsqr);
476 iy = cy + vy * contactTime;
482 if (crossY > radius * vy * signY)
485 if (crossY * crossY > rsqr * vsqrY)
491 contactTime = GetEdgeIntersection(dy, dz, vy, vz, vsqrY, rsqr);
492 ix = cx + vx * contactTime;
498 contactTime = (-dz + radius) / vz;
499 ix = contactTime * vx + cx;
500 iy = contactTime * vy + cy;
509 template <
typename Real>
511 FindJustEdgeIntersection(Real cy, Real ex, Real ey, Real ez, Real dx, Real dz,
512 Real vx, Real vy, Real vz, Real& ix, Real& iy, Real& iz, Real radius,
520 Real rsqr = radius * radius;
521 Real dy, crossZ, crossX;
533 crossZ = dx * vy - dy * vx;
534 crossX = dz * vy - dy * vz;
540 crossZ = dy * vx - dx * vy;
541 crossX = dy * vz - dz * vy;
545 if (crossZ >= (Real)0 && crossX >= (Real)0
546 && crossX * crossX + crossZ * crossZ > rsqr * vy * vy)
552 Real crossSqrLen =
Dot(cross, cross);
553 Real relvelSqrLen =
Dot(relVelocity, relVelocity);
554 if (crossSqrLen > rsqr * relvelSqrLen)
561 contactTime = GetVertexIntersection(dx, dy, dz, vx, vy, vz, rsqr);
569 Real vsqrX = vz * vz + vx * vx;
570 contactTime = GetEdgeIntersection(dx, dz, vx, vz, vsqrX, rsqr);
572 iy = cy + contactTime * vy;
578 template <
typename Real>
580 FindEdgeRegionIntersection(Real ex, Real ey, Real ez, Real cx, Real cy,
581 Real cz, Real vx, Real vy, Real vz, Real& ix, Real& iy, Real& iz,
582 bool aboveEdge, Real radius, Real& contactTime)
593 Real rsqr = radius * radius;
597 Real diff = dx * dx + dz * dz - rsqr;
601 contactTime = (Real)0;
606 Real dot = vx * dx + vz * dz;
615 Real dotPerp = vz * dx - vx * dz;
616 if (dotPerp >= (Real)0)
633 if (dotPerp <= -radius * vx)
635 return FindJustEdgeIntersection(cy, ez, ey, ex, dz, dx, vz, vy,
636 vx, iz, iy, ix, radius, contactTime);
640 return FindFaceRegionIntersection(ex, ey, ez, cx, cy, cz, vx, vy,
641 vz, ix, iy, iz,
false, radius, contactTime);
654 if (dotPerp >= radius * vz)
657 return FindJustEdgeIntersection(cy, ex, ey, ez, dx, dz, vx, vy,
658 vz, ix, iy, iz, radius, contactTime);
662 return FindFaceRegionIntersection(ez, ey, ex, cz, cy, cx, vz, vy,
663 vx, iz, iy, ix,
false, radius, contactTime);
667 template <
typename Real>
669 FindVertexRegionIntersection(Real ex, Real ey, Real ez, Real cx, Real cy,
670 Real cz, Real vx, Real vy, Real vz, Real& ix, Real& iy, Real& iz,
671 Real radius, Real& contactTime)
678 Real rsqr = radius * radius;
679 Real diff = dx * dx + dy * dy + dz * dz - rsqr;
683 contactTime = (Real)0;
687 if (vx * dx + vy * dy + vz * dz >= (Real)0)
717 Real crossX = vy * dz - vz * dy;
718 Real crossY = vx * dz - vz * dx;
719 Real crossZ = vy * dx - vx * dy;
720 Real crX2 = crossX * crossX;
721 Real crY2 = crossY * crossY;
722 Real crZ2 = crossZ * crossZ;
728 if ((crossY < (Real)0 && crossZ >= (Real)0 && crY2 + crZ2 <= rsqr * vx2)
729 || (crossZ < (Real)0 && crossX < (Real)0 && crX2 + crZ2 <= rsqr * vy2)
730 || (crossY >= (Real)0 && crossX >= (Real)0 && crX2 + crY2 <= rsqr * vz2))
733 contactTime = GetVertexIntersection(dx, dy, dz, vx, vy, vz, rsqr);
734 ix = contactTime * vx + cx;
735 iy = contactTime * vy + cy;
736 iz = contactTime * vz + cz;
739 else if (crossY < (Real)0 && crossZ >= (Real)0)
742 return FindEdgeRegionIntersection(ey, ex, ez, cy, cx, cz, vy, vx,
743 vz, iy, ix, iz,
false, radius, contactTime);
745 else if (crossZ < (Real)0 && crossX < (Real)0)
748 return FindEdgeRegionIntersection(ex, ey, ez, cx, cy, cz, vx, vy,
749 vz, ix, iy, iz,
false, radius, contactTime);
754 return FindEdgeRegionIntersection(ex, ez, ey, cx, cz, cy, vx, vz,
755 vy, ix, iz, iy,
false, radius, contactTime);
DualQuaternion< Real > Dot(DualQuaternion< Real > const &d0, DualQuaternion< Real > const &d1)
DualQuaternion< Real > Cross(DualQuaternion< Real > const &d0, DualQuaternion< Real > const &d1)
Vector3< Real > contactPoint
Result operator()(Type0 const &primitive0, Type1 const &primitive1)