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} and

7 L{trilaterate3d2}.

8'''

9

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

11 _0_0, _1_0

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

13 _xattr, _xError, _xkwds, _xkwds_get, _xkwds_item2

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

15# from pygeodesy.fsums import fsum1_ # from _MODS

16# from pygeodesy.formy import _radical2 # _MODS

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

18 _near_, _negative_, _no_, _too_

19 from pygeodesy.iters import PointsIter, Fmt

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

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

22 from pygeodesy.namedTuples import Intersection3Tuple, NearestOn2Tuple, \

23 NearestOn6Tuple, _v2Cls, Vector3Tuple # Vector4Tuple

24# from pygeodesy.nvectorBase import _nsumOf # _MODS

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

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

27 from pygeodesy.utily import atan2b, sincos2d

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

29 from pygeodesy.vector3dBase import Vector3dBase

30

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

32

33__all__ = _ALL_LAZY.vector3d

34__version__ = '24.11.22'

35

36_vector2d = _MODS.into(vector2d=__name__)

37

38

39 class 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 toCartesian(self, Cartesian, **Cartesian_kwds):

307 '''Return this C{Vector3d} as a C{Cartesian}.

308

309 @arg Cartesian: The C{Cartesian} class to use.

310 @kwarg Cartesian_kwds: Optional, additional C{Cartesian}

311 keyword arguments.

312

313 @return: The C{B{Cartesian}} instance.

314 '''

315 return _v2Cls(self, Cartesian, Cartesian_kwds)

316

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

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

319 and a radius.

320

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

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

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

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

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

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

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

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

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

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

331

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

333

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

335 centers, trilateration failed some other way

336 or the trilaterated point is off one circle

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

338

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

340

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

342

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

344 '''

345

346 def _xyr3(r, **name_v):

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

348 return v.x, v.y, r

349

350 try:

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

352 _xyr3(radius2, center2=center2) +

353 _xyr3(radius3, center3=center3)),

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

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

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

357 center2=center2, radius2=radius2,

358 center3=center3, radius3=radius3)

359

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

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

362 and a radius.

363

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

365 and C{z}).

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

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

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

369 and C{z}).

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

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

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

373 and C{z}).

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

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

376

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

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

379 spheres intersect or abut in a single point.

380

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

382 older than version 1.10.

383

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

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

386

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

388

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

390

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

392

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

394 version 1.10 or later.

395

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

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

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

399 '''

400 try:

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

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

403 center2, radius2,

404 center3, radius3,

405 eps=eps, clas=self.classof)

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

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

408 center2=center2, radius2=radius2,

409 center3=center3, radius3=radius3)

410

411

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

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

414 # separated to allow callers to embellish any exceptions

415

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

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

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

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

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

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

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

423 s, c = sincos2d(b1)

424 if s and c:

425 dx *= c / s

426 dy *= s / c

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

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

429 else: # orthogonal

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

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

432 return s1, e1

433

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

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

436

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

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

439 b1 = _isDegrees(end1)

440 if b1: # bearing, make an e1

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

442 else:

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

444 b2 = _isDegrees(end2)

445 if b2: # bearing, make an e2

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

447 else:

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

449

450 a = e1.minus(s1)

451 b = e2.minus(s2)

452 c = s2.minus(s1)

453

454 ab = a.cross(b)

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

456 e = max(EPS0, eps or _0_0)

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

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

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

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

461

462 # co-planar, non-skew lines

463 ab2 = ab.length2

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

465 x = a.length2 > b.length2

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

467 a, b = b, a

468 s1, s2 = s2, s1

469 e1, e2 = e2, e1

470 b1, b2 = b2, b1

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

472 if c.length < e:

473 return s1, 0, 0

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

475 return e1, 0, 0

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

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

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

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

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

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

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

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

484

485 cb = c.cross(b)

486 t = cb.dot(ab)

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

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

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

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

491 return v, o1, o2

492

493

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

495 **Vector_and_kwds):

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

497 by two points or by a point and a bearing.

498

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

514

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

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

517

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

519

520 @raise IntersectionError: Invalid, skew, non-co-planar or otherwise non-intersecting lines.

521

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

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

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

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

526 '''

527 try:

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

529 except (TypeError, ValueError) as x:

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

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

532 name=intersection3d3.__name__))

533 return Intersection3Tuple(v, o1, o2)

534

535

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

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

538 center point and a radius.

539

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

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

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

543 B{C{center1}} coordinates).

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

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

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

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

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

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

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

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

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

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

554

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

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

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

558

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

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

561 the I{radical center}.

562

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

564

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

566

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

568

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

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

571 Intersection.

