1

2# -*- coding: utf-8 -*-

3

4 u'''Extended 3-D vector class L{Vector3d} and functions.

5

6 Function L{intersection3d3}, L{intersections2}, L{parse3d}, L{sumOf},

7 L{trilaterate2d2} and L{trilaterate3d2}.

8'''

9

10# from pygeodesy.basics import isscalar # from .fmath

11 from pygeodesy.constants import EPS, EPS0, EPS1, EPS4, INT0, isnear0, \

12 _0_0, _1_0

13 from pygeodesy.errors import IntersectionError, _ValueError, VectorError, \

14 _xattr, _xError, _xkwds_get, _xkwds, _xkwds_popitem

15 from pygeodesy.fmath import euclid, fabs, fdot, hypot, sqrt, \

16 fsum1_, isscalar

17# from pygeodesy.fsums import fsum1_ # from .fmath

18# from pygeodesy.formy import _radical2 # in _intersects2 below

19 from pygeodesy.interns import NN, _COMMA_, _concentric_, _intersection_, \

20 _near_, _negative_, _no_, _too_

21 from pygeodesy.iters import PointsIter, Fmt

22 from pygeodesy.lazily import _ALL_DOCS, _ALL_LAZY, _ALL_MODS as _MODS

23 from pygeodesy.named import _xnamed, _xotherError

24 from pygeodesy.namedTuples import Intersection3Tuple, NearestOn2Tuple, \

25 NearestOn6Tuple, Vector3Tuple # Vector4Tuple

26# from pygeodesy.nvectorBase import _nsumOf # _MODS

27# from pygeodesy.streprs import Fmt # from .iters

28 from pygeodesy.units import _fi_j2, Radius, Radius_

29 from pygeodesy.utily import atan2b, sincos2d

30# from pygeodesy.vector2d import .... # in .... below

31 from pygeodesy.vector3dBase import Vector3dBase

32

33# from math import fabs, sqrt # from .fmath

34

35__all__ = _ALL_LAZY.vector3d

36__version__ = '23.08.05'

37

38

39 class Vector3d(Vector3dBase):

40 '''Extended 3-D vector.

41

42 In a geodesy context, these may be used to represent:

43 - earth-centered, earth-fixed cartesian (ECEF)

44 - n-vector representing a normal to a point on earth's surface

45 - great circle normal to vector

46 - motion vector on earth's surface

47 - etc.

48 '''

49

50 def bearing(self, useZ=True):

51 '''Get the "bearing" of this vector.

52

53 @kwarg useZ: If C{True}, use the Z component, otherwise

54 consider the Y as +Z axis.

55

56 @return: Bearing (compass C{degrees}), the counter-clockwise

57 angle off the +Z axis.

58 '''

59 x, y = self.x, self.y

60 if useZ:

61 x, y = hypot(x, y), self.z

62 return atan2b(x, y)

63

64 def circin6(self, point2, point3, eps=EPS4):

65 '''Return the radius and center of the I{inscribed} aka I{In- circle}

66 of a (3-D) triangle formed by this and two other points.

67

68 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},

69 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).

70 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},

71 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).

72 @kwarg eps: Tolerance for function L{pygeodesy.trilaterate3d2} if

73 C{B{useZ} is True} otherwise L{pygeodesy.trilaterate2d2}.

74

75 @return: L{Circin6Tuple}C{(radius, center, deltas, cA, cB, cC)}. The

76 C{center} and contact points C{cA}, C{cB} and C{cC}, each an

77 instance of this (sub-)class, are co-planar with this and the

78 two given points.

79

80 @raise ImportError: Package C{numpy} not found, not installed or older

81 than version 1.10.

82

83 @raise IntersectionError: Near-coincident or -colinear points or

84 a trilateration or C{numpy} issue.

85

86 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.

87

88 @see: Function L{pygeodesy.circin6}, U{Incircle

89 <https://MathWorld.Wolfram.com/Incircle.html>} and U{Contact

90 Triangle<https://MathWorld.Wolfram.com/ContactTriangle.html>}.

91 '''

92 try:

93 return _MODS.vector2d._circin6(self, point2, point3, eps=eps, useZ=True)

94 except (AssertionError, TypeError, ValueError) as x:

95 raise _xError(x, point=self, point2=point2, point3=point3)

96

97 def circum3(self, point2, point3, circum=True, eps=EPS4):

98 '''Return the radius and center of the smallest circle I{through} or

99 I{containing} this and two other (3-D) points.

100

101 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}

102 or C{Vector4Tuple}).

103 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}

104 or C{Vector4Tuple}).

105 @kwarg circum: If C{True} return the C{circumradius} and C{circumcenter},

106 always, ignoring the I{Meeus}' Type I case (C{bool}).

107 @kwarg eps: Tolerance passed to function L{pygeodesy.trilaterate3d2}.

108

109 @return: A L{Circum3Tuple}C{(radius, center, deltas)}. The C{center}, an

110 instance of this (sub-)class, is co-planar with this and the two

111 given points.

112

113 @raise ImportError: Package C{numpy} not found, not installed or older than

114 version 1.10.

115

116 @raise IntersectionError: Near-concentric, -coincident or -colinear points

117 or a trilateration or C{numpy} issue.

118

119 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.

120

121 @see: Function L{pygeodesy.circum3} and methods L{circum4_} and L{meeus2}.

122 '''

123 try:

124 return _MODS.vector2d._circum3(self, point2, point3, circum=circum,

125 eps=eps, useZ=True, clas=self.classof)

126 except (AssertionError, TypeError, ValueError) as x:

127 raise _xError(x, point=self, point2=point2, point3=point3, circum=circum)

128

129 def circum4_(self, *points):

