GeographicLib: Intersect.cpp Source File (original) (raw)

11#include

12#include

13#include

14#include

16using namespace std;

21 : _geod(geod)

22 , _a(_geod.EquatorialRadius())

23 , _f(_geod.Flattening())

24 , _rR(sqrt(_geod.EllipsoidArea() / (4 * Math::pi())))

25 , _d(_rR * Math::pi())

26 , _eps(3 * numeric_limits::epsilon())

27 , _tol(_d * pow(numeric_limits::epsilon(), 3/real(4)))

28 , _delta(_d * pow(numeric_limits::epsilon(), 1/real(5)))

29 , _comp(_delta)

30 , _cnt0(0)

31 , _cnt1(0)

32 , _cnt2(0)

33 , _cnt3(0)

34 , _cnt4(0)

35 {

36 _t1 = _t4 = _a * (1 - _f) * Math::pi();

37 _t2 = 2 * distpolar(90);

38 _geod.Inverse(0, 0, 90, 0, _t5); _t5 *= 2;

39 if (_f > 0) {

40 _t3 = distoblique();

41 _t4 = _t1;

42 } else {

43 _t3 = _t5;

44 _t4 = polarb();

45 swap(_t1, _t2);

46 }

47 _d1 = _t2 / 2;

48 _d2 = 2 * _t3 / 3;

49 _d3 = _t4 - _delta;

50 if (! (_d1 < _d3 && _d2 < _d3 && _d2 < 2 * _t1) )

51 throw GeographicErr("Ellipsoid too eccentric for Closest");

52 }

66 XPoint p = ClosestInt(lineX, lineY, XPoint(p0));

67 if (c) *c = p.c;

68 return p.data();

69 }

76 int& segmode, int* c) const {

79 segmode, c);

80 }

84 const GeodesicLine& lineY, int& segmode, int* c) const {

85 XPoint p = SegmentInt(lineX, lineY, segmode);

86 if (c) *c = p.c;

87 return p.data();

88 }

99 int* c) const {

100 XPoint p = NextInt(lineX, lineY);

101 if (c) *c = p.c;

102 return p.data();

103 }

104

105 std::vectorIntersect::Point

113

114 std::vectorIntersect::Point

117 Math::real maxdist, std::vector& c, const Point& p0)

118 const {

121 maxdist, c, p0);

122 }

123

124 std::vectorIntersect::Point

127 vector c;

128 return AllInternal(lineX, lineY, maxdist, p0, c, false);

129 }

130

131 std::vectorIntersect::Point

133 Math::real maxdist, std::vector& c, const Point& p0)

134 const {

135 return AllInternal(lineX, lineY, maxdist, p0, c, true);

136 }

137

138 Intersect::XPoint

140 const Intersect::XPoint& p) const {

141

142

143 real latX, lonX, aziX, latY, lonY, aziY;

144 lineX.Position(p.x , latX, lonX, aziX);

145 lineY.Position(p.y, latY, lonY, aziY);

146 real z, aziXa, aziYa;

147 _geod.Inverse(latX, lonX, latY, lonY, z, aziXa, aziYa);

148 real sinz = sin(z/_rR), cosz = cos(z/_rR);

149

150 real dX, dY, dXY,

153 real s = copysign(real(1), XY + (dXY + dY - dX));

154

155

156

157

158

159

163 int c;

164 if (z <= _eps * _rR) {

165 sX = sY = 0;

166

167 if (fabs(sinX - sinY) <= _eps && fabs(cosX - cosY) <= _eps)

168 c = 1;

169 else if (fabs(sinX + sinY) <= _eps && fabs(cosX + cosY) <= _eps)

170 c = -1;

171 else

172 c = 0;

173 } else if (fabs(sinX) <= _eps && fabs(sinY) <= _eps) {

174 c = cosX * cosY > 0 ? 1 : -1;

175

176 sX = cosX * z/2; sY = -cosY * z/2;

177

178

179 } else {

180

181

182

183

184 sX = _rR * atan2(sinY * sinz, sinY * cosX * cosz - cosY * sinX);

185 sY = _rR * atan2(sinX * sinz, -sinX * cosY * cosz + cosX * sinY);

186 c = 0;

187 }

188 return XPoint(sX, sY, c);

189 }