572 '''

573 try:

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

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

576 clas=center1.classof, **Vector_and_kwds)

577 except (TypeError, ValueError) as x:

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

579

580

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

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

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

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

585

586 def _nV3(x, y, z):

587 v = Vector3d(x, y, z)

588 n = intersections2.__name__

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

590

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

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

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

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

595

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

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

598

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

600 c1, c2 = c2, c1

601 r1, r2 = r2, r1

602

603 m = c2.minus(c1)

604 d = m.length

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

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

607

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

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

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

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

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

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

614 if y > EPS:

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

616 elif y < 0: # PYCHOK no cover

617 raise IntersectionError(_negative_)

618 else: # abutting

619 y = _0_0

620 elif o < 0: # PYCHOK no cover

621 if too_d is not None:

622 d = too_d

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

624 else: # abutting

625 x, y = r1, _0_0

626

627 u = m.unit()

628 if sphere: # sphere center and radius

629 c = c1 if x < EPS else (

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

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

632

633 elif y > 0: # intersecting circles

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

635 else: # abutting circles

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

637 t = t, t

638 return t

639

640

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

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

643

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

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

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

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

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

649

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

651 otherwise.

652

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

654

655 @see: Function L{nearestOn}.

656 '''

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

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

659

660

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

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

663

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

665 or C{Vector4Tuple}).

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

667 C{Vector4Tuple}).

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

669 C{Vector4Tuple}).

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

671 points, otherwise the closest point on the extended line

672 through both points (C{bool}).

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

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

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

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

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

678

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

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

681

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

683

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

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

686 and function L{sphericalTrigonometry.nearestOn3}.

687 '''

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

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

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

691

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

693 if Vector is not None:

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

695 elif p is p1:

696 p = point1

697 elif p is p2:

698 p = point2

699 else: # ignore Vector_kwds

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

701 return p

702

703

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

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

706 # separated to allow callers to embellish any exceptions

707 p21 = p2.minus(p1)

708 d2 = p21.length2

709 if d2 < eps: # coincident

710 p = p1 # ~= p2

711 t = 0

712 else: # see comments in .points.nearestOn5

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

714 if within and t < eps:

715 p = p1

716 t = 0

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

718 p = p2

719 t = 1

720 else:

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

722 return NearestOn2Tuple(p, t)

723

724

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

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

727

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

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

730

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

732 C{Vector4Tuple}).

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

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

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

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

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

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

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

740

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

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

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

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

745

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

747

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

749

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

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

752 C{LatLon.nearestOn6} methods.

753 '''

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

755 D2 = r.equirectangular # distance squared

756

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

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

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

760

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

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

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

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

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

766 if d2 < c2:

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

768 p1, i = p2, j

769

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

771

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

773 v = _nVc(c, **kwds)

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

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

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

777

778

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

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

781 name, kwds = _name2__(**name_Vector_kwds)

782 if Vector is not None:

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

784 elif clas is not None:

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

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

787

788

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

790 # check named vector instance, return Vector3d

791 n, v = _xkwds_item2(name_vector)

792 n = Fmt.INDEX(n, i)

793 if useZ and isinstance(v, Vector3dBase):

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

795 try:

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

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

798 pass

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

800

801

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

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

804

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

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

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

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

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

810

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

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

813

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

815 '''

816 try:

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

818 n = len(v)

819 if n != 3:

820 raise _ValueError(len=n)

821 except (TypeError, ValueError) as x:

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

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

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

825

826

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

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

829

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

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

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

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

834

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

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

837

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

839 '''

840 try:

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

842 except (TypeError, ValueError) as x:

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

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

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

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

847

848

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

850 eps=EPS, **Vector_and_kwds):

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

852

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

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

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

856 and C{z}).

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

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

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

860 and C{z}).

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

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

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

864 and C{z}).

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

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

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

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

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

870

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

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

873 spheres abut/intersect in a single point.

874

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

876 than version 1.10.

877

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

879 non-intersecting spheres.

880

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

882

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

884

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

886

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

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

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

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

891 '''

892 try:

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

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

895 center2, radius2, center3, radius3, eps=eps,

896 clas=center1.classof, **Vector_and_kwds)

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

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

899 center2=center2, radius2=radius2,

900 center3=center3, radius3=radius3)

901

902

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

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

905 '''

906 _x = _xattr

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

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

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

910

911

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

913

914# **) MIT License

915#

916# Copyright (C) 2016-2025 -- mrJean1 at Gmail -- All Rights Reserved.

917#

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

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

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

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

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

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

924#

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

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

927#

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

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

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

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

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

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

934# OTHER DEALINGS IN THE SOFTWARE.