130 '''Best-fit a sphere through this and two or more other (3-D) points.

131

132 @arg points: Other points (each a C{Cartesian}, L{Vector3d}, C{Vector3Tuple}

133 or C{Vector4Tuple}).

134

135 @return: L{Circum4Tuple}C{(radius, center, rank, residuals)} with C{center}

136 an instance if this (sub-)class.

137

138 @raise ImportError: Package C{numpy} not found, not installed or

139 older than version 1.10.

140

141 @raise NumPyError: Some C{numpy} issue.

142

143 @raise PointsError: Too few B{C{points}}.

144

145 @raise TypeError: One of the B{C{points}} invalid.

146

147 @see: Function L{pygeodesy.circum4_} and methods L{circum3} and L{meeus2}.

148 '''

149 return _MODS.vector2d.circum4_(self, *points, useZ=True, Vector=self.classof)

150

151 def iscolinearWith(self, point1, point2, eps=EPS):

152 '''Check whether this and two other (3-D) points are colinear.

153

154 @arg point1: One point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}

155 or C{Vector4Tuple}).

156 @arg point2: An other point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}

157 or C{Vector4Tuple}).

158 @kwarg eps: Tolerance (C{scalar}), same units as C{x},

159 C{y}, and C{z}.

160

161 @return: C{True} if this point is colinear with B{C{point1}} and

162 B{C{point2}}, C{False} otherwise.

163

164 @raise TypeError: Invalid B{C{point1}} or B{C{point2}}.

165

166 @see: Method L{nearestOn}.

167 '''

168 v = self if self.name else _otherV3d(NN_OK=False, this=self)

169 return _MODS.vector2d._iscolinearWith(v, point1, point2, eps=eps)

170

171 def meeus2(self, point2, point3, circum=False):

172 '''Return the radius and I{Meeus}' Type of the smallest circle I{through}

173 or I{containing} this and two other (3-D) points.

174

175 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}

176 or C{Vector4Tuple}).

177 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}

178 or C{Vector4Tuple}).

179 @kwarg circum: If C{True} return the C{circumradius} and C{circumcenter}

180 always, overriding I{Meeus}' Type II case (C{bool}).

181

182 @return: L{Meeus2Tuple}C{(radius, Type)}, with C{Type} the C{circumcenter}

183 iff C{B{circum}=True}.

184

185 @raise IntersectionError: Coincident or colinear points, iff C{B{circum}=True}.

186

187 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.

188

189 @see: Function L{pygeodesy.meeus2} and methods L{circum3} and L{circum4_}.

190 '''

191 try:

192 return _MODS.vector2d._meeus2(self, point2, point3, circum, clas=self.classof)

193 except (TypeError, ValueError) as x:

194 raise _xError(x, point=self, point2=point2, point3=point3, circum=circum)

195

196 def nearestOn(self, point1, point2, within=True):

197 '''Locate the point between two points closest to this point.

198

199 @arg point1: Start point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or

200 C{Vector4Tuple}).

201 @arg point2: End point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or

202 C{Vector4Tuple}).

203 @kwarg within: If C{True} return the closest point between the given

204 points, otherwise the closest point on the extended

205 line through both points (C{bool}).

206

207 @return: Closest point, either B{C{point1}} or B{C{point2}} or an instance

208 of this (sub-)class.

209

210 @raise TypeError: Invalid B{C{point1}} or B{C{point2}}.

211

212 @see: Method L{sphericalTrigonometry.LatLon.nearestOn3} and U{3-D Point-Line

213 Distance<https://MathWorld.Wolfram.com/Point-LineDistance3-Dimensional.html>}.

214 '''

215 return _nearestOn2(self, point1, point2, within=within).closest

216

217 def nearestOn6(self, points, closed=False, useZ=True): # eps=EPS

218 '''Locate the point on a path or polygon closest to this point.

219

220 The closest point is either on and within the extent of a polygon

221 edge or the nearest of that edge's end points.

222

223 @arg points: The path or polygon points (C{Cartesian}, L{Vector3d},

224 C{Vector3Tuple} or C{Vector4Tuple}[]).

225 @kwarg closed: Optionally, close the path or polygon (C{bool}).

226 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).

227

228 @return: A L{NearestOn6Tuple}C{(closest, distance, fi, j, start, end)}

229 with the C{closest}, the C{start} and the C{end} point each

230 an instance of this point's (sub-)class.

231

232 @raise PointsError: Insufficient number of B{C{points}}

233

234 @raise TypeError: Non-cartesian B{C{points}}.

235

236 @note: Distances measured with method L{Vector3d.equirectangular}.

237

238 @see: Function L{nearestOn6}.

239 '''

240 return nearestOn6(self, points, closed=closed, useZ=useZ) # Vector=self.classof

241

242 def parse(self, str3d, sep=_COMMA_, name=NN):

243 '''Parse an C{"x, y, z"} string to a L{Vector3d} instance.

244

245 @arg str3d: X, y and z string (C{str}), see function L{parse3d}.

246 @kwarg sep: Optional separator (C{str}).

247 @kwarg name: Optional instance name (C{str}), overriding this name.

248

249 @return: The instance (L{Vector3d}).

250

251 @raise VectorError: Invalid B{C{str3d}}.

252 '''

253 return parse3d(str3d, sep=sep, Vector=self.classof, name=name or self.name)

254

255 def radii11(self, point2, point3):

256 '''Return the radii of the C{Circum-}, C{In-}, I{Soddy} and C{Tangent}

257 circles of a (3-D) triangle.

258

259 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},

260 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).

261 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},

262 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).

263

264 @return: L{Radii11Tuple}C{(rA, rB, rC, cR, rIn, riS, roS, a, b, c, s)}.

265

266 @raise TriangleError: Near-coincident or -colinear points.

267

268 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.

269

270 @see: Function L{pygeodesy.radii11}, U{Incircle

271 <https://MathWorld.Wolfram.com/Incircle.html>}, U{Soddy Circles

272 <https://MathWorld.Wolfram.com/SoddyCircles.html>} and U{Tangent

273 Circles<https://MathWorld.Wolfram.com/TangentCircles.html>}.

274 '''

