GteIntrAlignedBox3Sphere3.h
Go to the documentation of this file.
1 // David Eberly, Geometric Tools, Redmond WA 98052
2 // Copyright (c) 1998-2017
3 // Distributed under the Boost Software License, Version 1.0.
4 // http://www.boost.org/LICENSE_1_0.txt
5 // http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
6 // File Version: 3.0.0 (2016/06/19)
7 
8 #pragma once
9 
10 #include <Mathematics/GteVector3.h>
13 #include <Mathematics/GteFIQuery.h>
14 #include <Mathematics/GteTIQuery.h>
15 
16 namespace gte
17 {
18 
19 template <typename Real>
20 class TIQuery<Real, AlignedBox3<Real>, Sphere3<Real>>
21 {
22 public:
23  // The intersection query considers the box and sphere to be solids.
24  // For example, if the sphere is strictly inside the box (does not touch
25  // the box faces), the objects intersect.
26  struct Result
27  {
28  bool intersect;
29  };
30 
31  Result operator()(AlignedBox3<Real> const& box, Sphere3<Real> const& sphere);
32 
33 protected:
34  void DoQuery(Vector3<Real> const& boxExtent, Vector3<Real>& cdiff,
35  Real radius, Result& result);
36 };
37 
38 template <typename Real>
39 class FIQuery<Real, AlignedBox3<Real>, Sphere3<Real>>
40 {
41 public:
42  // Currently, only a dynamic query is supported. The static query must
43  // compute the intersection set of (solid) box and sphere.
44  struct Result
45  {
46  bool intersect;
49  };
50 
51  Result operator()(Real maxTime, AlignedBox3<Real> const& box,
52  Vector3<Real> const& boxVelocity, Sphere3<Real> const& sphere,
53  Vector3<Real> const& sphereVelocity);
54 
55 protected:
56  void DoQuery(Real maxTime, Vector3<Real> const& boxCenter, Vector3<Real> const& boxExtent,
57  Sphere3<Real> const& sphere, Vector3<Real>& cdiff, Vector3<Real>& relvel,
58  Result& result);
59 
60  // Support for dynamic query.
61  Real GetVertexIntersection(Real dx, Real dy, Real dz, Real vx,
62  Real vy, Real vz, Real rsqr);
63 
64  Real GetEdgeIntersection(Real dx, Real dz, Real vx, Real vz,
65  Real vsqr, Real rsqr);
66 
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);
70 
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);
74 
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);
78 
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);
82 };
83 
84 
85 template <typename Real>
88  AlignedBox3<Real> const& box, Sphere3<Real> const& sphere)
89 {
90  // Test for intersection in the coordinate system of the box by
91  // transforming the sphere into that coordinate system.
92  Vector3<Real> boxCenter = (box.max + box.min) * (Real)0.5;
93  Vector3<Real> boxExtent = (box.max - box.min) * (Real)0.5;
94  Vector3<Real> cdiff = sphere.center - boxCenter;
95 
96  Result result;
97  DoQuery(boxExtent, cdiff, sphere.radius, result);
98  return result;
99 }
100 
101 template <typename Real>
103  Vector3<Real> const& boxExtent, Vector3<Real>& cdiff, Real radius, Result& result)
104 {
105  for (int i = 0; i < 3; ++i)
106  {
107  cdiff[i] = fabs(cdiff[i]);
108  }
109 
110  Vector3<Real> delta = cdiff - boxExtent;
111 
112  if (cdiff[0] <= boxExtent[0])
113  {
114  if (cdiff[1] <= boxExtent[1])
115  {
116  if (cdiff[2] <= boxExtent[2])
117  {
118  // Sphere center inside box.
119  result.intersect = true;
120  }
121  else
122  {
123  // Potential sphere-face intersection with face z.
124  result.intersect = (delta[2] <= radius);
125  }
126  }
127  else
128  {
129  if (cdiff[2] <= boxExtent[2])
130  {
131  // Potential sphere-face intersection with face y.
132  result.intersect = (delta[1] <= radius);
133  }
134  else
135  {
136  // Potential sphere-edge intersection with edge formed
137  // by faces y and z.
138  Real rsqr = radius * radius;
139  Real lsqr = delta[1] * delta[1] + delta[2] * delta[2];
140  result.intersect = (lsqr <= rsqr);
141  }
142  }
143  }
144  else
145  {
146  if (cdiff[1] <= boxExtent[1])
147  {
148  if (cdiff[2] <= boxExtent[2])
149  {
150  // Potential sphere-face intersection with face x.
151  result.intersect = (delta[0] <= radius);
152  }
153  else
154  {
155  // Potential sphere-edge intersection with edge formed
156  // by faces x and z.
157  Real rsqr = radius * radius;
158  Real lsqr = delta[0] * delta[0] + delta[2] * delta[2];
159  result.intersect = (lsqr <= rsqr);
160  }
161  }
162  else
163  {
164  if (cdiff[2] <= boxExtent[2])
165  {
166  // Potential sphere-edge intersection with edge formed
167  // by faces x and y.
168  Real rsqr = radius * radius;
169  Real lsqr = delta[0] * delta[0] + delta[1] * delta[1];
170  result.intersect = (lsqr <= rsqr);
171  }
172  else
173  {
174  // Potential sphere-vertex intersection at corner formed
175  // by faces x,y,z.
176  Real rsqr = radius * radius;
177  Real lsqr = Dot(delta, delta);
178  result.intersect = (lsqr <= rsqr);
179  }
180  }
181  }
182 }
183 
184 
185 template <typename Real>
188  AlignedBox3<Real> const& box, Vector3<Real> const& boxVelocity,
189  Sphere3<Real> const& sphere, Vector3<Real> const& sphereVelocity)
190 {
191  // Find intersections relative to the coordinate system of the box.
192  // The sphere is transformed to the box coordinates and the velocity of
193  // the sphere is relative to the box.
194  Vector3<Real> boxCenter = (box.max + box.min) * (Real)0.5;
195  Vector3<Real> boxExtent = (box.max - box.min) * (Real)0.5;
196  Vector3<Real> cdiff = sphere.center - boxCenter;
197  Vector3<Real> relvel = sphereVelocity - boxVelocity;
198 
199  Result result;
200  DoQuery(maxTime, boxCenter, boxExtent, sphere, cdiff, relvel, result);
201  return result;
202 }
203 
204 template <typename Real>
205 void FIQuery<Real, AlignedBox3<Real>, Sphere3<Real>>::DoQuery(Real maxTime,
206  Vector3<Real> const& boxCenter, Vector3<Real> const& boxExtent,
207  Sphere3<Real> const& sphere, Vector3<Real>& cdiff, Vector3<Real>& relvel,
208  Result& result)
209 {
210  // Flip coordinate frame into the first octant.
211  int signX = 1;
212  if (cdiff[0] < (Real)0)
213  {
214  cdiff[0] = -cdiff[0];
215  relvel[0] = -relvel[0];
216  signX = -1;
217  }
218 
219  int signY = 1;
220  if (cdiff[1] < (Real)0)
221  {
222  cdiff[1] = -cdiff[1];
223  relvel[1] = -relvel[1];
224  signY = -1;
225  }
226 
227  int signZ = 1;
228  if (cdiff[2] < (Real)0)
229  {
230  cdiff[2] = -cdiff[2];
231  relvel[2] = -relvel[2];
232  signZ = -1;
233  }
234 
235  // Intersection coordinates.
236  Real ix, iy, iz;
237  int retVal;
238 
239  if (cdiff[0] <= boxExtent[0])
240  {
241  if (cdiff[1] <= boxExtent[1])
242  {
243  if (cdiff[2] <= boxExtent[2])
244  {
245  // The sphere center is inside box. Use it as the contact
246  // point.
247  result.intersect = true;
248  result.contactTime = (Real)0;
249  result.contactPoint = sphere.center;
250  return;
251  }
252  else
253  {
254  // Sphere above face on axis Z.
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);
260  }
261  }
262  else
263  {
264  if (cdiff[2] <= boxExtent[2])
265  {
266  // Sphere above face on axis Y.
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);
272  }
273  else
274  {
275  // Sphere is above the edge formed by faces y and z.
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);
281  }
282  }
283  }
284  else
285  {
286  if (cdiff[1] <= boxExtent[1])
287  {
288  if (cdiff[2] <= boxExtent[2])
289  {
290  // Sphere above face on axis X.
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);
296  }
297  else
298  {
299  // Sphere is above the edge formed by faces x and z.
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);
305  }
306  }
307  else
308  {
309  if (cdiff[2] <= boxExtent[2])
310  {
311  // Sphere is above the edge formed by faces x and y.
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);
317  }
318  else
319  {
320  // sphere is above the corner formed by faces x,y,z
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);
326  }
327  }
328  }
329 
330  if (retVal == 0 || result.contactTime > maxTime)
331  {
332  result.intersect = false;
333  }
334  else
335  {
336  // Calculate actual intersection (move point back into world coordinates).
337  result.intersect = true;
338  result.contactPoint = boxCenter + Vector3<Real>{signX * ix, signY * iy, signZ * iz};
339  }
340 }
341 
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)
345 {
346  // Find the time of a 3D line-sphere intersection between a line P = Dt,
347  // where P = (dx, dy, dz) and D = (vx, vy, vz) and a sphere of radius^2
348  // rsqr. This is valid only if there is, in fact, an intersection.
349 
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);
355 }
356 
357 template <typename Real>
358 Real FIQuery<Real, AlignedBox3<Real>, Sphere3<Real>>::GetEdgeIntersection(
359  Real dx, Real dz, Real vx, Real vz, Real vsqr, Real rsqr)
360 {
361  // Find the time of a 2D line-circle intersection between a line
362  // P = Dt where P = (dx,dz) and D = (vx, vz) and a circle of radius^2
363  // rsqr. This is valid only if there is, in fact, an intersection.
364 
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);
369 }
370 
371 template <typename Real>
372 int FIQuery<Real, AlignedBox3<Real>, Sphere3<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)
376 {
377  // Returns when and whether a sphere in the region above face +Z
378  // intersects face +Z or any of its vertices or edges. The input
379  // aboveFace is true when the x and y coordinates are within the x and y
380  // extents. The function will still work if they are not, but it needs
381  // to be false then, to avoid some checks that assume that x and y are
382  // within the extents. This function checks face z, and the vertex and
383  // two edges that the velocity is headed towards on the face.
384 
385  // Check for already intersecting if above face.
386  if (cz <= ez + radius && aboveFace)
387  {
388  contactTime = (Real)0;
389  return -1;
390  }
391 
392  // Check for easy out (moving away on Z axis).
393  if (vz >= (Real)0)
394  {
395  return 0;
396  }
397 
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;
403  Real crossX, crossY;
404  int signX, signY;
405 
406  // This determines which way the box is heading and finds the values of
407  // CrossX and CrossY which are positive if the sphere center will not
408  // pass through the box. Then it is only necessary to check two edges,
409  // the face and the vertex for intersection.
410 
411  if (vx >= (Real)0)
412  {
413  signX = 1;
414  dx = cx - ex;
415  crossX = vx * dz - vz * dx;
416  }
417  else
418  {
419  signX = -1;
420  dx = cx + ex;
421  crossX = vz * dx - vx * dz;
422  }
423 
424  if (vy >= (Real)0)
425  {
426  signY = 1;
427  dy = cy - ey;
428  crossY = vy * dz - vz * dy;
429  }
430  else
431  {
432  signY = -1;
433  dy = cy + ey;
434  crossY = vz * dy - vy * dz;
435  }
436 
437  // Does the circle intersect along the x edge?
438  if (crossX > radius * vx * signX)
439  {
440  if (crossX * crossX > rsqr * vsqrX)
441  {
442  // Sphere overshoots box on the x-axis (either side).
443  return 0;
444  }
445 
446  // Does the circle hit the y edge?
447  if (crossY > radius * vy * signY)
448  {
449  // Potential vertex intersection.
450  if (crossY * crossY > rsqr * vsqrY)
451  {
452  // Sphere overshoots box on the y-axis (either side).
453  return 0;
454  }
455 
456  Vector3<Real> relVelocity{ vx, vy, vz };
457  Vector3<Real> D{ dx, dy, dz };
458  Vector3<Real> cross = Cross(D, relVelocity);
459  Real crossSqrLen = Dot(cross, cross);
460  Real relvelSqrLen = Dot(relVelocity, relVelocity);
461  if (crossSqrLen > rsqr * relvelSqrLen)
462  {
463  // Sphere overshoots the box on the corner.
464  return 0;
465  }
466 
467  contactTime = GetVertexIntersection(dx, dy, dz, vx, vy, vz, rsqr);
468  ix = ex * signX;
469  iy = ey * signY;
470  }
471  else
472  {
473  // x-edge intersection
474  contactTime = GetEdgeIntersection(dx, dz, vx, vz, vsqrX, rsqr);
475  ix = ex * signX;
476  iy = cy + vy * contactTime;
477  }
478  }
479  else
480  {
481  // Does the circle hit the y edge?
482  if (crossY > radius * vy * signY)
483  {
484  // Potential y-edge intersection.
485  if (crossY * crossY > rsqr * vsqrY)
486  {
487  // Sphere overshoots box on the y-axis (either side).
488  return 0;
489  }
490 
491  contactTime = GetEdgeIntersection(dy, dz, vy, vz, vsqrY, rsqr);
492  ix = cx + vx * contactTime;
493  iy = ey * signY;
494  }
495  else
496  {
497  // Face intersection (easy).
498  contactTime = (-dz + radius) / vz;
499  ix = contactTime * vx + cx;
500  iy = contactTime * vy + cy;
501  }
502  }
503 
504  // z coordinate of any intersection must be the face of z.
505  iz = ez;
506  return 1;
507 }
508 
509 template <typename Real>
510 int FIQuery<Real, AlignedBox3<Real>, Sphere3<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,
513  Real& contactTime)
514 {
515  // Finds the intersection of a point dx and dz away from an edge with
516  // direction y. The sphere is at a point cy, and the edge is at the
517  // point ex. Checks the edge and the vertex the velocity is heading
518  // towards.
519 
520  Real rsqr = radius * radius;
521  Real dy, crossZ, crossX; // possible edge/vertex intersection
522  int signY;
523 
524  // Depending on the sign of Vy, pick the vertex that the velocity is
525  // heading towards on the edge, as well as creating crossX and crossZ
526  // such that their sign will always be positive if the sphere center goes
527  // over that edge.
528 
529  if (vy >= (Real)0)
530  {
531  signY = 1;
532  dy = cy - ey;
533  crossZ = dx * vy - dy * vx;
534  crossX = dz * vy - dy * vz;
535  }
536  else
537  {
538  signY = -1;
539  dy = cy + ey;
540  crossZ = dy * vx - dx * vy;
541  crossX = dy * vz - dz * vy;
542  }
543 
544  // Check where on edge this intersection will occur.
545  if (crossZ >= (Real)0 && crossX >= (Real)0
546  && crossX * crossX + crossZ * crossZ > rsqr * vy * vy)
547  {
548  // Sphere potentially intersects with vertex.
549  Vector3<Real> relVelocity{ vx, vy, vz };
550  Vector3<Real> D{ dx, dy, dz };
551  Vector3<Real> cross = Cross(D, relVelocity);
552  Real crossSqrLen = Dot(cross, cross);
553  Real relvelSqrLen = Dot(relVelocity, relVelocity);
554  if (crossSqrLen > rsqr * relvelSqrLen)
555  {
556  // Sphere overshoots the box on the vertex.
557  return 0;
558  }
559 
560  // Sphere actually does intersect the vertex.
561  contactTime = GetVertexIntersection(dx, dy, dz, vx, vy, vz, rsqr);
562  ix = ex;
563  iy = signY * ey;
564  iz = ez;
565  }
566  else
567  {
568  // Sphere intersects with edge.
569  Real vsqrX = vz * vz + vx * vx;
570  contactTime = GetEdgeIntersection(dx, dz, vx, vz, vsqrX, rsqr);
571  ix = ex;
572  iy = cy + contactTime * vy;
573  iz = ez;
574  }
575  return 1;
576 }
577 
578 template <typename Real>
579 int FIQuery<Real, AlignedBox3<Real>, Sphere3<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)
583 {
584  // Assumes the sphere center is in the region above the x and z planes.
585  // The input aboveEdge is true when the y coordinate is within the y
586  // extents. The function will still work if it is not, but it needs to be
587  // false then, to avoid some checks that assume that y is within the
588  // extent. This function checks the edge that the region is above, as
589  // well as does a "face region" check on the face it is heading towards.
590 
591  Real dx = cx - ex;
592  Real dz = cz - ez;
593  Real rsqr = radius * radius;
594 
595  if (aboveEdge)
596  {
597  Real diff = dx * dx + dz * dz - rsqr;
598  if (diff <= (Real)0)
599  {
600  // Circle is already intersecting the box.
601  contactTime = (Real)0;
602  return -1;
603  }
604  }
605 
606  Real dot = vx * dx + vz * dz;
607  if (dot >= (Real)0)
608  {
609  // Circle not moving towards box.
610  return 0;
611  }
612 
613  // The value dotPerp splits the region of interest along the edge in the
614  // middle of that region.
615  Real dotPerp = vz * dx - vx * dz;
616  if (dotPerp >= (Real)0)
617  {
618  // Sphere moving towards +z face.
619  if (vx >= (Real)0)
620  {
621  // Passed corner, moving away from box.
622  return 0;
623  }
624 
625  // Intersection with x-z edge. If there is trouble with objects that
626  // "scrape" the surface (velocity perpendicular to face normal, and
627  // point of contact with a radius direction parallel to the face
628  // normal), this check may need to be a little more inclusive (small
629  // tolerance due to floating point errors) as the edge check needs
630  // to get "scraping" objects (as they hit the edge with the point)
631  // and the face region check will not catch it because the object is
632  // not moving towards the face.
633  if (dotPerp <= -radius * vx)
634  {
635  return FindJustEdgeIntersection(cy, ez, ey, ex, dz, dx, vz, vy,
636  vx, iz, iy, ix, radius, contactTime);
637  }
638 
639  // Now, check the face of z for intersections.
640  return FindFaceRegionIntersection(ex, ey, ez, cx, cy, cz, vx, vy,
641  vz, ix, iy, iz, false, radius, contactTime);
642  }
643  else
644  {
645  // Sphere moving towards +x face.
646  if (vz >= (Real)0)
647  {
648  // Passed corner, moving away from box.
649  return 0;
650  }
651 
652  // Check intersection with x-z edge. See the note above about
653  // "scraping" objects.
654  if (dotPerp >= radius * vz)
655  {
656  // Possible edge/vertex intersection.
657  return FindJustEdgeIntersection(cy, ex, ey, ez, dx, dz, vx, vy,
658  vz, ix, iy, iz, radius, contactTime);
659  }
660 
661  // Now, check the face of x for intersections.
662  return FindFaceRegionIntersection(ez, ey, ex, cz, cy, cx, vz, vy,
663  vx, iz, iy, ix, false, radius, contactTime);
664  }
665 }
666 
667 template <typename Real>
668 int FIQuery<Real, AlignedBox3<Real>, Sphere3<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)
672 {
673  // Assumes the sphere is above the vertex +ex, +ey, +ez.
674 
675  Real dx = cx - ex;
676  Real dy = cy - ey;
677  Real dz = cz - ez;
678  Real rsqr = radius * radius;
679  Real diff = dx * dx + dy * dy + dz * dz - rsqr;
680  if (diff <= (Real)0)
681  {
682  // Sphere is already intersecting the box.
683  contactTime = (Real)0;
684  return -1;
685  }
686 
687  if (vx * dx + vy * dy + vz * dz >= (Real)0)
688  {
689  // Sphere not moving towards box.
690  return 0;
691  }
692 
693  // The box can be divided up into 3 regions, which simplifies checking to
694  // see what the sphere hits. The regions are divided by which edge
695  // (formed by the vertex and some axis) is closest to the velocity
696  // vector.
697  //
698  // To check if it hits the vertex, look at the edge (E) it is going
699  // towards. Create a plane formed by the other two edges (with E as the
700  // plane normal) with the vertex at the origin. The intercept along an
701  // axis, in that plane, of the line formed by the sphere center and the
702  // velocity as its direction, will be crossAxis/vEdge. Thus, the
703  // distance from the origin to the point in the plane that that line from
704  // the sphere in the velocity direction crosses will be the squared sum
705  // of those two intercepts. If that sum is less than the radius squared,
706  // then the sphere will hit the vertex otherwise, it will continue on
707  // past the vertex. If it misses, since it is known which edge the box
708  // is near, the find edge region test can be used.
709  //
710  // Also, due to the constrained conditions, only those six cases (two for
711  // each region, since crossEdge can be + or -) of signs of cross values
712  // can occur.
713  //
714  // The third case will also pick up cases where D = V, causing a zero
715  // cross.
716 
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;
723  Real vx2 = vx * vx;
724  Real vy2 = vy * vy;
725  Real vz2 = vz * vz;
726 
727  // Intersection with the vertex?
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))
731  {
732  // Standard line-sphere intersection.
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;
737  return 1;
738  }
739  else if (crossY < (Real)0 && crossZ >= (Real)0)
740  {
741  // x edge region, check y,z planes.
742  return FindEdgeRegionIntersection(ey, ex, ez, cy, cx, cz, vy, vx,
743  vz, iy, ix, iz, false, radius, contactTime);
744  }
745  else if (crossZ < (Real)0 && crossX < (Real)0)
746  {
747  // y edge region, check x,z planes.
748  return FindEdgeRegionIntersection(ex, ey, ez, cx, cy, cz, vx, vy,
749  vz, ix, iy, iz, false, radius, contactTime);
750  }
751  else // crossY >= 0 && crossX >= 0
752  {
753  // z edge region, check x,y planes.
754  return FindEdgeRegionIntersection(ex, ez, ey, cx, cz, cy, vx, vz,
755  vy, ix, iz, iy, false, radius, contactTime);
756  }
757 }
758 
759 }
DualQuaternion< Real > Dot(DualQuaternion< Real > const &d0, DualQuaternion< Real > const &d1)
DualQuaternion< Real > Cross(DualQuaternion< Real > const &d0, DualQuaternion< Real > const &d1)
Result operator()(Type0 const &primitive0, Type1 const &primitive1)
Vector< N, Real > center
GLuint64EXT * result
Definition: glext.h:10003


geometric_tools_engine
Author(s): Yijiang Huang
autogenerated on Thu Jul 18 2019 04:00:00