190

191 Intersect::XPoint

192 Intersect::Basic(const GeodesicLine& lineX, const GeodesicLine& lineY,

193 const Intersect::XPoint& p0) const {

194 ++_cnt1;

195 XPoint q = p0;

196 for (int n = 0;

197 n < numit_ ||

199 ++n) {

200 ++_cnt0;

201 XPoint dq = Spherical(lineX, lineY, q);

202 q += dq;

203 if (q.c || !(dq.Dist() > _tol)) break;

204 }

205 return q;

206 }

207

208 Intersect::XPoint

209 Intersect::ClosestInt(const GeodesicLine& lineX, const GeodesicLine& lineY,

210 const Intersect::XPoint& p0) const {

211 const int num = 5;

212 const int ix[num] = { 0, 1, -1, 0, 0 };

213 const int iy[num] = { 0, 0, 0, 1, -1 };

214 bool skip[num] = { 0, 0, 0, 0, 0 };

215 XPoint q;

216 for (int n = 0; n < num; ++n) {

217 if (skip[n]) continue;

218 XPoint qx = Basic(lineX, lineY, p0 + XPoint(ix[n] * _d1, iy[n] * _d1));

219 qx = fixcoincident(p0, qx);

220 if (_comp.eq(q, qx)) continue;

221 if (qx.Dist(p0) < _t1) { q = qx; ++_cnt2; break; }

222 if (n == 0 || qx.Dist(p0) < q.Dist(p0)) { q = qx; ++_cnt2; }

223 for (int m = n + 1; m < num; ++m)

224 skip[m] = skip[m] ||

225 qx.Dist(p0 + XPoint(ix[m]*_d1, iy[m]*_d1)) < 2*_t1 - _d1 - _delta;

226 }

227 return q;

228 }

229

230 Intersect::XPoint

231 Intersect::NextInt(const GeodesicLine& lineX, const GeodesicLine& lineY)

232 const {

233 const int num = 8;

234 const int ix[num] = { -1, -1, 1, 1, -2, 0, 2, 0 };

235 const int iy[num] = { -1, 1, -1, 1, 0, 2, 0, -2 };

236 bool skip[num] = { 0, 0, 0, 0, 0, 0, 0, 0 };

237 XPoint z(0,0),

239 for (int n = 0; n < num; ++n) {

240 if (skip[n]) continue;

241 XPoint qx = Basic(lineX, lineY, XPoint(ix[n] * _d2, iy[n] * _d2));

242 qx = fixcoincident(z, qx);

243 bool zerop = _comp.eq(z, qx);

244 if (qx.c == 0 && zerop) continue;

245 if (qx.c && zerop) {

246 for (int sgn = -1; sgn <= 1; sgn+=2) {

247 real s = ConjugateDist(lineX, sgn * _d, false);

248 XPoint qa(s, qx.c*s, qx.c);

249 if (qa.Dist() < q.Dist()) { q = qa; ++_cnt2; }

250 }

251 } else {

252 if (qx.Dist() < q.Dist()) { q = qx; ++_cnt2; }

253 }

254 for (int sgn = -1; sgn <= 1; ++sgn) {

255

256

257 if ((qx.c == 0 && sgn != 0) || (zerop && sgn == 0)) continue;

258 XPoint qy = qx.c ? qx + Point(sgn * _d2, qx.c * sgn *_d2) : qx;

259 for (int m = n + 1; m < num; ++m)

260 skip[m] = skip[m] ||

261 qy.Dist(XPoint(ix[m]*_d2, iy[m]*_d2)) < 2*_t1 - _d2 - _delta;

262 }

263 }

264 return q;

265 }

266

267 Intersect::XPoint