275 try:

276 return _MODS.vector2d._radii11ABC(self, point2, point3, useZ=True)[0]

277 except (TypeError, ValueError) as x:

278 raise _xError(x, point=self, point2=point2, point3=point3)

279

280 def soddy4(self, point2, point3, eps=EPS4):

281 '''Return the radius and center of the C{inner} I{Soddy} circle of a

282 (3-D) triangle.

283

284 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},

285 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).

286 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},

287 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).

288 @kwarg eps: Tolerance for function L{pygeodesy.trilaterate3d2} if

289 C{B{useZ} is True} otherwise L{pygeodesy.trilaterate2d2}.

290

291 @return: L{Soddy4Tuple}C{(radius, center, deltas, outer)}. The C{center},

292 an instance of B{C{point1}}'s (sub-)class, is co-planar with the

293 three given points.

294

295 @raise ImportError: Package C{numpy} not found, not installed or older

296 than version 1.10.

297

298 @raise IntersectionError: Near-coincident or -colinear points or

299 a trilateration or C{numpy} issue.

300

301 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.

302

303 @see: Function L{pygeodesy.soddy4}.

304 '''

305 return _MODS.vector2d.soddy4(self, point2, point3, eps=eps, useZ=True)

306

307 def trilaterate2d2(self, radius, center2, radius2, center3, radius3, eps=EPS, z=INT0):

308 '''Trilaterate this and two other circles, each given as a (2-D) center

309 and a radius.

310

311 @arg radius: Radius of this circle (same C{units} as this C{x} and C{y}.

312 @arg center2: Center of the 2nd circle (C{Cartesian}, L{Vector3d},

313 C{Vector2Tuple}, C{Vector3Tuple} or C{Vector4Tuple}).

314 @arg radius2: Radius of this circle (same C{units} as this C{x} and C{y}.

315 @arg center3: Center of the 3rd circle (C{Cartesian}, L{Vector3d},

316 C{Vector2Tuple}, C{Vector3Tuple} or C{Vector4Tuple}).

317 @arg radius3: Radius of the 3rd circle (same C{units} as this C{x} and C{y}.

318 @kwarg eps: Tolerance to check the trilaterated point I{delta} on all

319 3 circles (C{scalar}) or C{None} for no checking.

320 @kwarg z: Optional Z component of the trilaterated point (C{scalar}).

321

322 @return: Trilaterated point, an instance of this (sub-)class with C{z=B{z}}.

323

324 @raise IntersectionError: No intersection, near-concentric or -colinear

325 centers, trilateration failed some other way

326 or the trilaterated point is off one circle

327 by more than B{C{eps}}.

328

329 @raise TypeError: Invalid B{C{center2}} or B{C{center3}}.

330

331 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}.

332

333 @see: Function L{pygeodesy.trilaterate2d2}.

334 '''

335

336 def _xyr3(r, **name_v):

337 v = _otherV3d(useZ=False, **name_v)

338 return v.x, v.y, r

339

340 try:

341 return _MODS.vector2d._trilaterate2d2(*(_xyr3(radius, center=self) +

342 _xyr3(radius2, center2=center2) +

343 _xyr3(radius3, center3=center3)),

344 eps=eps, Vector=self.classof, z=z)

345 except (AssertionError, TypeError, ValueError) as x:

346 raise _xError(x, center=self, radius=radius,

347 center2=center2, radius2=radius2,

348 center3=center3, radius3=radius3)

349

350 def trilaterate3d2(self, radius, center2, radius2, center3, radius3, eps=EPS):

351 '''Trilaterate this and two other spheres, each given as a (3-D) center

352 and a radius.

353

354 @arg radius: Radius of this sphere (same C{units} as this C{x}, C{y}

355 and C{z}).

356 @arg center2: Center of the 2nd sphere (C{Cartesian}, L{Vector3d},

357 C{Vector3Tuple} or C{Vector4Tuple}).

358 @arg radius2: Radius of this sphere (same C{units} as this C{x}, C{y}

359 and C{z}).

360 @arg center3: Center of the 3rd sphere (C{Cartesian}, , L{Vector3d},

361 C{Vector3Tuple} or C{Vector4Tuple}).

362 @arg radius3: Radius of the 3rd sphere (same C{units} as this C{x}, C{y}

363 and C{z}).

364 @kwarg eps: Pertubation tolerance (C{scalar}), same units as C{x}, C{y}

365 and C{z} or C{None} for no pertubations.

366

367 @return: 2-Tuple with two trilaterated points, each an instance of this

368 (sub-)class. Both points are the same instance if all three

369 spheres intersect or abut in a single point.

370

371 @raise ImportError: Package C{numpy} not found, not installed or

372 older than version 1.10.

373

374 @raise IntersectionError: Near-concentric, -colinear, too distant or

375 non-intersecting spheres or C{numpy} issue.

376

377 @raise NumPyError: Some C{numpy} issue.

378

379 @raise TypeError: Invalid B{C{center2}} or B{C{center3}}.

380

381 @raise UnitError: Invalid B{C{radius}}, B{C{radius2}} or B{C{radius3}}.

382

383 @note: Package U{numpy<https://PyPI.org/project/numpy>} is required,

384 version 1.10 or later.

385

386 @see: Norrdine, A. U{I{An Algebraic Solution to the Multilateration

387 Problem}<https://www.ResearchGate.net/publication/275027725>}

388 and U{I{implementation}<https://www.ResearchGate.net/publication/288825016>}.

389 '''

