GteDistSegmentSegmentExact.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/GteDCPQuery.h>
11 #include <Mathematics/GteSegment.h>
12 
13 // Compute the closest points on the line segments P(s) = (1-s)*P0 + s*P1 and
14 // Q(t) = (1-t)*Q0 + t*Q1 for 0 <= s <= 1 and 0 <= t <= 1. The algorithm
15 // relies on exact rational arithmetic.
16 
17 namespace gte
18 {
19 
20 template <int N, typename Rational>
21 class DistanceSegmentSegmentExact
22 {
23 public:
24  struct Result
25  {
26  Rational sqrDistance;
27  Rational parameter[2];
28  Vector<N, Rational> closest[2];
29  };
30 
31  Result operator()(Segment<N, Rational> const& segment0,
32  Segment<N, Rational> const& segment1);
33 
34  Result operator()(
35  Vector<N, Rational> const& P0, Vector<N, Rational> const& P1,
36  Vector<N, Rational> const& Q0, Vector<N, Rational> const& Q1);
37 };
38 
39 
40 template <int N, typename Rational>
41 typename DistanceSegmentSegmentExact<N, Rational>::Result
42 DistanceSegmentSegmentExact<N, Rational>::operator()(
43  Segment<N, Rational> const& segment0,
44  Segment<N, Rational> const& segment1)
45 {
46  return operator()(segment0.p[0], segment0.p[1], segment1.p[0],
47  segment1.p[1]);
48 }
49 
50 template <int N, typename Rational>
51 typename DistanceSegmentSegmentExact<N, Rational>::Result
52 DistanceSegmentSegmentExact<N, Rational>::operator()(
53  Vector<N, Rational> const& P0, Vector<N, Rational> const& P1,
54  Vector<N, Rational> const& Q0, Vector<N, Rational> const& Q1)
55 {
56  Vector<N, Rational> P1mP0 = P1 - P0;
57  Vector<N, Rational> Q1mQ0 = Q1 - Q0;
58  Vector<N, Rational> P0mQ0 = P0 - Q0;
59  Rational a = Dot(P1mP0, P1mP0);
60  Rational b = Dot(P1mP0, Q1mQ0);
61  Rational c = Dot(Q1mQ0, Q1mQ0);
62  Rational d = Dot(P1mP0, P0mQ0);
63  Rational e = Dot(Q1mQ0, P0mQ0);
64  Rational const zero = (Rational)0;
65  Rational const one = (Rational)1;
66  Rational det = a * c - b * b;
67  Rational s, t, nd, bmd, bte, ctd, bpe, ate, btd;
68 
69  if (det > zero)
70  {
71  bte = b * e;
72  ctd = c * d;
73  if (bte <= ctd) // s <= 0
74  {
75  s = zero;
76  if (e <= zero) // t <= 0
77  {
78  // region 6
79  t = zero;
80  nd = -d;
81  if (nd >= a)
82  {
83  s = one;
84  }
85  else if (nd > zero)
86  {
87  s = nd / a;
88  }
89  // else: s is already zero
90  }
91  else if (e < c) // 0 < t < 1
92  {
93  // region 5
94  t = e / c;
95  }
96  else // t >= 1
97  {
98  // region 4
99  t = one;
100  bmd = b - d;
101  if (bmd >= a)
102  {
103  s = one;
104  }
105  else if (bmd > zero)
106  {
107  s = bmd / a;
108  }
109  // else: s is already zero
110  }
111  }
112  else // s > 0
113  {
114  s = bte - ctd;
115  if (s >= det) // s >= 1
116  {
117  // s = 1
118  s = one;
119  bpe = b + e;
120  if (bpe <= zero) // t <= 0
121  {
122  // region 8
123  t = zero;
124  nd = -d;
125  if (nd <= zero)
126  {
127  s = zero;
128  }
129  else if (nd < a)
130  {
131  s = nd / a;
132  }
133  // else: s is already one
134  }
135  else if (bpe < c) // 0 < t < 1
136  {
137  // region 1
138  t = bpe / c;
139  }
140  else // t >= 1
141  {
142  // region 2
143  t = one;
144  bmd = b - d;
145  if (bmd <= zero)
146  {
147  s = zero;
148  }
149  else if (bmd < a)
150  {
151  s = bmd / a;
152  }
153  // else: s is already one
154  }
155  }
156  else // 0 < s < 1
157  {
158  ate = a * e;
159  btd = b * d;
160  if (ate <= btd) // t <= 0
161  {
162  // region 7
163  t = zero;
164  nd = -d;
165  if (nd <= zero)
166  {
167  s = zero;
168  }
169  else if (nd >= a)
170  {
171  s = one;
172  }
173  else
174  {
175  s = nd / a;
176  }
177  }
178  else // t > 0
179  {
180  t = ate - btd;
181  if (t >= det) // t >= 1
182  {
183  // region 3
184  t = one;
185  bmd = b - d;
186  if (bmd <= zero)
187  {
188  s = zero;
189  }
190  else if (bmd >= a)
191  {
192  s = one;
193  }
194  else
195  {
196  s = bmd / a;
197  }
198  }
199  else // 0 < t < 1
200  {
201  // region 0
202  s /= det;
203  t /= det;
204  }
205  }
206  }
207  }
208  }
209  else
210  {
211  // The segments are parallel. The quadratic factors to R(s,t) =
212  // a*(s-(b/a)*t)^2 + 2*d*(s - (b/a)*t) + f, where a*c = b^2,
213  // e = b*d/a, f = |P0-Q0|^2, and b is not zero. R is constant along
214  // lines of the form s-(b/a)*t = k, and the minimum of R occurs on the
215  // line a*s - b*t + d = 0. This line must intersect both the s-axis
216  // and the t-axis because 'a' and 'b' are not zero. Because of
217  // parallelism, the line is also represented by -b*s + c*t - e = 0.
218  //
219  // The code determines an edge of the domain [0,1]^2 that intersects
220  // the minimum line, or if none of the edges intersect, it determines
221  // the closest corner to the minimum line. The conditionals are
222  // designed to test first for intersection with the t-axis (s = 0)
223  // using -b*s + c*t - e = 0 and then with the s-axis (t = 0) using
224  // a*s - b*t + d = 0.
225 
226  // When s = 0, solve c*t - e = 0 (t = e/c).
227  if (e <= zero) // t <= 0
228  {
229  // Now solve a*s - b*t + d = 0 for t = 0 (s = -d/a).
230  t = zero;
231  nd = -d;
232  if (nd <= zero) // s <= 0
233  {
234  // region 6
235  s = zero;
236  }
237  else if (nd >= a) // s >= 1
238  {
239  // region 8
240  s = one;
241  }
242  else // 0 < s < 1
243  {
244  // region 7
245  s = nd / a;
246  }
247  }
248  else if (e >= c) // t >= 1
249  {
250  // Now solve a*s - b*t + d = 0 for t = 1 (s = (b-d)/a).
251  t = one;
252  bmd = b - d;
253  if (bmd <= zero) // s <= 0
254  {
255  // region 4
256  s = zero;
257  }
258  else if (bmd >= a) // s >= 1
259  {
260  // region 2
261  s = one;
262  }
263  else // 0 < s < 1
264  {
265  // region 3
266  s = bmd / a;
267  }
268  }
269  else // 0 < t < 1
270  {
271  // The point (0,e/c) is on the line and domain, so we have one
272  // point at which R is a minimum.
273  s = zero;
274  t = e / c;
275  }
276  }
277 
278  Result result;
279  result.parameter[0] = s;
280  result.parameter[1] = t;
281  result.closest[0] = P0 + s * P1mP0;
282  result.closest[1] = Q0 + t * Q1mQ0;
283  Vector<N, Rational> diff = result.closest[1] - result.closest[0];
284  result.sqrDistance = Dot(diff, diff);
285  return result;
286 }
287 
288 
289 }
gte::Segment::p
std::array< Vector< N, Real >, 2 > p
Definition: GteSegment.h:46
a
GLboolean GLboolean GLboolean GLboolean a
Definition: glcorearb.h:1217
c
const GLubyte * c
Definition: glext.h:11671
gte::Dot
DualQuaternion< Real > Dot(DualQuaternion< Real > const &d0, DualQuaternion< Real > const &d1)
t
GLdouble GLdouble t
Definition: glext.h:239
b
GLboolean GLboolean GLboolean b
Definition: glcorearb.h:1217
s
GLdouble s
Definition: glext.h:231
gte::DistanceSegmentSegmentExact::operator()
Result operator()(Segment< N, Rational > const &segment0, Segment< N, Rational > const &segment1)
Definition: GteDistSegmentSegmentExact.h:42
result
GLuint64EXT * result
Definition: glext.h:10003

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