268 Intersect::SegmentInt(const GeodesicLine& lineX, const GeodesicLine& lineY,

269 int& segmode) const {

270

271

272

273 const bool conjectureproved = false;

274 real sx = lineX.Distance(), sy = lineY.Distance();

275

276 XPoint p0 = XPoint(sx/2, sy/2), q = ClosestInt(lineX, lineY, p0);

277 q = fixsegment(sx, sy, q);

278 segmode = segmentmode(sx, sy, q);

279

280 if (!conjectureproved && segmode != 0 && p0.Dist() >= p0.Dist(q)) {

281 int segmodex = 1;

282 XPoint qx;

283

284 for (int ix = 0; ix < 2 && segmodex != 0; ++ix) {

285 for (int iy = 0; iy < 2 && segmodex != 0; ++iy) {

286 XPoint t(ix * sx, iy * sy);

287

288 if (q.Dist(t) >= 2 * _t1) {

289 ++_cnt3;

290 qx = Basic(lineX, lineY, t);

291

292

293 qx = fixcoincident(t, qx);

294

295

296 segmodex = segmentmode(sx, sy, qx);

297 }

298 }

299 }

300 if (segmodex == 0) { ++_cnt4; segmode = 0; q = qx; }

301 }

302 return q;

303 }

304

305 std::vectorIntersect::XPoint

306 Intersect::AllInt0(const GeodesicLine& lineX,

307 const GeodesicLine& lineY,

308 Math::real maxdist, const XPoint& p0) const {

309 real maxdistx = maxdist + _delta;

310 const int m = int(ceil(maxdistx / _d3)),

311 m2 = m*m + (m - 1) % 2,

312 n = m - 1;

313 real d3 = maxdistx/m;

314 vector start(m2);

315 vector skip(m2, false);

316 int h = 0, c0 = 0;

317 start[h++] = p0;

318 for (int i = -n; i <= n; i += 2)

319 for (int j = -n; j <= n; j += 2) {

320 if (!(i == 0 && j == 0))

321 start[h++] = p0 + XPoint( d3 * (i + j) / 2, d3 * (i - j) / 2);

322 }

323

324 set<XPoint, SetComp> r(_comp);

325 set<XPoint, SetComp> c(_comp);

326 vector added;

327 for (int k = 0; k < m2; ++k) {

328 if (skip[k]) continue;

329 XPoint q = Basic(lineX, lineY, start[k]);

330 if (r.find(q) != r.end()

331

332 || (c0 != 0 && c.find(fixcoincident(p0, q)) != c.end()))

333 continue;

334 added.clear();

335 if (q.c != 0) {

336

337

338 c0 = q.c;

339

340 q = fixcoincident(p0, q);

341 c.insert(q);

342

343

344 for (auto qp = r.begin(); qp != r.end(); ) {

345 if (_comp.eq(fixcoincident(p0, *qp, c0), q)) {

346 qp = r.erase(qp);

347 }

348 else

349 ++qp;

350 }

351 real s0 = q.x;

352 XPoint qc;

353 real t, m12, M12, M21;

354 lineX.GenPosition(false, s0,

357 t, t, t, t, m12, M12, M21, t);

358

359 for (int sgn = -1; sgn <= 1; sgn += 2) {

361 do {

362 sa = ConjugateDist(lineX, s0 + sa + sgn*_d, false, m12, M12, M21)

363 - s0;

364 qc = q + XPoint(sa, c0*sa);

365 added.push_back(qc);

366 r.insert(qc);

367 } while (qc.Dist(p0) <= maxdistx);

368 }

369 }

370 added.push_back(q);

371 r.insert(q);

372 for (auto qp = added.cbegin(); qp != added.cend(); ++qp) {

373 for (int l = k + 1; l < m2; ++l)

374 skip[l] = skip[l] || qp->Dist(start[l]) < 2*_t1 - d3 - _delta;

375 }

376 }

377

378 for (auto qp = r.begin(); qp != r.end(); ) {

379 if (!(qp->Dist(p0) <= maxdist))

380 qp = r.erase(qp);

381 else

382 ++qp;

383 }

384 vector v(r.size());

385 int i = 0;

386 for (auto p = r.cbegin(); p != r.cend(); ++p)

387 v[i++] = *p;

388 sort(v.begin(), v.end(), RankPoint(p0));

389 return v;

390 }

391

392 std::vectorIntersect::Point

393 Intersect::AllInternal(const GeodesicLine& lineX, const GeodesicLine& lineY,

395 std::vector& c, bool cp) const {

396 const vector

397 v = AllInt0(lineX, lineY, fmax(real(0), maxdist), XPoint(p0));

398 int i = int(v.size());

399 vector u(i);

400 if (cp) c.resize(i);

401 for (int j = 0; j < i; ++j) {

402 u[j] = v[j].data();

403 if (cp) c[j] = v[j].c;

404 }

405 return u;

406 }

