Coverage for pygeodesy/vector3d.py: 97%

233 statements  

« prev     ^ index     » next       coverage.py v7.6.1, created at 2024-11-12 16:17 -0500

1 

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

3 

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

5 

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

7L{trilaterate3d2}. 

8''' 

9 

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

11 _0_0, _1_0 

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

13 _xattr, _xError, _xkwds, _xkwds_get, _xkwds_item2 

14from pygeodesy.fmath import euclid, fabs, fdot, hypot, sqrt 

15# from pygeodesy.fsums import fsum1_ # from _MODS 

16# from pygeodesy.formy import _radical2 # _MODS 

17from pygeodesy.interns import _COMMA_, _concentric_, _intersection_, \ 

18 _near_, _negative_, _no_, _too_ 

19from pygeodesy.iters import PointsIter, Fmt 

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

21from pygeodesy.named import _name__, _name2__, _xnamed, _xotherError 

22from pygeodesy.namedTuples import Intersection3Tuple, NearestOn2Tuple, \ 

23 NearestOn6Tuple, Vector3Tuple # Vector4Tuple 

24# from pygeodesy.nvectorBase import _nsumOf # _MODS 

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

26from pygeodesy.units import _fi_j2, _isDegrees, Radius, Radius_ 

27from pygeodesy.utily import atan2b, sincos2d 

28# import pygeodesy.vector2d as _vector2d # _MODS.into 

29from pygeodesy.vector3dBase import Vector3dBase 

30 

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

32 

33__all__ = _ALL_LAZY.vector3d 

34__version__ = '24.11.11' 

35 

36_vector2d = _MODS.into(vector2d=__name__) 

37 

38 

39class Vector3d(Vector3dBase): 

40 '''Extended 3-D vector. 

41 

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

43 - n-vector, the normal to a point on the earth's surface 

44 - Earth-Centered, Earth-Fixed (ECEF) cartesian (== spherical n-vector) 

45 - great circle normal to the vector 

46 - motion vector on the earth's surface 

47 - etc. 

48 ''' 

49 

50 def bearing(self, useZ=True): 

51 '''Get this vector's "bearing", the angle off the +Z axis, clockwise. 

52 

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

54 Z component and consider the +Y as the +Z axis. 

55 

56 @return: Bearing (compass C{degrees}). 

57 ''' 

58 x, y = self.x, self.y 

59 if useZ: 

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

61 return atan2b(x, y) 

62 

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

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

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

66 

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

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

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

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

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

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

73 

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

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

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

77 two given points. 

78 

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

80 than version 1.10. 

81 

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

83 a trilateration or C{numpy} issue. 

84 

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

86 

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

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

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

90 ''' 

91 try: 

92 return _vector2d._circin6(self, point2, point3, eps=eps, useZ=True) 

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

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

95 

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

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

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

99 

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

101 or C{Vector4Tuple}). 

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

103 or C{Vector4Tuple}). 

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

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

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

107 

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

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

110 given points. 

111 

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

113 version 1.10. 

114 

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

116 or a trilateration or C{numpy} issue. 

117 

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

119 

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

121 ''' 

122 try: 

123 return _vector2d._circum3(self, point2, point3, circum=circum, 

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

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

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

127 

128 def circum4_(self, *points): 

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

130 

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

132 or C{Vector4Tuple}). 

133 

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

135 an instance if this (sub-)class. 

136 

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

138 older than version 1.10. 

139 

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

141 

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

143 

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

145 

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

147 ''' 

148 return _vector2d.circum4_(self, *points, useZ=True, Vector=self.classof) 

149 

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

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

152 

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

154 or C{Vector4Tuple}). 

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

156 or C{Vector4Tuple}). 

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

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

159 

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

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

162 

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

164 

165 @see: Method L{nearestOn}. 

166 ''' 

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

168 return _vector2d._iscolinearWith(v, point1, point2, eps=eps) 

169 

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

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

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

173 

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

175 or C{Vector4Tuple}). 

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

177 or C{Vector4Tuple}). 

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

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

180 

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

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

183 

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

185 

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

187 

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

189 ''' 

190 try: 

191 return _vector2d._meeus2(self, point2, point3, circum, clas=self.classof) 

192 except (TypeError, ValueError) as x: 

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

194 

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

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

197 

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

199 C{Vector4Tuple}). 

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

201 C{Vector4Tuple}). 

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

203 points, otherwise the closest point on the extended 

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