390 try:

391 c1 = _otherV3d(center=self, NN_OK=False)

392 return _MODS.vector2d._trilaterate3d2(c1, Radius_(radius, low=eps),

393 center2, radius2,

394 center3, radius3,

395 eps=eps, clas=self.classof)

396 except (AssertionError, TypeError, ValueError) as x:

397 raise _xError(x, center=self, radius=radius,

398 center2=center2, radius2=radius2,

399 center3=center3, radius3=radius3)

400

401

402 def _intersect3d3(start1, end1, start2, end2, eps=EPS, useZ=False): # MCCABE 16 in .formy.intersection2, .rhumbBase

403 # (INTERNAL) Intersect two lines, see L{intersection3d3} below,

404 # separated to allow callers to embellish any exceptions

405

406 def _outside(t, d2, o): # -o before start#, +o after end#

407 return -o if t < 0 else (o if t > d2 else 0) # XXX d2 + eps?

408

409 def _rightangle2(s1, b1, s2, useZ):

410 # Get the C{s1'} and C{e1'}, corners of a right-angle

411 # triangle with the hypotenuse thru C{s1} at bearing

412 # C{b1} and the right angle at C{s2}

413 dx, dy, d = s2.minus(s1).xyz

414 if useZ and not isnear0(d): # not supported

415 raise IntersectionError(useZ=d, bearing=b1)

416 s, c = sincos2d(b1)

417 if s and c:

418 dx *= c / s

419 dy *= s / c

420 e1 = Vector3d(s2.x, s1.y + dx, s1.z)

421 s1 = Vector3d(s1.x + dy, s2.y, s1.z)

422 else: # orthogonal

423 d = euclid(dx, dy) # hypot?

424 e1 = Vector3d(s1.x + s * d, s1.y + c * d, s1.z)

425 return s1, e1

426

427 s1 = x = _otherV3d(useZ=useZ, start1=start1)

428 s2 = _otherV3d(useZ=useZ, start2=start2)

429 b1 = isscalar(end1)

430 if b1: # bearing, make an e1

431 s1, e1 = _rightangle2(s1, end1, s2, useZ)

432 else:

433 e1 = _otherV3d(useZ=useZ, end1=end1)

434 b2 = isscalar(end2)

435 if b2: # bearing, make an e2

436 s2, e2 = _rightangle2(s2, end2, x, useZ)

437 else:

438 e2 = _otherV3d(useZ=useZ, end2=end2)

439

440 a = e1.minus(s1)

441 b = e2.minus(s2)

442 c = s2.minus(s1)

443

444 ab = a.cross(b)

445 d = fabs(c.dot(ab))

446 e = max(EPS0, eps or _0_0)

447 if d > EPS0 and ab.length > e: # PYCHOK no cover

448 d = d / ab.length # /= chokes PyChecker

449 if d > e: # argonic, skew lines distance

450 raise IntersectionError(skew_d=d, txt=_no_(_intersection_))

451

452 # co-planar, non-skew lines

453 ab2 = ab.length2

454 if ab2 < e: # colinear, parallel or null line(s)

455 x = b.length2 < a.length2

456 if x: # make C{a} the shortest

457 a, b = b, a

458 s1, s2 = s2, s1

459 e1, e2 = e2, e1

460 b1, b2 = b2, b1

461 if b.length2 < e: # PYCHOK no cover

462 if c.length < e:

463 return s1, 0, 0

464 elif e2.minus(e1).length < e:

465 return e1, 0, 0

466 elif a.length2 < e: # null (s1, e1), non-null (s2, e2)

467 # like _nearestOn2(s1, s2, e2, within=False, eps=e)

468 t = s1.minus(s2).dot(b)

469 v = s2.plus(b.times(t / b.length2))

470 if s1.minus(v).length < e:

471 o = 0 if b2 else _outside(t, b.length2, 1 if x else 2)

472 return (v, o, 0) if x else (v, 0, o)

473 raise IntersectionError(length2=ab2, txt=_no_(_intersection_))

474

475 cb = c.cross(b)

476 t = cb.dot(ab)

477 o1 = 0 if b1 else _outside(t, ab2, 1)

478 v = s1.plus(a.times(t / ab2))

479 o2 = 0 if b2 else _outside(v.minus(s2).dot(b), b.length2, 2)

480 return v, o1, o2

481

482

483 def intersection3d3(start1, end1, start2, end2, eps=EPS, useZ=True,

484 **Vector_and_kwds):

485 '''Compute the intersection point of two lines, each defined by two

486 points or by a point and a bearing.

487

488 @arg start1: Start point of the first line (C{Cartesian}, L{Vector3d},

489 C{Vector3Tuple} or C{Vector4Tuple}).

490 @arg end1: End point of the first line (C{Cartesian}, L{Vector3d},

491 C{Vector3Tuple} or C{Vector4Tuple}) or the bearing at

492 B{C{start1}} (compass C{degrees}).

493 @arg start2: Start point of the second line (C{Cartesian}, L{Vector3d},

494 C{Vector3Tuple} or C{Vector4Tuple}).

495 @arg end2: End point of the second line (C{Cartesian}, L{Vector3d},

496 C{Vector3Tuple} or C{Vector4Tuple}) or the bearing at

497 B{C{start2}} (Ccompass C{degrees}).

498 @kwarg eps: Tolerance for skew line distance and length (C{EPS}).

499 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).

500 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the

501 intersection points and optional, additional B{C{Vector}}

502 keyword arguments, otherwise B{C{start1}}'s (sub-)class.

503

504 @return: An L{Intersection3Tuple}C{(point, outside1, outside2)} with

505 C{point} an instance of B{C{Vector}} or B{C{start1}}'s (sub-)class.

506

507 @note: The C{outside} values is C{0} for lines specified by point and bearing.

508

509 @raise IntersectionError: Invalid, skew, non-co-planar or otherwise

510 non-intersecting lines.

511

512 @see: U{Line-line intersection<https://MathWorld.Wolfram.com/Line-LineIntersection.html>}

513 and U{line-line distance<https://MathWorld.Wolfram.com/Line-LineDistance.html>},

514 U{skew lines<https://MathWorld.Wolfram.com/SkewLines.html>} and U{point-line

515 distance<https://MathWorld.Wolfram.com/Point-LineDistance3-Dimensional.html>}.

516 '''

