GteDistLine3AlignedBox3.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>
11 #include <Mathematics/GteDistPointAlignedBox.h>
12 #include <Mathematics/GteLine.h>
13 
14 namespace gte
15 {
16 
17 template <typename Real>
18 class DCPQuery<Real, Line3<Real>, AlignedBox3<Real>>
19 {
20 public:
21  struct Result
22  {
23  Real distance, sqrDistance;
24  Real lineParameter;
25  Vector3<Real> closestPoint[2];
26  };
27 
28  Result operator()(Line3<Real> const& line, AlignedBox3<Real> const& box);
29 
30 protected:
31  // Compute the distance and closest point between a line and an
32  // axis-aligned box whose center is the origin. On input, 'point' is the
33  // line origin and 'direction' is the line direction. On output, 'point'
34  // is the point on the box closest to the line. The 'direction' is
35  // non-const to allow transforming the problem into the first octant.
36  void DoQuery(Vector3<Real>& point, Vector3<Real>& direction,
37  Vector3<Real> const& boxExtent, Result& result);
38 
39 private:
40  void Face(int i0, int i1, int i2, Vector3<Real>& pnt,
41  Vector3<Real> const& dir, Vector3<Real> const& PmE,
42  Vector3<Real> const& boxExtent, Result& result);
43 
44  void CaseNoZeros(Vector3<Real>& pnt, Vector3<Real> const& dir,
45  Vector3<Real> const& boxExtent, Result& result);
46 
47  void Case0(int i0, int i1, int i2, Vector3<Real>& pnt,
48  Vector3<Real> const& dir, Vector3<Real> const& boxExtent,
49  Result& result);
50 
51  void Case00(int i0, int i1, int i2, Vector3<Real>& pnt,
52  Vector3<Real> const& dir, Vector3<Real> const& boxExtent,
53  Result& result);
54 
55  void Case000(Vector3<Real>& pnt, Vector3<Real> const& boxExtent,
56  Result& result);
57 };
58 
59 
60 template <typename Real>
61 typename DCPQuery<Real, Line3<Real>, AlignedBox3<Real>>::Result
62 DCPQuery<Real, Line3<Real>, AlignedBox3<Real>>::operator()(
63  Line3<Real> const& line, AlignedBox3<Real> const& box)
64 {
65  // Translate the line and box so that the box has center at the origin.
66  Vector3<Real> boxCenter, boxExtent;
67  box.GetCenteredForm(boxCenter, boxExtent);
68  Vector3<Real> point = line.origin - boxCenter;
69  Vector3<Real> direction = line.direction;
70 
71  Result result;
72  DoQuery(point, direction, boxExtent, result);
73 
74  // Compute the closest point on the line.
75  result.closestPoint[0] =
76  line.origin + result.lineParameter*line.direction;
77 
78  // Compute the closest point on the box.
79  result.closestPoint[1] = boxCenter + point;
80  return result;
81 }
82 
83 template <typename Real>
84 void DCPQuery<Real, Line3<Real>, AlignedBox3<Real>>::DoQuery(
85  Vector3<Real>& point, Vector3<Real>& direction,
86  Vector3<Real> const& boxExtent, Result& result)
87 {
88  result.sqrDistance = (Real)0;
89  result.lineParameter = (Real)0;
90 
91  // Apply reflections so that direction vector has nonnegative components.
92  bool reflect[3];
93  for (int i = 0; i < 3; ++i)
94  {
95  if (direction[i] < (Real)0)
96  {
97  point[i] = -point[i];
98  direction[i] = -direction[i];
99  reflect[i] = true;
100  }
101  else
102  {
103  reflect[i] = false;
104  }
105  }
106 
107  if (direction[0] > (Real)0)
108  {
109  if (direction[1] > (Real)0)
110  {
111  if (direction[2] > (Real)0) // (+,+,+)
112  {
113  CaseNoZeros(point, direction, boxExtent, result);
114  }
115  else // (+,+,0)
116  {
117  Case0(0, 1, 2, point, direction, boxExtent, result);
118  }
119  }
120  else
121  {
122  if (direction[2] > (Real)0) // (+,0,+)
123  {
124  Case0(0, 2, 1, point, direction, boxExtent, result);
125  }
126  else // (+,0,0)
127  {
128  Case00(0, 1, 2, point, direction, boxExtent, result);
129  }
130  }
131  }
132  else
133  {
134  if (direction[1] > (Real)0)
135  {
136  if (direction[2] > (Real)0) // (0,+,+)
137  {
138  Case0(1, 2, 0, point, direction, boxExtent, result);
139  }
140  else // (0,+,0)
141  {
142  Case00(1, 0, 2, point, direction, boxExtent, result);
143  }
144  }
145  else
146  {
147  if (direction[2] > (Real)0) // (0,0,+)
148  {
149  Case00(2, 0, 1, point, direction, boxExtent, result);
150  }
151  else // (0,0,0)
152  {
153  Case000(point, boxExtent, result);
154  }
155  }
156  }
157 
158  // Undo the reflections applied previously.
159  for (int i = 0; i < 3; ++i)
160  {
161  if (reflect[i])
162  {
163  point[i] = -point[i];
164  }
165  }
166 
167  result.distance = sqrt(result.sqrDistance);
168 }
169 
170 template <typename Real>
171 void DCPQuery<Real, Line3<Real>, AlignedBox3<Real>>::Face(int i0, int i1,
172  int i2, Vector3<Real>& pnt, Vector3<Real> const& dir,
173  Vector3<Real> const& PmE, Vector3<Real> const& boxExtent, Result& result)
174 {
175  Vector3<Real> PpE;
176  Real lenSqr, inv, tmp, param, t, delta;
177 
178  PpE[i1] = pnt[i1] + boxExtent[i1];
179  PpE[i2] = pnt[i2] + boxExtent[i2];
180  if (dir[i0] * PpE[i1] >= dir[i1] * PmE[i0])
181  {
182  if (dir[i0] * PpE[i2] >= dir[i2] * PmE[i0])
183  {
184  // v[i1] >= -e[i1], v[i2] >= -e[i2] (distance = 0)
185  pnt[i0] = boxExtent[i0];
186  inv = ((Real)1) / dir[i0];
187  pnt[i1] -= dir[i1] * PmE[i0] * inv;
188  pnt[i2] -= dir[i2] * PmE[i0] * inv;
189  result.lineParameter = -PmE[i0] * inv;
190  }
191  else
192  {
193  // v[i1] >= -e[i1], v[i2] < -e[i2]
194  lenSqr = dir[i0] * dir[i0] + dir[i2] * dir[i2];
195  tmp = lenSqr*PpE[i1] - dir[i1] * (dir[i0] * PmE[i0] +
196  dir[i2] * PpE[i2]);
197  if (tmp <= ((Real)2)*lenSqr*boxExtent[i1])
198  {
199  t = tmp / lenSqr;
200  lenSqr += dir[i1] * dir[i1];
201  tmp = PpE[i1] - t;
202  delta = dir[i0] * PmE[i0] + dir[i1] * tmp + dir[i2] * PpE[i2];
203  param = -delta / lenSqr;
204  result.sqrDistance += PmE[i0] * PmE[i0] + tmp*tmp +
205  PpE[i2] * PpE[i2] + delta*param;
206 
207  result.lineParameter = param;
208  pnt[i0] = boxExtent[i0];
209  pnt[i1] = t - boxExtent[i1];
210  pnt[i2] = -boxExtent[i2];
211  }
212  else
213  {
214  lenSqr += dir[i1] * dir[i1];
215  delta = dir[i0] * PmE[i0] + dir[i1] * PmE[i1] + dir[i2] * PpE[i2];
216  param = -delta / lenSqr;
217  result.sqrDistance += PmE[i0] * PmE[i0] + PmE[i1] * PmE[i1]
218  + PpE[i2] * PpE[i2] + delta*param;
219 
220  result.lineParameter = param;
221  pnt[i0] = boxExtent[i0];
222  pnt[i1] = boxExtent[i1];
223  pnt[i2] = -boxExtent[i2];
224  }
225  }
226  }
227  else
228  {
229  if (dir[i0] * PpE[i2] >= dir[i2] * PmE[i0])
230  {
231  // v[i1] < -e[i1], v[i2] >= -e[i2]
232  lenSqr = dir[i0] * dir[i0] + dir[i1] * dir[i1];
233  tmp = lenSqr*PpE[i2] - dir[i2] * (dir[i0] * PmE[i0] +
234  dir[i1] * PpE[i1]);
235  if (tmp <= ((Real)2)*lenSqr*boxExtent[i2])
236  {
237  t = tmp / lenSqr;
238  lenSqr += dir[i2] * dir[i2];
239  tmp = PpE[i2] - t;
240  delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1] + dir[i2] * tmp;
241  param = -delta / lenSqr;
242  result.sqrDistance += PmE[i0] * PmE[i0] + PpE[i1] * PpE[i1] +
243  tmp*tmp + delta*param;
244 
245  result.lineParameter = param;
246  pnt[i0] = boxExtent[i0];
247  pnt[i1] = -boxExtent[i1];
248  pnt[i2] = t - boxExtent[i2];
249  }
250  else
251  {
252  lenSqr += dir[i2] * dir[i2];
253  delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1] + dir[i2] * PmE[i2];
254  param = -delta / lenSqr;
255  result.sqrDistance += PmE[i0] * PmE[i0] + PpE[i1] * PpE[i1] +
256  PmE[i2] * PmE[i2] + delta*param;
257 
258  result.lineParameter = param;
259  pnt[i0] = boxExtent[i0];
260  pnt[i1] = -boxExtent[i1];
261  pnt[i2] = boxExtent[i2];
262  }
263  }
264  else
265  {
266  // v[i1] < -e[i1], v[i2] < -e[i2]
267  lenSqr = dir[i0] * dir[i0] + dir[i2] * dir[i2];
268  tmp = lenSqr*PpE[i1] - dir[i1] * (dir[i0] * PmE[i0] +
269  dir[i2] * PpE[i2]);
270  if (tmp >= (Real)0)
271  {
272  // v[i1]-edge is closest
273  if (tmp <= ((Real)2)*lenSqr*boxExtent[i1])
274  {
275  t = tmp / lenSqr;
276  lenSqr += dir[i1] * dir[i1];
277  tmp = PpE[i1] - t;
278  delta = dir[i0] * PmE[i0] + dir[i1] * tmp + dir[i2] * PpE[i2];
279  param = -delta / lenSqr;
280  result.sqrDistance += PmE[i0] * PmE[i0] + tmp*tmp +
281  PpE[i2] * PpE[i2] + delta*param;
282 
283  result.lineParameter = param;
284  pnt[i0] = boxExtent[i0];
285  pnt[i1] = t - boxExtent[i1];
286  pnt[i2] = -boxExtent[i2];
287  }
288  else
289  {
290  lenSqr += dir[i1] * dir[i1];
291  delta = dir[i0] * PmE[i0] + dir[i1] * PmE[i1]
292  + dir[i2] * PpE[i2];
293  param = -delta / lenSqr;
294  result.sqrDistance += PmE[i0] * PmE[i0] + PmE[i1] * PmE[i1]
295  + PpE[i2] * PpE[i2] + delta*param;
296 
297  result.lineParameter = param;
298  pnt[i0] = boxExtent[i0];
299  pnt[i1] = boxExtent[i1];
300  pnt[i2] = -boxExtent[i2];
301  }
302  return;
303  }
304 
305  lenSqr = dir[i0] * dir[i0] + dir[i1] * dir[i1];
306  tmp = lenSqr*PpE[i2] - dir[i2] * (dir[i0] * PmE[i0] +
307  dir[i1] * PpE[i1]);
308  if (tmp >= (Real)0)
309  {
310  // v[i2]-edge is closest
311  if (tmp <= ((Real)2)*lenSqr*boxExtent[i2])
312  {
313  t = tmp / lenSqr;
314  lenSqr += dir[i2] * dir[i2];
315  tmp = PpE[i2] - t;
316  delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1] + dir[i2] * tmp;
317  param = -delta / lenSqr;
318  result.sqrDistance += PmE[i0] * PmE[i0] + PpE[i1] * PpE[i1] +
319  tmp*tmp + delta*param;
320 
321  result.lineParameter = param;
322  pnt[i0] = boxExtent[i0];
323  pnt[i1] = -boxExtent[i1];
324  pnt[i2] = t - boxExtent[i2];
325  }
326  else
327  {
328  lenSqr += dir[i2] * dir[i2];
329  delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1]
330  + dir[i2] * PmE[i2];
331  param = -delta / lenSqr;
332  result.sqrDistance += PmE[i0] * PmE[i0] + PpE[i1] * PpE[i1]
333  + PmE[i2] * PmE[i2] + delta*param;
334 
335  result.lineParameter = param;
336  pnt[i0] = boxExtent[i0];
337  pnt[i1] = -boxExtent[i1];
338  pnt[i2] = boxExtent[i2];
339  }
340  return;
341  }
342 
343  // (v[i1],v[i2])-corner is closest
344  lenSqr += dir[i2] * dir[i2];
345  delta = dir[i0] * PmE[i0] + dir[i1] * PpE[i1] + dir[i2] * PpE[i2];
346  param = -delta / lenSqr;
347  result.sqrDistance += PmE[i0] * PmE[i0] + PpE[i1] * PpE[i1]
348  + PpE[i2] * PpE[i2] + delta*param;
349 
350  result.lineParameter = param;
351  pnt[i0] = boxExtent[i0];
352  pnt[i1] = -boxExtent[i1];
353  pnt[i2] = -boxExtent[i2];
354  }
355  }
356 }
357 
358 template <typename Real>
359 void DCPQuery<Real, Line3<Real>, AlignedBox3<Real>>::CaseNoZeros(
360  Vector3<Real>& pnt, Vector3<Real> const& dir,
361  Vector3<Real> const& boxExtent, Result& result)
362 {
363  Vector3<Real> PmE = pnt - boxExtent;
364  Real prodDxPy = dir[0] * PmE[1];
365  Real prodDyPx = dir[1] * PmE[0];
366  Real prodDzPx, prodDxPz, prodDzPy, prodDyPz;
367 
368  if (prodDyPx >= prodDxPy)
369  {
370  prodDzPx = dir[2] * PmE[0];
371  prodDxPz = dir[0] * PmE[2];
372  if (prodDzPx >= prodDxPz)
373  {
374  // line intersects x = e0
375  Face(0, 1, 2, pnt, dir, PmE, boxExtent, result);
376  }
377  else
378  {
379  // line intersects z = e2
380  Face(2, 0, 1, pnt, dir, PmE, boxExtent, result);
381  }
382  }
383  else
384  {
385  prodDzPy = dir[2] * PmE[1];
386  prodDyPz = dir[1] * PmE[2];
387  if (prodDzPy >= prodDyPz)
388  {
389  // line intersects y = e1
390  Face(1, 2, 0, pnt, dir, PmE, boxExtent, result);
391  }
392  else
393  {
394  // line intersects z = e2
395  Face(2, 0, 1, pnt, dir, PmE, boxExtent, result);
396  }
397  }
398 }
399 
400 template <typename Real>
401 void DCPQuery<Real, Line3<Real>, AlignedBox3<Real>>::Case0(int i0, int i1,
402  int i2, Vector3<Real>& pnt, Vector3<Real> const& dir,
403  Vector3<Real> const& boxExtent, Result& result)
404 {
405  Real PmE0 = pnt[i0] - boxExtent[i0];
406  Real PmE1 = pnt[i1] - boxExtent[i1];
407  Real prod0 = dir[i1] * PmE0;
408  Real prod1 = dir[i0] * PmE1;
409  Real delta, invLSqr, inv;
410 
411  if (prod0 >= prod1)
412  {
413  // line intersects P[i0] = e[i0]
414  pnt[i0] = boxExtent[i0];
415 
416  Real PpE1 = pnt[i1] + boxExtent[i1];
417  delta = prod0 - dir[i0] * PpE1;
418  if (delta >= (Real)0)
419  {
420  invLSqr = ((Real)1) / (dir[i0] * dir[i0] + dir[i1] * dir[i1]);
421  result.sqrDistance += delta*delta*invLSqr;
422  pnt[i1] = -boxExtent[i1];
423  result.lineParameter = -(dir[i0] * PmE0 + dir[i1] * PpE1)*invLSqr;
424  }
425  else
426  {
427  inv = ((Real)1) / dir[i0];
428  pnt[i1] -= prod0*inv;
429  result.lineParameter = -PmE0*inv;
430  }
431  }
432  else
433  {
434  // line intersects P[i1] = e[i1]
435  pnt[i1] = boxExtent[i1];
436 
437  Real PpE0 = pnt[i0] + boxExtent[i0];
438  delta = prod1 - dir[i1] * PpE0;
439  if (delta >= (Real)0)
440  {
441  invLSqr = ((Real)1) / (dir[i0] * dir[i0] + dir[i1] * dir[i1]);
442  result.sqrDistance += delta*delta*invLSqr;
443  pnt[i0] = -boxExtent[i0];
444  result.lineParameter = -(dir[i0] * PpE0 + dir[i1] * PmE1)*invLSqr;
445  }
446  else
447  {
448  inv = ((Real)1) / dir[i1];
449  pnt[i0] -= prod1*inv;
450  result.lineParameter = -PmE1*inv;
451  }
452  }
453 
454  if (pnt[i2] < -boxExtent[i2])
455  {
456  delta = pnt[i2] + boxExtent[i2];
457  result.sqrDistance += delta*delta;
458  pnt[i2] = -boxExtent[i2];
459  }
460  else if (pnt[i2] > boxExtent[i2])
461  {
462  delta = pnt[i2] - boxExtent[i2];
463  result.sqrDistance += delta*delta;
464  pnt[i2] = boxExtent[i2];
465  }
466 }
467 
468 template <typename Real>
469 void DCPQuery<Real, Line3<Real>, AlignedBox3<Real>>::Case00(int i0, int i1,
470  int i2, Vector3<Real>& pnt, Vector3<Real> const& dir,
471  Vector3<Real> const& boxExtent, Result& result)
472 {
473  Real delta;
474 
475  result.lineParameter = (boxExtent[i0] - pnt[i0]) / dir[i0];
476 
477  pnt[i0] = boxExtent[i0];
478 
479  if (pnt[i1] < -boxExtent[i1])
480  {
481  delta = pnt[i1] + boxExtent[i1];
482  result.sqrDistance += delta*delta;
483  pnt[i1] = -boxExtent[i1];
484  }
485  else if (pnt[i1] > boxExtent[i1])
486  {
487  delta = pnt[i1] - boxExtent[i1];
488  result.sqrDistance += delta*delta;
489  pnt[i1] = boxExtent[i1];
490  }
491 
492  if (pnt[i2] < -boxExtent[i2])
493  {
494  delta = pnt[i2] + boxExtent[i2];
495  result.sqrDistance += delta*delta;
496  pnt[i2] = -boxExtent[i2];
497  }
498  else if (pnt[i2] > boxExtent[i2])
499  {
500  delta = pnt[i2] - boxExtent[i2];
501  result.sqrDistance += delta*delta;
502  pnt[i2] = boxExtent[i2];
503  }
504 }
505 
506 template <typename Real>
507 void DCPQuery<Real, Line3<Real>, AlignedBox3<Real>>::Case000(
508  Vector3<Real>& pnt, Vector3<Real> const& boxExtent, Result& result)
509 {
510  Real delta;
511 
512  if (pnt[0] < -boxExtent[0])
513  {
514  delta = pnt[0] + boxExtent[0];
515  result.sqrDistance += delta*delta;
516  pnt[0] = -boxExtent[0];
517  }
518  else if (pnt[0] > boxExtent[0])
519  {
520  delta = pnt[0] - boxExtent[0];
521  result.sqrDistance += delta*delta;
522  pnt[0] = boxExtent[0];
523  }
524 
525  if (pnt[1] < -boxExtent[1])
526  {
527  delta = pnt[1] + boxExtent[1];
528  result.sqrDistance += delta*delta;
529  pnt[1] = -boxExtent[1];
530  }
531  else if (pnt[1] > boxExtent[1])
532  {
533  delta = pnt[1] - boxExtent[1];
534  result.sqrDistance += delta*delta;
535  pnt[1] = boxExtent[1];
536  }
537 
538  if (pnt[2] < -boxExtent[2])
539  {
540  delta = pnt[2] + boxExtent[2];
541  result.sqrDistance += delta*delta;
542  pnt[2] = -boxExtent[2];
543  }
544  else if (pnt[2] > boxExtent[2])
545  {
546  delta = pnt[2] - boxExtent[2];
547  result.sqrDistance += delta*delta;
548  pnt[2] = boxExtent[2];
549  }
550 }
551 
552 
553 }
param
GLenum GLfloat param
Definition: glcorearb.h:99
distance
GLsizei GLsizei GLfloat distance
Definition: glext.h:9704
gte::DCPQuery::operator()
Result operator()(Type0 const &primitive0, Type1 const &primitive1)
t
GLdouble GLdouble t
Definition: glext.h:239
result
GLuint64EXT * result
Definition: glext.h:10003

geometric_tools_engine
Author(s): Yijiang Huang
autogenerated on Thu Jul 18 2019 03:59:59