205 

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

207 of this (sub-)class. 

208 

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

210 

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

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

213 ''' 

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

215 

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

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

218 

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

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

221 

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

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

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

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

226 

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

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

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

230 

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

232 

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

234 

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

236 

237 @see: Function L{nearestOn6}. 

238 ''' 

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

240 

241 def parse(self, str3d, sep=_COMMA_, **name): 

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

243 

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

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

246 @kwarg name: Optional instance C{B{name}=NN} (C{str}), overriding this name. 

247 

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

249 

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

251 ''' 

252 return parse3d(str3d, sep=sep, Vector=self.classof, name=self._name__(name)) 

253 

254 def radii11(self, point2, point3): 

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

256 circles of a (3-D) triangle. 

257 

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

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

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

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

262 

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

264 

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

266 

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

268 

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

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

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

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

273 ''' 

274 try: 

275 return _vector2d._radii11ABC4(self, point2, point3, useZ=True)[0] 

276 except (TypeError, ValueError) as x: 

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

278 

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

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

281 (3-D) triangle. 

282 

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

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

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

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

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

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

289 

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

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

292 three given points. 

293 

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

295 than version 1.10. 

296 

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

298 a trilateration or C{numpy} issue. 

299 

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

301 

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

303 ''' 

304 return _vector2d.soddy4(self, point2, point3, eps=eps, useZ=True) 

305 

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

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

308 and a radius. 

309 

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

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

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

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

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

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

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

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

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

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

320 

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

322 

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

324 centers, trilateration failed some other way 

325 or the trilaterated point is off one circle 

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

327 

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

329 

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

331 

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

333 ''' 

334 

335 def _xyr3(r, **name_v): 

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

337 return v.x, v.y, r 

338 

339 try: 

340 return _vector2d._trilaterate2d2(*(_xyr3(radius, center=self) + 

341 _xyr3(radius2, center2=center2) + 

342 _xyr3(radius3, center3=center3)), 

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

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

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

346 center2=center2, radius2=radius2, 

347 center3=center3, radius3=radius3) 

348 

349 def trilaterate3d2(self, radius, center2, radius2, center3, radius3, eps=EPS4): 

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

351 and a radius. 

352 

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

354 and C{z}). 

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

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

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

358 and C{z}). 

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

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

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

362 and C{z}). 

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

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

365 

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

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

368 spheres intersect or abut in a single point. 

369 

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

371 older than version 1.10. 

372 

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

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

375 

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

377 

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

379 

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

381 

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

383 version 1.10 or later. 

384 

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

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

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

388 ''' 

389 try: 

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

391 return _vector2d._trilaterate3d2(c1, Radius_(radius, low=eps), 

392 center2, radius2, 

393 center3, radius3, 

394 eps=eps, clas=self.classof) 

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

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

397 center2=center2, radius2=radius2, 

398 center3=center3, radius3=radius3) 

399 

400 

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

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

403 # separated to allow callers to embellish any exceptions 

404 

405 def _corners2(s1, b1, s2, useZ): 

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

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

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

409 dx, dy, d = s2.minus(s1).xyz3 

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

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

412 s, c = sincos2d(b1) 

413 if s and c: 

414 dx *= c / s 

415 dy *= s / c 

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

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

418 else: # orthogonal 

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

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

421 return s1, e1 

422 

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

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

425 

426 s1 = t = _otherV3d(useZ=useZ, start1=start1) 

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

428 b1 = _isDegrees(end1) 

429 if b1: # bearing, make an e1 

430 s1, e1 = _corners2(s1, end1, s2, useZ) 

431 else: 

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

433 b2 = _isDegrees(end2) 

434 if b2: # bearing, make an e2 

435 s2, e2 = _corners2(s2, end2, t, useZ) 

436 else: 

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

438 

439 a = e1.minus(s1) 

440 b = e2.minus(s2) 

441 c = s2.minus(s1) 

442 

443 ab = a.cross(b) 

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

445 e = max(EPS0, eps or _0_0) 

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

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

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

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

450 

451 # co-planar, non-skew lines 

452 ab2 = ab.length2 

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

454 x = a.length2 > b.length2 

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

456 a, b = b, a 

457 s1, s2 = s2, s1 

458 e1, e2 = e2, e1 

459 b1, b2 = b2, b1 

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

461 if c.length < e: 

462 return s1, 0, 0 

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

464 return e1, 0, 0 

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

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

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

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

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

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

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

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

473 

474 cb = c.cross(b) 

475 t = cb.dot(ab) 

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

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

478 t = v.minus(s2).dot(b) 

479 o2 = 0 if b2 else _outside(t, b.length2, 2) 

480 return v, o1, o2 

481 

482 

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

484 **Vector_and_kwds): 

485 '''Compute the intersection point of two (2- or 3-D) lines, each defined 