517 try:

518 v, o1, o2 = _intersect3d3(start1, end1, start2, end2, eps=eps, useZ=useZ)

519 except (TypeError, ValueError) as x:

520 raise _xError(x, start1=start1, end1=end1, start2=start2, end2=end2)

521 v = _nVc(v, **_xkwds(Vector_and_kwds, clas=start1.classof,

522 name=intersection3d3.__name__))

523 return Intersection3Tuple(v, o1, o2)

524

525

526 def intersections2(center1, radius1, center2, radius2, sphere=True, **Vector_and_kwds):

527 '''Compute the intersection of two spheres or circles, each defined by a

528 (3-D) center point and a radius.

529

530 @arg center1: Center of the first sphere or circle (C{Cartesian}, L{Vector3d},

531 C{Vector3Tuple} or C{Vector4Tuple}).

532 @arg radius1: Radius of the first sphere or circle (same units as the

533 B{C{center1}} coordinates).

534 @arg center2: Center of the second sphere or circle (C{Cartesian}, L{Vector3d},

535 C{Vector3Tuple} or C{Vector4Tuple}).

536 @arg radius2: Radius of the second sphere or circle (same units as the

537 B{C{center1}} and B{C{center2}} coordinates).

538 @kwarg sphere: If C{True} compute the center and radius of the intersection of

539 two spheres. If C{False}, ignore the C{z}-component and compute

540 the intersection of two circles (C{bool}).

541 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the

542 intersection points and optional, additional B{C{Vector}}

543 keyword arguments, otherwise B{C{center1}}'s (sub-)class.

544

545 @return: If B{C{sphere}} is C{True}, a 2-tuple of the C{center} and C{radius}

546 of the intersection of the I{spheres}. The C{radius} is C{0.0} for

547 abutting spheres (and the C{center} is aka the I{radical center}).

548

549 If B{C{sphere}} is C{False}, a 2-tuple with the two intersection

550 points of the I{circles}. For abutting circles, both points are

551 the same instance, aka the I{radical center}.

552

553 @raise IntersectionError: Concentric, invalid or non-intersecting spheres

554 or circles.

555

556 @raise TypeError: Invalid B{C{center1}} or B{C{center2}}.

557

558 @raise UnitError: Invalid B{C{radius1}} or B{C{radius2}}.

559

560 @see: U{Sphere-Sphere<https://MathWorld.Wolfram.com/Sphere-SphereIntersection.html>} and

561 U{Circle-Circle<https://MathWorld.Wolfram.com/Circle-CircleIntersection.html>}

562 Intersection.

563 '''

564 try:

565 return _intersects2(center1, Radius_(radius1=radius1),

566 center2, Radius_(radius2=radius2), sphere=sphere,

567 clas=center1.classof, **Vector_and_kwds)

568 except (TypeError, ValueError) as x:

569 raise _xError(x, center1=center1, radius1=radius1, center2=center2, radius2=radius2)

570

571

572 def _intersects2(center1, r1, center2, r2, sphere=True, too_d=None, # in CartesianEllipsoidalBase.intersections2,

573 **clas_Vector_and_kwds): # .ellipsoidalBaseDI._intersections2, .formy.intersections2

574 # (INTERNAL) Intersect two spheres or circles, see L{intersections2}

575 # above, separated to allow callers to embellish any exceptions

576

577 def _nV3(x, y, z):

578 v = Vector3d(x, y, z)

579 n = intersections2.__name__

580 return _nVc(v, **_xkwds(clas_Vector_and_kwds, name=n))

581

582 def _xV3(c1, u, x, y):

583 xy1 = x, y, _1_0 # transform to original space

584 return _nV3(fdot(xy1, u.x, -u.y, c1.x),

585 fdot(xy1, u.y, u.x, c1.y), _0_0)

586

587 c1 = _otherV3d(useZ=sphere, center1=center1)

588 c2 = _otherV3d(useZ=sphere, center2=center2)

589

590 if r1 < r2: # r1, r2 == R, r

591 c1, c2 = c2, c1

592 r1, r2 = r2, r1

593

594 m = c2.minus(c1)

595 d = m.length

596 if d < max(r2 - r1, EPS):

597 raise IntersectionError(_near_(_concentric_)) # XXX ConcentricError?

598

599 o = fsum1_(-d, r1, r2) # overlap == -(d - (r1 + r2))

600 # compute intersections with c1 at (0, 0) and c2 at (d, 0), like

601 # <https://MathWorld.Wolfram.com/Circle-CircleIntersection.html>

602 if o > EPS: # overlapping, r1, r2 == R, r

603 x = _MODS.formy._radical2(d, r1, r2).xline

604 y = _1_0 - (x / r1)**2

605 if y > EPS:

606 y = r1 * sqrt(y) # y == a / 2

607 elif y < 0: # PYCHOK no cover

608 raise IntersectionError(_negative_)

609 else: # abutting

610 y = _0_0

611 elif o < 0: # PYCHOK no cover

612 t = d if too_d is None else too_d