407

409 const {

410 GeodesicLine line = _geod.Line(lat1, 0, 0,

414 real s = ConjugateDist(line, (1 + _f/2) * _a * Math::pi() / 2, true);

415 if (lat2) {

418 *lat2, t, t, t, t, t, t, t);

419 }

420 return s;

421 }

422

424 if (_f == 0) {

425 if (lata) *lata = 64;

426 if (latb) *latb = 90-64;

427 return _d;

428 }

430 lat0 = 63, s0 = distpolar(lat0),

431 lat1 = 65, s1 = distpolar(lat1),

432 lat2 = 64, s2 = distpolar(lat2),

433 latx = lat2, sx = s2;

434

435 for (int i = 0; i < 10; ++i) {

436 real den = (lat1-lat0)*s2 + (lat0-lat2)*s1 + (lat2-lat1)*s0;

437 if (!(den < 0 || den > 0)) break;

438 real latn = ((lat1-lat0)*(lat1+lat0)*s2 + (lat0-lat2)*(lat0+lat2)*s1 +

439 (lat2-lat1)*(lat2+lat1)*s0) / (2*den);

440 lat0 = lat1; s0 = s1;

441 lat1 = lat2; s1 = s2;

442 lat2 = latn; s2 = distpolar(lat2);

443 if (_f < 0 ? (s2 < sx) : (s2 > sx)) {

444 sx = s2;

445 latx = lat2;

446 }

447 }

448 if (lata) *lata = latx;

449 if (latb) distpolar(latx, latb);

450 return 2 * sx;

451 }

452

453

457

458

459

460

462 for (int i = 0; i < 100; ++i) {

463 real t, m13, M13, M31;

464 line.GenPosition(false, s,

467 t, t, t, t, m13, M13, M31, t);

469

470 m23 = m13 * M12 - m12 * M13,

471

472 M23 = M13 * M21 + (m12 == 0 ? 0 : (1 - M12 * M21) * m13/m12),

473 M32 = M31 * M12 + (m13 == 0 ? 0 : (1 - M13 * M31) * m12/m13);

474 real ds = semi ? m23 * M23 / (1 - M23*M32) : -m23 / M32;

475 s = s + ds;

476 if (!(fabs(ds) > _tol)) break;

477 }

478 return s;

479 }

480

484 GeodesicLine line = _geod.Line(0, 0, azi, LineCaps);

485 real s = ConjugateDist(line, _d, false);

486 if (ds) {

487 XPoint p = Basic(line, line, XPoint(s/2, -3*s/2));

488 if (sp) *sp = p.x;

489 if (sm) *sm = p.y;

490 *ds = p.Dist() - 2*s;

491 }

492 return s;

493 }

494

498 if (_f == 0) {

499 if (azi) *azi = 45;

500 if (sp) *sp = 0.5;

501 if (sm) *sm = -1.5;

502 return _d;

503 }

505 azi0 = 46, ds0, s0 = conjdist(azi0, &ds0, &sa, &sb),

506 azi1 = 44, ds1, s1 = conjdist(azi1, &ds1, &sa, &sb),

507 azix = azi1, dsx = fabs(ds1), sx = s1, sax = sa, sbx = sb;

508

509 (void) s0;

510 for (int i = 0; i < 10 && ds1 != ds0; ++i) {

511 real azin = (azi0*ds1-azi1*ds0)/(ds1-ds0);

512 azi0 = azi1; s0 = s1; ds0 = ds1;

513 azi1 = azin; s1 = conjdist(azi1, &ds1, &sa, &sb);

514 if (fabs(ds1) < dsx) {

515 azix = azi1, sx = s1, dsx = fabs(ds1);

516 sax = sa; sbx = sb;

517 if (ds1 == 0) break;

518 }

519 }

520 if (azi) *azi = azix;

521 if (sp) *sp = sax;

522 if (sm) *sm = sbx;

523 return sx;

524 }

525

526 Intersect::XPoint

