278 {
279
280
281
282 double delta;
283 double averagephi, dist, dphiin, dphiex;
285 double bfparn[2];
286 bfparn[0] = 0;
287 bfparn[1] = 0;
288
289
290 int ncandid[4], ncand;
291 double bcand[2][4];
292 int segnob[] = {1, -1, 1, -1};
294 double angularcoefficient[4];
295 double bfpar[4];
296
298
299
302 averagephi = std::atan2(dy, dx);
303 dist = std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
305
306 dphiin = std::asin(delta / dist);
307
309 dphiex = std::asin(delta / dist);
310
312 phi = averagephi +
f * dphiex;
314 angularcoefficient[0] = std::tan(
phi);
315
317 phi = averagephi +
f * dphiex;
319 angularcoefficient[1] = std::tan(
phi);
320
322 phi = averagephi +
f * dphiin;
324 angularcoefficient[2] = std::tan(
phi);
325
327 phi = averagephi +
f * dphiin;
329 angularcoefficient[3] = std::tan(
phi);
330
331
332 for (i = 0;
i < 4;
i++) {
333 bpar[0] =
y1 - angularcoefficient[
i] *
x1 + segnob[0] *
r1 * std::sqrt(1 + angularcoefficient[i] * angularcoefficient[i]);
334 bpar[1] =
y1 - angularcoefficient[
i] *
x1 + segnob[1] *
r1 * std::sqrt(1 + angularcoefficient[i] * angularcoefficient[i]);
335 bpar[2] =
y2 - angularcoefficient[
i] *
x2 + segnob[2] *
r2 * std::sqrt(1 + angularcoefficient[i] * angularcoefficient[i]);
336 bpar[3] =
y2 - angularcoefficient[
i] *
x2 + segnob[3] *
r2 * std::sqrt(1 + angularcoefficient[i] * angularcoefficient[i]);
337 double delta = 0.00001;
338 ncand = 0;
339 if (std::abs(bpar[0] - bpar[2]) < delta) {
340 bfparn[ncand] = bpar[0];
341 ncand = ncand + 1;
342 }
343 if (std::abs(bpar[0] - bpar[3]) < delta) {
344 bfparn[ncand] = bpar[0];
345 ncand = ncand + 1;
346 }
347 if (std::abs(bpar[1] - bpar[2]) < delta) {
348 bfparn[ncand] = bpar[1];
349 ncand = ncand + 1;
350 }
351 if (std::abs(bpar[1] - bpar[3]) < delta) {
352 bfparn[ncand] = bpar[1];
353 ncand = ncand + 1;
354 }
355 bcand[0][
i] = bfparn[0];
356 bcand[1][
i] = bfparn[1];
358 }
359 firsttime = 0;
360 for (i = 0;
i < 4;
i++) {
361 if (ncandid[i] == 1) {
362 bfpar[
i] = bcand[0][
i];
363 } else {
364 bfpar[
i] = bcand[firsttime][
i];
365 firsttime++;
366 }
367 }
368
369 for (i = 0;
i < 4;
i++) {
372 }
373 }