613 raise IntersectionError(_too_(Fmt.distant(t)))

614 else: # abutting

615 x, y = r1, _0_0

616

617 u = m.unit()

618 if sphere: # sphere center and radius

619 c = c1 if x < EPS else (

620 c2 if x > EPS1 else c1.plus(u.times(x)))

621 t = _nV3(c.x, c.y, c.z), Radius(y)

622

623 elif y > 0: # intersecting circles

624 t = _xV3(c1, u, x, y), _xV3(c1, u, x, -y)

625 else: # abutting circles

626 t = _xV3(c1, u, x, 0)

627 t = t, t

628 return t

629

630

631 def iscolinearWith(point, point1, point2, eps=EPS, useZ=True):

632 '''Check whether a point is colinear with two other (2- or 3-D) points.

633

634 @arg point: The point (L{Vector3d}, C{Vector3Tuple} or C{Vector4Tuple}).

635 @arg point1: First point (L{Vector3d}, C{Vector3Tuple} or C{Vector4Tuple}).

636 @arg point2: Second point (L{Vector3d}, C{Vector3Tuple} or C{Vector4Tuple}).

637 @kwarg eps: Tolerance (C{scalar}), same units as C{x}, C{y} and C{z}.

638 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).

639

640 @return: C{True} if B{C{point}} is colinear B{C{point1}} and B{C{point2}},

641 C{False} otherwise.

642

643 @raise TypeError: Invalid B{C{point}}, B{C{point1}} or B{C{point2}}.

644

645 @see: Function L{nearestOn}.

646 '''

647 p = _otherV3d(useZ=useZ, point=point)

648 return _MODS.vector2d._iscolinearWith(p, point1, point2, eps=eps, useZ=useZ)

649

650

651 def nearestOn(point, point1, point2, within=True, useZ=True, Vector=None, **Vector_kwds):

652 '''Locate the point between two points closest to a reference (2- or 3-D).

653

654 @arg point: Reference point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}

655 or C{Vector4Tuple}).

656 @arg point1: Start point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or

657 C{Vector4Tuple}).

658 @arg point2: End point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or

659 C{Vector4Tuple}).

660 @kwarg within: If C{True} return the closest point between both given

661 points, otherwise the closest point on the extended line

662 through both points (C{bool}).

663 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).

664 @kwarg Vector: Class to return closest point (C{Cartesian}, L{Vector3d}

665 or C{Vector3Tuple}) or C{None}.

666 @kwarg Vector_kwds: Optional, additional B{C{Vector}} keyword arguments,

667 ignored if C{B{Vector} is None}.

668

669 @return: Closest point, either B{C{point1}} or B{C{point2}} or an instance

670 of the B{C{point}}'s (sub-)class or B{C{Vector}} if not C{None}.

671

672 @raise TypeError: Invalid B{C{point}}, B{C{point1}} or B{C{point2}}.

673

674 @see: U{3-D Point-Line Distance<https://MathWorld.Wolfram.com/Point-LineDistance3-Dimensional.html>},

675 C{Cartesian} and C{LatLon} methods C{nearestOn}, method L{sphericalTrigonometry.LatLon.nearestOn3}

676 and function L{sphericalTrigonometry.nearestOn3}.

677 '''

678 p0 = _otherV3d(useZ=useZ, point =point)

679 p1 = _otherV3d(useZ=useZ, point1=point1)

680 p2 = _otherV3d(useZ=useZ, point2=point2)

681

682 n = nearestOn.__name__

683 p, _ = _nearestOn2(p0, p1, p2, within=within)

684 if Vector is not None:

685 p = Vector(p.x, p.y, **_xkwds(Vector_kwds, z=p.z, name=n))

686 elif p is p1:

687 p = point1

688 elif p is p2:

689 p = point2

690 else: # ignore Vector_kwds

691 p = point.classof(p.x, p.y, _xkwds_get(Vector_kwds, z=p.z), name=n)

692 return p

693

694

695 def _nearestOn2(p0, p1, p2, within=True, eps=EPS):

696 # (INTERNAL) Closest point and fraction, see L{nearestOn} above,

697 # separated to allow callers to embellish any exceptions

698 p21 = p2.minus(p1)

699 d2 = p21.length2

700 if d2 < eps: # coincident

701 p = p1 # ~= p2

702 t = 0

703 else: # see comments in .points.nearestOn5

704 t = p0.minus(p1).dot(p21) / d2

705 if within and t < eps:

706 p = p1

707 t = 0

708 elif within and t > (_1_0 - eps):

709 p = p2

710 t = 1

711 else:

712 p = p1.plus(p21.times(t))

713 return NearestOn2Tuple(p, t)

714

715

716 def nearestOn6(point, points, closed=False, useZ=True, **Vector_and_kwds): # eps=EPS

717 '''Locate the point on a path or polygon closest to a reference point.

718

719 The closest point on each polygon edge is either the nearest of that

720 edge's end points or a point in between.

721

722 @arg point: Reference point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or

723 C{Vector4Tuple}).

724 @arg points: The path or polygon points (C{Cartesian}, L{Vector3d},

725 C{Vector3Tuple} or C{Vector4Tuple}[]).

726 @kwarg closed: Optionally, close the path or polygon (C{bool}).

727 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).

728 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the closest

729 point and optional, additional B{C{Vector}} keyword

730 arguments, otherwise B{C{point}}'s (sub-)class.

731

732 @return: A L{NearestOn6Tuple}C{(closest, distance, fi, j, start, end)} with the

733 C{closest}, the C{start} and the C{end} point each an instance of the

734 B{C{Vector}} keyword argument of if {B{Vector}=None} or not specified,

735 an instance of the reference B{C{point}}'s (sub-)class.

736

737 @raise PointsError: Insufficient number of B{C{points}}

738

739 @raise TypeError: Non-cartesian B{C{point}} and B{C{points}}.

740

741 @note: Distances measured with method L{Vector3d.equirectangular}. For

742 geodetic distances use function L{nearestOn5} or one of the

743 C{LatLon.nearestOn6} methods.

744 '''