486 by two 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 intersection 

501 points and optional, additional B{C{Vector}} keyword arguments, 

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

503 

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

505 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 non-intersecting lines. 

510 

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

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

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

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

515 ''' 

516 try: 

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

518 except (TypeError, ValueError) as x: 

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

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

521 name=intersection3d3.__name__)) 

522 return Intersection3Tuple(v, o1, o2) 

523 

524 

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

526 '''Compute the intersection of two spheres or circles, each defined by a (3-D) 

527 center point and a radius. 

528 

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

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

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

532 B{C{center1}} coordinates). 

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

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

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

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

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

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

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

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

541 points and optionally, additional B{C{Vector}} keyword arguments, 

542 otherwise B{C{center1}}'s (sub-)class. 

543 

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

545 intersection of the I{spheres}. For abutting circles, C{radius} is C{0.0} 

546 and C{center} is the I{radical center}. 

547 

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

549 I{circles}. For abutting circles, both points are the same instance, aka 

550 the I{radical center}. 

551 

552 @raise IntersectionError: Concentric, invalid or non-intersecting spheres or circles. 

553 

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

555 

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

557 

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

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

560 Intersection. 

561 ''' 

562 try: 

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

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

565 clas=center1.classof, **Vector_and_kwds) 

566 except (TypeError, ValueError) as x: 

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

568 

569 

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

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

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

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

574 

575 def _nV3(x, y, z): 

576 v = Vector3d(x, y, z) 

577 n = intersections2.__name__ 

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

579 

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

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

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

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

584 

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

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

587 

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

589 c1, c2 = c2, c1 

590 r1, r2 = r2, r1 

591 

592 m = c2.minus(c1) 

593 d = m.length 

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

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

596 

597 o = _MODS.fsums.fsum1_(-d, r1, r2) # overlap == -(d - (r1 + r2)) 

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

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

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

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

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

603 if y > EPS: 

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

605 elif y < 0: # PYCHOK no cover 

606 raise IntersectionError(_negative_) 

607 else: # abutting 

608 y = _0_0 

609 elif o < 0: # PYCHOK no cover 

610 if too_d is not None: 

611 d = too_d 

612 raise IntersectionError(_too_(Fmt.distant(d))) 

613 else: # abutting 

614 x, y = r1, _0_0 

615 

616 u = m.unit() 

617 if sphere: # sphere center and radius 

618 c = c1 if x < EPS else ( 

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

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

621 

622 elif y > 0: # intersecting circles 

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

624 else: # abutting circles 

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

626 t = t, t 

627 return t 

628 

629 

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

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

632 

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

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

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

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

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

638 

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

640 otherwise. 

641 

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

643 

644 @see: Function L{nearestOn}. 

645 ''' 

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

647 return _vector2d._iscolinearWith(p, point1, point2, eps=eps, useZ=useZ) 

648 

649 

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

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

652 

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

654 or C{Vector4Tuple}). 

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

656 C{Vector4Tuple}). 

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

658 C{Vector4Tuple}). 

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

660 points, otherwise the closest point on the extended line 

661 through both points (C{bool}). 

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

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

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

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

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

667 

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

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

670 

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

672 

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

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

675 and function L{sphericalTrigonometry.nearestOn3}. 

676 ''' 

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

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

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

680 

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

682 if Vector is not None: 

683 p = Vector(p.x, p.y, **_xkwds(Vector_kwds, z=p.z, name__=nearestOn)) 

684 elif p is p1: 

685 p = point1 

686 elif p is p2: 

687 p = point2 

688 else: # ignore Vector_kwds 

689 p = point.classof(p.x, p.y, _xkwds_get(Vector_kwds, z=p.z), name__=nearestOn) 

690 return p 

691 

692 

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

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

695 # separated to allow callers to embellish any exceptions 

696 p21 = p2.minus(p1) 

697 d2 = p21.length2 

698 if d2 < eps: # coincident 

699 p = p1 # ~= p2 

700 t = 0 

701 else: # see comments in .points.nearestOn5 

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

703 if within and t < eps: 

704 p = p1 

705 t = 0 

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

707 p = p2 

708 t = 1 

709 else: 

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

711 return NearestOn2Tuple(p, t) 

712 

713 

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

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

716 

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

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

719 

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

721 C{Vector4Tuple}). 

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

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

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

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

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

727 point and optionally, additional B{C{Vector}} keyword arguments, 

728 otherwise B{C{point}}'s (sub-)class. 

729 

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

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

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

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

734 

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

736 

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

738 

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

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

741 C{LatLon.nearestOn6} methods. 

742 ''' 

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