527 Intersect::fixcoincident(const Intersect::XPoint& p0,

528 const Intersect::XPoint& p) {

529 return fixcoincident(p0, p, p.c);

530 }

531

532 Intersect::XPoint

533 Intersect::fixcoincident(const Intersect::XPoint& p0,

534 const Intersect::XPoint& p, int c) {

535 if (c == 0) return p;

536

537

538

539

540

541

542

543 real s = ((p0.x + c * p0.y) - (p.x + c * p.y))/2;

544 return p + XPoint(s, c*s);

545 }

546

547 Intersect::XPoint

549 const Intersect::XPoint& p) {

550 if (p.c == 0) return p;

551

552

553

554

555

556

557

558

559

561 pya = p.y - p.c * p.x, sa = -p.x,

562 pyb = p.y - p.c * (p.x-sx), sb = sx - p.x,

563 pxc = p.x - p.c * p.y, sc = p.c * -p.y,

564 pxd = p.x - p.c * (p.y-sy), sd = p.c * (sy - p.y);

565 bool

566 ga = 0 <= pya && pya <= sy,

567 gb = 0 <= pyb && pyb <= sy,

568 gc = 0 <= pxc && pxc <= sx,

569 gd = 0 <= pxd && pxd <= sx;

571

572 if (ga && gb) s = (sa + sb) / 2;

573 else if (gc && gd) s = (sc + sd) / 2;

574 else if (ga && gc) s = (sa + sc) / 2;

575 else if (ga && gd) s = (sa + sd) / 2;

576 else if (gb && gc) s = (sb + sc) / 2;

577 else if (gb && gd) s = (sb + sd) / 2;

578 else {

579

580 if (p.c > 0) {

581

582

583 if (fabs((p.x - p.y) + sy) < fabs((p.x - p.y) - sx))

584 s = (sy - (p.x + p.y))/2;

585 else

586 s = (sx - (p.x + p.y))/2;

587 } else {

588

589

590 if (fabs(p.x + p.y) < fabs((p.x + p.y) - (sx + sy)))

591 s = (0 - (p.x - p.y))/2;

592 else

593 s = ((sx - sy) - (p.x - p.y))/2;

594 }

595 }

596 return p + XPoint(s, p.c*s);

597 }

598

599}

GeographicLib::Math::real real

Header for GeographicLib::Intersect class.

#define GEOGRAPHICLIB_PANIC(msg)

Math::real Position(real s12, real &lat2, real &lon2, real &azi2, real &m12, real &M12, real &M21, real &S12) const

GeodesicLine InverseLine(real lat1, real lon1, real lat2, real lon2, unsigned caps=ALL) const

GeodesicLine Line(real lat1, real lon1, real azi1, unsigned caps=ALL) const

Math::real Inverse(real lat1, real lon1, real lat2, real lon2, real &s12, real &azi1, real &azi2, real &m12, real &M12, real &M21, real &S12) const

Exception handling for GeographicLib.

Point Segment(Math::real latX1, Math::real lonX1, Math::real latX2, Math::real lonX2, Math::real latY1, Math::real lonY1, Math::real latY2, Math::real lonY2, int &segmode, int *c=nullptr) const

Definition Intersect.cpp:72

static const unsigned LineCaps

Point Closest(Math::real latX, Math::real lonX, Math::real aziX, Math::real latY, Math::real lonY, Math::real aziY, const Point &p0=Point(0, 0), int *c=nullptr) const

Definition Intersect.cpp:55

std::pair< Math::real, Math::real > Point

std::vector< Point > All(Math::real latX, Math::real lonX, Math::real aziX, Math::real latY, Math::real lonY, Math::real aziY, Math::real maxdist, std::vector< int > &c, const Point &p0=Point(0, 0)) const

Definition Intersect.cpp:115

Point Next(Math::real latX, Math::real lonX, Math::real aziX, Math::real aziY, int *c=nullptr) const

Definition Intersect.cpp:91

Intersect(const Geodesic &geod)

Definition Intersect.cpp:20

Mathematical functions needed by GeographicLib.

static void sincosde(T x, T t, T &sinx, T &cosx)

static T AngDiff(T x, T y, T &e)

Namespace for GeographicLib.

void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)