745 r = _otherV3d(useZ=useZ, point=point)

746 D2 = r.equirectangular # distance squared

747

748 Ps = PointsIter(points, loop=1, name=nearestOn6.__name__)

749 p1 = c = s = e = _otherV3d(useZ=useZ, i=0, points=Ps[0])

750 c2 = D2(c) # == r.minus(c).length2

751

752 f = i = 0 # p1..p2 == points[i]..[j]

753 for j, p2 in Ps.enumerate(closed=closed):

754 p2 = _otherV3d(useZ=useZ, i=j, points=p2)

755 p, t = _nearestOn2(r, p1, p2) # within=True, eps=EPS

756 d2 = D2(p) # == r.minus(p).length2

757 if d2 < c2:

758 c2, c, s, e, f = d2, p, p1, p2, (i + t)

759 p1, i = p2, j

760

761 f, j = _fi_j2(f, len(Ps)) # like .ellipsoidalBaseDI._nearestOn2_

762

763 kwds = _xkwds(Vector_and_kwds, clas=point.classof, name=Ps.name)

764 v = _nVc(c, **kwds)

765 s = _nVc(s, **kwds) if s is not c else v

766 e = _nVc(e, **kwds) if e is not c else v

767 return NearestOn6Tuple(v, sqrt(c2), f, j, s, e)

768

769

770 def _nVc(v, clas=None, name=NN, Vector=None, **Vector_kwds): # in .vector2d

771 # return a named C{Vector} or C{clas} instance

772 if Vector is not None:

773 v = Vector(v.x, v.y, v.z, **Vector_kwds)

774 elif clas is not None:

775 v = clas(v.x, v.y, v.z) # ignore Vector_kwds

776 return _xnamed(v, name) if name else v

777

778

779 def _otherV3d(useZ=True, NN_OK=True, i=None, **name_v): # in .CartesianEllipsoidalBase.intersections2,

780 # check named vector instance, return Vector3d .Ellipsoid.height4, .formy.hartzell, .vector2d

781 def _name_i(name, i):

782 return name if i is None else Fmt.SQUARE(name, i)

783

784 name, v = _xkwds_popitem(name_v)

785 if useZ and isinstance(v, Vector3dBase):

786 return v if NN_OK or v.name else v.copy(name=_name_i(name, i))

787 try:

788 return Vector3d(v.x, v.y, (v.z if useZ else INT0), name=_name_i(name, i))

789 except AttributeError: # no .x, .y or .z attr

790 pass

791 raise _xotherError(Vector3d(0, 0, 0), v, name=_name_i(name, i), up=2)

792

793

794 def parse3d(str3d, sep=_COMMA_, Vector=Vector3d, **Vector_kwds):

795 '''Parse an C{"x, y, z"} string.

796

797 @arg str3d: X, y and z values (C{str}).

798 @kwarg sep: Optional separator (C{str}).

799 @kwarg Vector: Optional class (L{Vector3d}).

800 @kwarg Vector_kwds: Optional B{C{Vector}} keyword arguments,

801 ignored if C{B{Vector} is None}.

802

803 @return: A B{C{Vector}} instance or if B{C{Vector}} is C{None},

804 a named L{Vector3Tuple}C{(x, y, z)}.

805

806 @raise VectorError: Invalid B{C{str3d}}.

807 '''

808 try:

809 v = [float(v.strip()) for v in str3d.split(sep)]

810 n = len(v)

811 if n != 3:

812 raise _ValueError(len=n)

813 except (TypeError, ValueError) as x:

814 raise VectorError(str3d=str3d, cause=x)

815 return _xnamed((Vector3Tuple(v) if Vector is None else # *v

816 Vector(*v, **Vector_kwds)), parse3d.__name__)

817

818

819 def sumOf(vectors, Vector=Vector3d, **Vector_kwds):

820 '''Compute the I{vectorial} sum of two oe more vectors.

821

822 @arg vectors: Vectors to be added (L{Vector3d}[]).

823 @kwarg Vector: Optional class for the vectorial sum (L{Vector3d}).

824 @kwarg Vector_kwds: Optional B{C{Vector}} keyword arguments,

825 ignored if C{B{Vector} is None}.

826

827 @return: Vectorial sum as B{C{Vector}} or if B{C{Vector}} is

828 C{None}, a named L{Vector3Tuple}C{(x, y, z)}.

829

830 @raise VectorError: No B{C{vectors}}.

831 '''

832 try:

833 t = _MODS.nvectorBase._nsumOf(vectors, 0, None, {}) # no H

834 except (TypeError, ValueError) as x:

835 raise VectorError(vectors=vectors, Vector=Vector, cause=x)

836 x, y, z = t[:3]

837 n = sumOf.__name__

838 return Vector3Tuple(x, y, z, name=n) if Vector is None else \

839 Vector(x, y, z, **_xkwds(Vector_kwds, name=n))

840

841

842 def trilaterate2d2(x1, y1, radius1, x2, y2, radius2, x3, y3, radius3,

843 eps=None, **Vector_and_kwds):