744 D2 = r.equirectangular # distance squared 

745 

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

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

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

749 

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

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

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

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

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

755 if d2 < c2: 

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

757 p1, i = p2, j 

758 

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

760 

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

762 v = _nVc(c, **kwds) 

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

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

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

766 

767 

768def _nVc(v, clas=None, Vector=None, **name_Vector_kwds): # in .vector2d 

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

770 name, kwds = _name2__(**name_Vector_kwds) 

771 if Vector is not None: 

772 v = Vector(v.x, v.y, v.z, **kwds) 

773 elif clas is not None: 

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

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

776 

777 

778def _otherV3d(useZ=True, NN_OK=True, i=None, **name_vector): 

779 # check named vector instance, return Vector3d 

780 n, v = _xkwds_item2(name_vector) 

781 n = Fmt.INDEX(n, i) 

782 if useZ and isinstance(v, Vector3dBase): 

783 return v if NN_OK or v.name else v.copy(name=n) 

784 try: 

785 return Vector3d(v.x, v.y, (v.z if useZ else INT0), name=n) 

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

787 pass 

788 raise _xotherError(Vector3d(0, 0, 0), v, name=n, up=2) 

789 

790 

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

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

793 

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

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

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

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

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

799 

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

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

802 

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

804 ''' 

805 try: 

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

807 n = len(v) 

808 if n != 3: 

809 raise _ValueError(len=n) 

810 except (TypeError, ValueError) as x: 

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

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

813 Vector(*v, **Vector_kwds)), name__=parse3d) # .__name__ 

814 

815 

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

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

818 

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

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

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

822 if C{B{Vector} is None}. 

823 

824 @return: Vectorial sum as B{C{Vector}} or if B{C{Vector} is None}, 

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

826 

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

828 ''' 

829 try: 

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

831 except (TypeError, ValueError) as x: 

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

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

834 return Vector3Tuple(x, y, z, name__=sumOf) if Vector is None else \ 

835 Vector(x, y, z, **_xkwds(Vector_kwds, name__=sumOf)) # .__name__ 

836 

837 

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

839 eps=EPS, **Vector_and_kwds): 

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

841 

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

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

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

845 and C{z}). 

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

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

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

849 and C{z}). 

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

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

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

853 and C{z}). 

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

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

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

857 trilateration and optionally, additional B{C{Vector}} 

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

859 

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

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

862 spheres abut/intersect in a single point. 

863 

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

865 than version 1.10. 

866 

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

868 non-intersecting spheres. 

869 

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

871 

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

873 

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

875 

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

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

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

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

880 ''' 

881 try: 

882 return _vector2d._trilaterate3d2(_otherV3d(center1=center1, NN_OK=False), 

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

884 center2, radius2, center3, radius3, eps=eps, 

885 clas=center1.classof, **Vector_and_kwds) 

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

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

888 center2=center2, radius2=radius2, 

889 center3=center3, radius3=radius3) 

890 

891 

892def _xyzhdlln4(xyz, height, datum, ll=None, **name): # in .cartesianBase, .nvectorBase 

893 '''(INTERNAL) Get a C{(h, d, ll, name)} 4-tuple. 

894 ''' 

895 _x = _xattr 

896 h = height or _x(xyz, height=None) or _x(xyz, h=None) or _x(ll, height=None) 

897 d = datum or _x(xyz, datum=None) or _x(ll, datum=None) 

898 return h, d, ll, _name__(name, _or_nameof=ll) 

899 

900 

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

902 

903# **) MIT License 

904# 

905# Copyright (C) 2016-2024 -- mrJean1 at Gmail -- All Rights Reserved. 

906# 

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

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

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

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

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

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

913# 

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

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

916# 

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

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

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

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

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

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

923# OTHER DEALINGS IN THE SOFTWARE.