844 '''Trilaterate three circles, each given as a (2-D) center and a radius.

845

846 @arg x1: Center C{x} coordinate of the 1st circle (C{scalar}).

847 @arg y1: Center C{y} coordinate of the 1st circle (C{scalar}).

848 @arg radius1: Radius of the 1st circle (C{scalar}).

849 @arg x2: Center C{x} coordinate of the 2nd circle (C{scalar}).

850 @arg y2: Center C{y} coordinate of the 2nd circle (C{scalar}).

851 @arg radius2: Radius of the 2nd circle (C{scalar}).

852 @arg x3: Center C{x} coordinate of the 3rd circle (C{scalar}).

853 @arg y3: Center C{y} coordinate of the 3rd circle (C{scalar}).

854 @arg radius3: Radius of the 3rd circle (C{scalar}).

855 @kwarg eps: Tolerance to check the trilaterated point I{delta} on all

856 3 circles (C{scalar}) or C{None} for no checking.

857 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the

858 trilateration and optional, additional B{C{Vector}}

859 keyword arguments, otherwise (L{Vector3d}).

860

861 @return: Trilaterated point as C{B{Vector}(x, y, **B{Vector_kwds})}

862 or L{Vector2Tuple}C{(x, y)} if C{B{Vector} is None}..

863

864 @raise IntersectionError: No intersection, near-concentric or -colinear

865 centers, trilateration failed some other way

866 or the trilaterated point is off one circle

867 by more than B{C{eps}}.

868

869 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}.

870

871 @see: U{Issue #49<https://GitHub.com/mrJean1/PyGeodesy/issues/49>},

872 U{Find X location using 3 known (X,Y) location using trilateration

873 <https://math.StackExchange.com/questions/884807>} and function

874 L{pygeodesy.trilaterate3d2}.

875 '''

876 return _MODS.vector2d._trilaterate2d2(x1, y1, radius1,

877 x2, y2, radius2,

878 x3, y3, radius3, eps=eps, **Vector_and_kwds)

879

880

881 def trilaterate3d2(center1, radius1, center2, radius2, center3, radius3,

882 eps=EPS, **Vector_and_kwds):

883 '''Trilaterate three spheres, each given as a (3-D) center and a radius.

884

885 @arg center1: Center of the 1st sphere (C{Cartesian}, L{Vector3d},

886 C{Vector3Tuple} or C{Vector4Tuple}).

887 @arg radius1: Radius of the 1st sphere (same C{units} as C{x}, C{y}

888 and C{z}).

889 @arg center2: Center of the 2nd sphere (C{Cartesian}, L{Vector3d},

890 C{Vector3Tuple} or C{Vector4Tuple}).

891 @arg radius2: Radius of this sphere (same C{units} as C{x}, C{y}

892 and C{z}).

893 @arg center3: Center of the 3rd sphere (C{Cartesian}, L{Vector3d},

894 C{Vector3Tuple} or C{Vector4Tuple}).

895 @arg radius3: Radius of the 3rd sphere (same C{units} as C{x}, C{y}

896 and C{z}).

897 @kwarg eps: Pertubation tolerance (C{scalar}), same units as C{x},

898 C{y} and C{z} or C{None} for no pertubations.

899 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the

900 trilateration and optional, additional B{C{Vector}}

901 keyword arguments, otherwise B{C{center1}}'s

902 (sub-)class.

903

904 @return: 2-Tuple with two trilaterated points, each a B{C{Vector}}

905 instance. Both points are the same instance if all three

906 spheres abut/intersect in a single point.

907

908 @raise ImportError: Package C{numpy} not found, not installed or

909 older than version 1.10.

910

911 @raise IntersectionError: Near-concentric, -colinear, too distant or

912 non-intersecting spheres.

913

914 @raise NumPyError: Some C{numpy} issue.

915

916 @raise TypeError: Invalid B{C{center1}}, B{C{center2}} or B{C{center3}}.

917

918 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}.

919

920 @see: Norrdine, A. U{I{An Algebraic Solution to the Multilateration

921 Problem}<https://www.ResearchGate.net/publication/275027725>},

922 the U{I{implementation}<https://www.ResearchGate.net/publication/

923 288825016>} and function L{pygeodesy.trilaterate2d2}.

924 '''

925 try:

926 return _MODS.vector2d._trilaterate3d2(_otherV3d(center1=center1, NN_OK=False),

927 Radius_(radius1=radius1, low=eps),

928 center2, radius2, center3, radius3, eps=eps,

929 clas=center1.classof, **Vector_and_kwds)

930 except (AssertionError, TypeError, ValueError) as x:

931 raise _xError(x, center1=center1, radius1=radius1,

932 center2=center2, radius2=radius2,

933 center3=center3, radius3=radius3)

934

935

936 def _xyzhdn3(xyz, height, datum, ll): # in .cartesianBase, .nvectorBase

937 '''(INTERNAL) Get a C{(h, d, name)} 3-tuple.

938 '''

939 h = height or _xattr(xyz, height=None) \

940 or _xattr(xyz, h=None) \

941 or _xattr(ll, height=None)

942

943 d = datum or _xattr(xyz, datum=None) \

944 or _xattr(ll, datum=None)

945

946 return h, d, _xattr(xyz, name=NN)

947

948

949__all__ += _ALL_DOCS(intersections2, sumOf, Vector3dBase)

950

951# **) MIT License

952#

953# Copyright (C) 2016-2023 -- mrJean1 at Gmail -- All Rights Reserved.

954#

955# Permission is hereby granted, free of charge, to any person obtaining a

956# copy of this software and associated documentation files (the "Software"),

957# to deal in the Software without restriction, including without limitation

958# the rights to use, copy, modify, merge, publish, distribute, sublicense,

959# and/or sell copies of the Software, and to permit persons to whom the

960# Software is furnished to do so, subject to the following conditions:

961#

962# The above copyright notice and this permission notice shall be included

963# in all copies or substantial portions of the Software.

964#

965# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS

966# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

967# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL

968# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR

969# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,

970# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

971# OTHER DEALINGS IN THE SOFTWARE.