Coverage for pygeodesy/vector3d.py: 96%
235 statements
« prev ^ index » next coverage.py v7.2.2, created at 2023-07-12 13:40 -0400
« prev ^ index » next coverage.py v7.2.2, created at 2023-07-12 13:40 -0400
2# -*- coding: utf-8 -*-
4u'''Extended 3-D vector class L{Vector3d} and functions.
6Function L{intersection3d3}, L{intersections2}, L{parse3d}, L{sumOf},
7L{trilaterate2d2} and L{trilaterate3d2}.
8'''
10# from pygeodesy.basics import isscalar # from .fmath
11from pygeodesy.constants import EPS, EPS0, EPS1, EPS4, INT0, isnear0, \
12 _0_0, _1_0
13from pygeodesy.errors import IntersectionError, _ValueError, VectorError, \
14 _xattr, _xError, _xkwds_get, _xkwds, _xkwds_popitem
15from 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
19from pygeodesy.interns import NN, _COMMA_, _concentric_, _intersection_, \
20 _near_, _negative_, _no_, _too_
21from pygeodesy.iters import PointsIter, Fmt
22from pygeodesy.lazily import _ALL_DOCS, _ALL_LAZY, _ALL_MODS as _MODS
23from pygeodesy.named import _xnamed, _xotherError
24from pygeodesy.namedTuples import Intersection3Tuple, NearestOn2Tuple, \
25 NearestOn6Tuple, Vector3Tuple # Vector4Tuple
26# from pygeodesy.nvectorBase import _nsumOf # _MODS
27# from pygeodesy.streprs import Fmt # from .iters
28from pygeodesy.units import _fi_j2, Radius, Radius_
29from pygeodesy.utily import atan2b, sincos2d
30# from pygeodesy.vector2d import .... # in .... below
31from pygeodesy.vector3dBase import Vector3dBase
33# from math import fabs, sqrt # from .fmath
35__all__ = _ALL_LAZY.vector3d
36__version__ = '23.06.05'
39class Vector3d(Vector3dBase):
40 '''Extended 3-D vector.
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 '''
50 def bearing(self, useZ=True):
51 '''Get the "bearing" of this vector.
53 @kwarg useZ: If C{True}, use the Z component, otherwise
54 consider the Y as +Z axis.
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)
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.
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}.
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.
80 @raise ImportError: Package C{numpy} not found, not installed or older
81 than version 1.10.
83 @raise IntersectionError: Near-coincident or -colinear points or
84 a trilateration or C{numpy} issue.
86 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.
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)
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.
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}.
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.
113 @raise ImportError: Package C{numpy} not found, not installed or older than
114 version 1.10.
116 @raise IntersectionError: Near-concentric, -coincident or -colinear points
117 or a trilateration or C{numpy} issue.
119 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.
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)
129 def circum4_(self, *points):
130 '''Best-fit a sphere through this and two or more other (3-D) points.
132 @arg points: Other points (each a C{Cartesian}, L{Vector3d}, C{Vector3Tuple}
133 or C{Vector4Tuple}).
135 @return: L{Circum4Tuple}C{(radius, center, rank, residuals)} with C{center}
136 an instance if this (sub-)class.
138 @raise ImportError: Package C{numpy} not found, not installed or
139 older than version 1.10.
141 @raise NumPyError: Some C{numpy} issue.
143 @raise PointsError: Too few B{C{points}}.
145 @raise TypeError: One of the B{C{points}} invalid.
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)
151 def iscolinearWith(self, point1, point2, eps=EPS):
152 '''Check whether this and two other (3-D) points are colinear.
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}.
161 @return: C{True} if this point is colinear with B{C{point1}} and
162 B{C{point2}}, C{False} otherwise.
164 @raise TypeError: Invalid B{C{point1}} or B{C{point2}}.
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)
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.
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}).
182 @return: L{Meeus2Tuple}C{(radius, Type)}, with C{Type} the C{circumcenter}
183 iff C{B{circum}=True}.
185 @raise IntersectionError: Coincident or colinear points, iff C{B{circum}=True}.
187 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.
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)
196 def nearestOn(self, point1, point2, within=True):
197 '''Locate the point between two points closest to this point.
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}).
207 @return: Closest point, either B{C{point1}} or B{C{point2}} or an instance
208 of this (sub-)class.
210 @raise TypeError: Invalid B{C{point1}} or B{C{point2}}.
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
217 def nearestOn6(self, points, closed=False, useZ=True): # eps=EPS
218 '''Locate the point on a path or polygon closest to this point.
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.
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}).
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.
232 @raise PointsError: Insufficient number of B{C{points}}
234 @raise TypeError: Non-cartesian B{C{points}}.
236 @note: Distances measured with method L{Vector3d.equirectangular}.
238 @see: Function L{nearestOn6}.
239 '''
240 return nearestOn6(self, points, closed=closed, useZ=useZ) # Vector=self.classof
242 def parse(self, str3d, sep=_COMMA_, name=NN):
243 '''Parse an C{"x, y, z"} string to a L{Vector3d} instance.
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.
249 @return: The instance (L{Vector3d}).
251 @raise VectorError: Invalid B{C{str3d}}.
252 '''
253 return parse3d(str3d, sep=sep, Vector=self.classof, name=name or self.name)
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.
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}).
264 @return: L{Radii11Tuple}C{(rA, rB, rC, cR, rIn, riS, roS, a, b, c, s)}.
266 @raise TriangleError: Near-coincident or -colinear points.
268 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.
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)
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.
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}.
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.
295 @raise ImportError: Package C{numpy} not found, not installed or older
296 than version 1.10.
298 @raise IntersectionError: Near-coincident or -colinear points or
299 a trilateration or C{numpy} issue.
301 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}.
303 @see: Function L{pygeodesy.soddy4}.
304 '''
305 return _MODS.vector2d.soddy4(self, point2, point3, eps=eps, useZ=True)
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.
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}).
322 @return: Trilaterated point, an instance of this (sub-)class with C{z=B{z}}.
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}}.
329 @raise TypeError: Invalid B{C{center2}} or B{C{center3}}.
331 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}.
333 @see: Function L{pygeodesy.trilaterate2d2}.
334 '''
336 def _xyr3(r, **name_v):
337 v = _otherV3d(useZ=False, **name_v)
338 return v.x, v.y, r
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)
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.
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.
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.
371 @raise ImportError: Package C{numpy} not found, not installed or
372 older than version 1.10.
374 @raise IntersectionError: Near-concentric, -colinear, too distant or
375 non-intersecting spheres or C{numpy} issue.
377 @raise NumPyError: Some C{numpy} issue.
379 @raise TypeError: Invalid B{C{center2}} or B{C{center3}}.
381 @raise UnitError: Invalid B{C{radius}}, B{C{radius2}} or B{C{radius3}}.
383 @note: Package U{numpy<https://PyPI.org/project/numpy>} is required,
384 version 1.10 or later.
386 @see: Norrdine, A. U{I{An Algebraic Solution to the Multilateration
387 Problem}<https://www.ResearchGate.net/publication/
388 275027725_An_Algebraic_Solution_to_the_Multilateration_Problem>}
389 and U{I{implementation}<https://www.ResearchGate.net/publication/
390 288825016_Trilateration_Matlab_Code>}.
391 '''
392 try:
393 c1 = _otherV3d(center=self, NN_OK=False)
394 return _MODS.vector2d._trilaterate3d2(c1, Radius_(radius, low=eps),
395 center2, radius2,
396 center3, radius3,
397 eps=eps, clas=self.classof)
398 except (AssertionError, TypeError, ValueError) as x:
399 raise _xError(x, center=self, radius=radius,
400 center2=center2, radius2=radius2,
401 center3=center3, radius3=radius3)
404def _intersect3d3(start1, end1, start2, end2, eps=EPS, useZ=False): # MCCABE 16 in .formy.intersection2, .rhumbx._RhumbLine
405 # (INTERNAL) Intersect two lines, see L{intersection3d3} below,
406 # separated to allow callers to embellish any exceptions
408 def _outside(t, d2, o): # -o before start#, +o after end#
409 return -o if t < 0 else (o if t > d2 else 0) # XXX d2 + eps?
411 def _rightangle2(s1, b1, s2, useZ):
412 # Get the C{s1'} and C{e1'}, corners of a right-angle
413 # triangle with the hypotenuse thru C{s1} at bearing
414 # C{b1} and the right angle at C{s2}
415 dx, dy, d = s2.minus(s1).xyz
416 if useZ and not isnear0(d): # not supported
417 raise IntersectionError(useZ=d, bearing=b1)
418 s, c = sincos2d(b1)
419 if s and c:
420 dx *= c / s
421 dy *= s / c
422 e1 = Vector3d(s2.x, s1.y + dx, s1.z)
423 s1 = Vector3d(s1.x + dy, s2.y, s1.z)
424 else: # orthogonal
425 d = euclid(dx, dy) # hypot?
426 e1 = Vector3d(s1.x + s * d, s1.y + c * d, s1.z)
427 return s1, e1
429 s1 = x = _otherV3d(useZ=useZ, start1=start1)
430 s2 = _otherV3d(useZ=useZ, start2=start2)
431 b1 = isscalar(end1)
432 if b1: # bearing, make an e1
433 s1, e1 = _rightangle2(s1, end1, s2, useZ)
434 else:
435 e1 = _otherV3d(useZ=useZ, end1=end1)
436 b2 = isscalar(end2)
437 if b2: # bearing, make an e2
438 s2, e2 = _rightangle2(s2, end2, x, useZ)
439 else:
440 e2 = _otherV3d(useZ=useZ, end2=end2)
442 a = e1.minus(s1)
443 b = e2.minus(s2)
444 c = s2.minus(s1)
446 ab = a.cross(b)
447 d = fabs(c.dot(ab))
448 e = max(EPS0, eps or _0_0)
449 if d > EPS0 and ab.length > e: # PYCHOK no cover
450 d = d / ab.length # /= chokes PyChecker
451 if d > e: # argonic, skew lines distance
452 raise IntersectionError(skew_d=d, txt=_no_(_intersection_))
454 # co-planar, non-skew lines
455 ab2 = ab.length2
456 if ab2 < e: # colinear, parallel or null line(s)
457 x = b.length2 < a.length2
458 if x: # make C{a} the shortest
459 a, b = b, a
460 s1, s2 = s2, s1
461 e1, e2 = e2, e1
462 b1, b2 = b2, b1
463 if b.length2 < e: # PYCHOK no cover
464 if c.length < e:
465 return s1, 0, 0
466 elif e2.minus(e1).length < e:
467 return e1, 0, 0
468 elif a.length2 < e: # null (s1, e1), non-null (s2, e2)
469 # like _nearestOn2(s1, s2, e2, within=False, eps=e)
470 t = s1.minus(s2).dot(b)
471 v = s2.plus(b.times(t / b.length2))
472 if s1.minus(v).length < e:
473 o = 0 if b2 else _outside(t, b.length2, 1 if x else 2)
474 return (v, o, 0) if x else (v, 0, o)
475 raise IntersectionError(length2=ab2, txt=_no_(_intersection_))
477 cb = c.cross(b)
478 t = cb.dot(ab)
479 o1 = 0 if b1 else _outside(t, ab2, 1)
480 v = s1.plus(a.times(t / ab2))
481 o2 = 0 if b2 else _outside(v.minus(s2).dot(b), b.length2, 2)
482 return v, o1, o2
485def intersection3d3(start1, end1, start2, end2, eps=EPS, useZ=True,
486 **Vector_and_kwds):
487 '''Compute the intersection point of two lines, each defined by two
488 points or by a point and a bearing.
490 @arg start1: Start point of the first line (C{Cartesian}, L{Vector3d},
491 C{Vector3Tuple} or C{Vector4Tuple}).
492 @arg end1: End point of the first line (C{Cartesian}, L{Vector3d},
493 C{Vector3Tuple} or C{Vector4Tuple}) or the bearing at
494 B{C{start1}} (compass C{degrees}).
495 @arg start2: Start point of the second line (C{Cartesian}, L{Vector3d},
496 C{Vector3Tuple} or C{Vector4Tuple}).
497 @arg end2: End point of the second line (C{Cartesian}, L{Vector3d},
498 C{Vector3Tuple} or C{Vector4Tuple}) or the bearing at
499 B{C{start2}} (Ccompass C{degrees}).
500 @kwarg eps: Tolerance for skew line distance and length (C{EPS}).
501 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).
502 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the
503 intersection points and optional, additional B{C{Vector}}
504 keyword arguments, otherwise B{C{start1}}'s (sub-)class.
506 @return: An L{Intersection3Tuple}C{(point, outside1, outside2)} with
507 C{point} an instance of B{C{Vector}} or B{C{start1}}'s (sub-)class.
509 @note: The C{outside} values is C{0} for lines specified by point and bearing.
511 @raise IntersectionError: Invalid, skew, non-co-planar or otherwise
512 non-intersecting lines.
514 @see: U{Line-line intersection<https://MathWorld.Wolfram.com/Line-LineIntersection.html>}
515 and U{line-line distance<https://MathWorld.Wolfram.com/Line-LineDistance.html>},
516 U{skew lines<https://MathWorld.Wolfram.com/SkewLines.html>} and U{point-line
517 distance<https://MathWorld.Wolfram.com/Point-LineDistance3-Dimensional.html>}.
518 '''
519 try:
520 v, o1, o2 = _intersect3d3(start1, end1, start2, end2, eps=eps, useZ=useZ)
521 except (TypeError, ValueError) as x:
522 raise _xError(x, start1=start1, end1=end1, start2=start2, end2=end2)
523 v = _nVc(v, **_xkwds(Vector_and_kwds, clas=start1.classof,
524 name=intersection3d3.__name__))
525 return Intersection3Tuple(v, o1, o2)
528def intersections2(center1, radius1, center2, radius2, sphere=True, **Vector_and_kwds):
529 '''Compute the intersection of two spheres or circles, each defined by a
530 (3-D) center point and a radius.
532 @arg center1: Center of the first sphere or circle (C{Cartesian}, L{Vector3d},
533 C{Vector3Tuple} or C{Vector4Tuple}).
534 @arg radius1: Radius of the first sphere or circle (same units as the
535 B{C{center1}} coordinates).
536 @arg center2: Center of the second sphere or circle (C{Cartesian}, L{Vector3d},
537 C{Vector3Tuple} or C{Vector4Tuple}).
538 @arg radius2: Radius of the second sphere or circle (same units as the
539 B{C{center1}} and B{C{center2}} coordinates).
540 @kwarg sphere: If C{True} compute the center and radius of the intersection of
541 two spheres. If C{False}, ignore the C{z}-component and compute
542 the intersection of two circles (C{bool}).
543 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the
544 intersection points and optional, additional B{C{Vector}}
545 keyword arguments, otherwise B{C{center1}}'s (sub-)class.
547 @return: If B{C{sphere}} is C{True}, a 2-tuple of the C{center} and C{radius}
548 of the intersection of the I{spheres}. The C{radius} is C{0.0} for
549 abutting spheres (and the C{center} is aka the I{radical center}).
551 If B{C{sphere}} is C{False}, a 2-tuple with the two intersection
552 points of the I{circles}. For abutting circles, both points are
553 the same instance, aka the I{radical center}.
555 @raise IntersectionError: Concentric, invalid or non-intersecting spheres
556 or circles.
558 @raise TypeError: Invalid B{C{center1}} or B{C{center2}}.
560 @raise UnitError: Invalid B{C{radius1}} or B{C{radius2}}.
562 @see: U{Sphere-Sphere<https://MathWorld.Wolfram.com/Sphere-SphereIntersection.html>} and
563 U{Circle-Circle<https://MathWorld.Wolfram.com/Circle-CircleIntersection.html>}
564 Intersection.
565 '''
566 try:
567 return _intersects2(center1, Radius_(radius1=radius1),
568 center2, Radius_(radius2=radius2), sphere=sphere,
569 clas=center1.classof, **Vector_and_kwds)
570 except (TypeError, ValueError) as x:
571 raise _xError(x, center1=center1, radius1=radius1, center2=center2, radius2=radius2)
574def _intersects2(center1, r1, center2, r2, sphere=True, too_d=None, # in CartesianEllipsoidalBase.intersections2,
575 **clas_Vector_and_kwds): # .ellipsoidalBaseDI._intersections2, .formy.intersections2
576 # (INTERNAL) Intersect two spheres or circles, see L{intersections2}
577 # above, separated to allow callers to embellish any exceptions
579 def _nV3(x, y, z):
580 v = Vector3d(x, y, z)
581 n = intersections2.__name__
582 return _nVc(v, **_xkwds(clas_Vector_and_kwds, name=n))
584 def _xV3(c1, u, x, y):
585 xy1 = x, y, _1_0 # transform to original space
586 return _nV3(fdot(xy1, u.x, -u.y, c1.x),
587 fdot(xy1, u.y, u.x, c1.y), _0_0)
589 c1 = _otherV3d(useZ=sphere, center1=center1)
590 c2 = _otherV3d(useZ=sphere, center2=center2)
592 if r1 < r2: # r1, r2 == R, r
593 c1, c2 = c2, c1
594 r1, r2 = r2, r1
596 m = c2.minus(c1)
597 d = m.length
598 if d < max(r2 - r1, EPS):
599 raise IntersectionError(_near_(_concentric_)) # XXX ConcentricError?
601 o = fsum1_(-d, r1, r2) # overlap == -(d - (r1 + r2))
602 # compute intersections with c1 at (0, 0) and c2 at (d, 0), like
603 # <https://MathWorld.Wolfram.com/Circle-CircleIntersection.html>
604 if o > EPS: # overlapping, r1, r2 == R, r
605 x = _MODS.formy._radical2(d, r1, r2).xline
606 y = _1_0 - (x / r1)**2
607 if y > EPS:
608 y = r1 * sqrt(y) # y == a / 2
609 elif y < 0: # PYCHOK no cover
610 raise IntersectionError(_negative_)
611 else: # abutting
612 y = _0_0
613 elif o < 0: # PYCHOK no cover
614 t = d if too_d is None else too_d
615 raise IntersectionError(_too_(Fmt.distant(t)))
616 else: # abutting
617 x, y = r1, _0_0
619 u = m.unit()
620 if sphere: # sphere center and radius
621 c = c1 if x < EPS else (
622 c2 if x > EPS1 else c1.plus(u.times(x)))
623 t = _nV3(c.x, c.y, c.z), Radius(y)
625 elif y > 0: # intersecting circles
626 t = _xV3(c1, u, x, y), _xV3(c1, u, x, -y)
627 else: # abutting circles
628 t = _xV3(c1, u, x, 0)
629 t = t, t
630 return t
633def iscolinearWith(point, point1, point2, eps=EPS, useZ=True):
634 '''Check whether a point is colinear with two other (2- or 3-D) points.
636 @arg point: The point (L{Vector3d}, C{Vector3Tuple} or C{Vector4Tuple}).
637 @arg point1: First point (L{Vector3d}, C{Vector3Tuple} or C{Vector4Tuple}).
638 @arg point2: Second point (L{Vector3d}, C{Vector3Tuple} or C{Vector4Tuple}).
639 @kwarg eps: Tolerance (C{scalar}), same units as C{x}, C{y} and C{z}.
640 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).
642 @return: C{True} if B{C{point}} is colinear B{C{point1}} and B{C{point2}},
643 C{False} otherwise.
645 @raise TypeError: Invalid B{C{point}}, B{C{point1}} or B{C{point2}}.
647 @see: Function L{nearestOn}.
648 '''
649 p = _otherV3d(useZ=useZ, point=point)
650 return _MODS.vector2d._iscolinearWith(p, point1, point2, eps=eps, useZ=useZ)
653def nearestOn(point, point1, point2, within=True, useZ=True, Vector=None, **Vector_kwds):
654 '''Locate the point between two points closest to a reference (2- or 3-D).
656 @arg point: Reference point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}
657 or C{Vector4Tuple}).
658 @arg point1: Start point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or
659 C{Vector4Tuple}).
660 @arg point2: End point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or
661 C{Vector4Tuple}).
662 @kwarg within: If C{True} return the closest point between both given
663 points, otherwise the closest point on the extended line
664 through both points (C{bool}).
665 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).
666 @kwarg Vector: Class to return closest point (C{Cartesian}, L{Vector3d}
667 or C{Vector3Tuple}) or C{None}.
668 @kwarg Vector_kwds: Optional, additional B{C{Vector}} keyword arguments,
669 ignored if C{B{Vector} is None}.
671 @return: Closest point, either B{C{point1}} or B{C{point2}} or an instance
672 of the B{C{point}}'s (sub-)class or B{C{Vector}} if not C{None}.
674 @raise TypeError: Invalid B{C{point}}, B{C{point1}} or B{C{point2}}.
676 @see: U{3-D Point-Line Distance<https://MathWorld.Wolfram.com/Point-LineDistance3-Dimensional.html>},
677 C{Cartesian} and C{LatLon} methods C{nearestOn}, method L{sphericalTrigonometry.LatLon.nearestOn3}
678 and function L{sphericalTrigonometry.nearestOn3}.
679 '''
680 p0 = _otherV3d(useZ=useZ, point =point)
681 p1 = _otherV3d(useZ=useZ, point1=point1)
682 p2 = _otherV3d(useZ=useZ, point2=point2)
684 n = nearestOn.__name__
685 p, _ = _nearestOn2(p0, p1, p2, within=within)
686 if Vector is not None:
687 p = Vector(p.x, p.y, **_xkwds(Vector_kwds, z=p.z, name=n))
688 elif p is p1:
689 p = point1
690 elif p is p2:
691 p = point2
692 else: # ignore Vector_kwds
693 p = point.classof(p.x, p.y, _xkwds_get(Vector_kwds, z=p.z), name=n)
694 return p
697def _nearestOn2(p0, p1, p2, within=True, eps=EPS):
698 # (INTERNAL) Closest point and fraction, see L{nearestOn} above,
699 # separated to allow callers to embellish any exceptions
700 p21 = p2.minus(p1)
701 d2 = p21.length2
702 if d2 < eps: # coincident
703 p = p1 # ~= p2
704 t = 0
705 else: # see comments in .points.nearestOn5
706 t = p0.minus(p1).dot(p21) / d2
707 if within and t < eps:
708 p = p1
709 t = 0
710 elif within and t > (_1_0 - eps):
711 p = p2
712 t = 1
713 else:
714 p = p1.plus(p21.times(t))
715 return NearestOn2Tuple(p, t)
718def nearestOn6(point, points, closed=False, useZ=True, **Vector_and_kwds): # eps=EPS
719 '''Locate the point on a path or polygon closest to a reference point.
721 The closest point on each polygon edge is either the nearest of that
722 edge's end points or a point in between.
724 @arg point: Reference point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or
725 C{Vector4Tuple}).
726 @arg points: The path or polygon points (C{Cartesian}, L{Vector3d},
727 C{Vector3Tuple} or C{Vector4Tuple}[]).
728 @kwarg closed: Optionally, close the path or polygon (C{bool}).
729 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).
730 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the closest
731 point and optional, additional B{C{Vector}} keyword
732 arguments, otherwise B{C{point}}'s (sub-)class.
734 @return: A L{NearestOn6Tuple}C{(closest, distance, fi, j, start, end)} with the
735 C{closest}, the C{start} and the C{end} point each an instance of the
736 B{C{Vector}} keyword argument of if {B{Vector}=None} or not specified,
737 an instance of the reference B{C{point}}'s (sub-)class.
739 @raise PointsError: Insufficient number of B{C{points}}
741 @raise TypeError: Non-cartesian B{C{point}} and B{C{points}}.
743 @note: Distances measured with method L{Vector3d.equirectangular}. For
744 geodetic distances use function L{nearestOn5} or one of the
745 C{LatLon.nearestOn6} methods.
746 '''
747 r = _otherV3d(useZ=useZ, point=point)
748 D2 = r.equirectangular # distance squared
750 Ps = PointsIter(points, loop=1, name=nearestOn6.__name__)
751 p1 = c = s = e = _otherV3d(useZ=useZ, i=0, points=Ps[0])
752 c2 = D2(c) # == r.minus(c).length2
754 f = i = 0 # p1..p2 == points[i]..[j]
755 for j, p2 in Ps.enumerate(closed=closed):
756 p2 = _otherV3d(useZ=useZ, i=j, points=p2)
757 p, t = _nearestOn2(r, p1, p2) # within=True, eps=EPS
758 d2 = D2(p) # == r.minus(p).length2
759 if d2 < c2:
760 c2, c, s, e, f = d2, p, p1, p2, (i + t)
761 p1, i = p2, j
763 f, j = _fi_j2(f, len(Ps)) # like .ellipsoidalBaseDI._nearestOn2_
765 kwds = _xkwds(Vector_and_kwds, clas=point.classof, name=Ps.name)
766 v = _nVc(c, **kwds)
767 s = _nVc(s, **kwds) if s is not c else v
768 e = _nVc(e, **kwds) if e is not c else v
769 return NearestOn6Tuple(v, sqrt(c2), f, j, s, e)
772def _nVc(v, clas=None, name=NN, Vector=None, **Vector_kwds): # in .vector2d
773 # return a named C{Vector} or C{clas} instance
774 if Vector is not None:
775 v = Vector(v.x, v.y, v.z, **Vector_kwds)
776 elif clas is not None:
777 v = clas(v.x, v.y, v.z) # ignore Vector_kwds
778 return _xnamed(v, name) if name else v
781def _otherV3d(useZ=True, NN_OK=True, i=None, **name_v): # in .CartesianEllipsoidalBase.intersections2,
782 # check named vector instance, return Vector3d .Ellipsoid.height4, .formy.hartzell, .vector2d
783 def _name_i(name, i):
784 return name if i is None else Fmt.SQUARE(name, i)
786 name, v = _xkwds_popitem(name_v)
787 if useZ and isinstance(v, Vector3dBase):
788 return v if NN_OK or v.name else v.copy(name=_name_i(name, i))
789 try:
790 return Vector3d(v.x, v.y, (v.z if useZ else INT0), name=_name_i(name, i))
791 except AttributeError: # no .x, .y or .z attr
792 pass
793 raise _xotherError(Vector3d(0, 0, 0), v, name=_name_i(name, i), up=2)
796def parse3d(str3d, sep=_COMMA_, Vector=Vector3d, **Vector_kwds):
797 '''Parse an C{"x, y, z"} string.
799 @arg str3d: X, y and z values (C{str}).
800 @kwarg sep: Optional separator (C{str}).
801 @kwarg Vector: Optional class (L{Vector3d}).
802 @kwarg Vector_kwds: Optional B{C{Vector}} keyword arguments,
803 ignored if C{B{Vector} is None}.
805 @return: A B{C{Vector}} instance or if B{C{Vector}} is C{None},
806 a named L{Vector3Tuple}C{(x, y, z)}.
808 @raise VectorError: Invalid B{C{str3d}}.
809 '''
810 try:
811 v = [float(v.strip()) for v in str3d.split(sep)]
812 n = len(v)
813 if n != 3:
814 raise _ValueError(len=n)
815 except (TypeError, ValueError) as x:
816 raise VectorError(str3d=str3d, cause=x)
817 return _xnamed((Vector3Tuple(v) if Vector is None else # *v
818 Vector(*v, **Vector_kwds)), parse3d.__name__)
821def sumOf(vectors, Vector=Vector3d, **Vector_kwds):
822 '''Compute the I{vectorial} sum of two oe more vectors.
824 @arg vectors: Vectors to be added (L{Vector3d}[]).
825 @kwarg Vector: Optional class for the vectorial sum (L{Vector3d}).
826 @kwarg Vector_kwds: Optional B{C{Vector}} keyword arguments,
827 ignored if C{B{Vector} is None}.
829 @return: Vectorial sum as B{C{Vector}} or if B{C{Vector}} is
830 C{None}, a named L{Vector3Tuple}C{(x, y, z)}.
832 @raise VectorError: No B{C{vectors}}.
833 '''
834 try:
835 t = _MODS.nvectorBase._nsumOf(vectors, 0, None, {}) # no H
836 except (TypeError, ValueError) as x:
837 raise VectorError(vectors=vectors, Vector=Vector, cause=x)
838 x, y, z = t[:3]
839 n = sumOf.__name__
840 return Vector3Tuple(x, y, z, name=n) if Vector is None else \
841 Vector(x, y, z, **_xkwds(Vector_kwds, name=n))
844def trilaterate2d2(x1, y1, radius1, x2, y2, radius2, x3, y3, radius3,
845 eps=None, **Vector_and_kwds):
846 '''Trilaterate three circles, each given as a (2-D) center and a radius.
848 @arg x1: Center C{x} coordinate of the 1st circle (C{scalar}).
849 @arg y1: Center C{y} coordinate of the 1st circle (C{scalar}).
850 @arg radius1: Radius of the 1st circle (C{scalar}).
851 @arg x2: Center C{x} coordinate of the 2nd circle (C{scalar}).
852 @arg y2: Center C{y} coordinate of the 2nd circle (C{scalar}).
853 @arg radius2: Radius of the 2nd circle (C{scalar}).
854 @arg x3: Center C{x} coordinate of the 3rd circle (C{scalar}).
855 @arg y3: Center C{y} coordinate of the 3rd circle (C{scalar}).
856 @arg radius3: Radius of the 3rd circle (C{scalar}).
857 @kwarg eps: Tolerance to check the trilaterated point I{delta} on all
858 3 circles (C{scalar}) or C{None} for no checking.
859 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the
860 trilateration and optional, additional B{C{Vector}}
861 keyword arguments, otherwise (L{Vector3d}).
863 @return: Trilaterated point as C{B{Vector}(x, y, **B{Vector_kwds})}
864 or L{Vector2Tuple}C{(x, y)} if C{B{Vector} is None}..
866 @raise IntersectionError: No intersection, near-concentric or -colinear
867 centers, trilateration failed some other way
868 or the trilaterated point is off one circle
869 by more than B{C{eps}}.
871 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}.
873 @see: U{Issue #49<https://GitHub.com/mrJean1/PyGeodesy/issues/49>},
874 U{Find X location using 3 known (X,Y) location using trilateration
875 <https://math.StackExchange.com/questions/884807>} and function
876 L{pygeodesy.trilaterate3d2}.
877 '''
878 return _MODS.vector2d._trilaterate2d2(x1, y1, radius1,
879 x2, y2, radius2,
880 x3, y3, radius3, eps=eps, **Vector_and_kwds)
883def trilaterate3d2(center1, radius1, center2, radius2, center3, radius3,
884 eps=EPS, **Vector_and_kwds):
885 '''Trilaterate three spheres, each given as a (3-D) center and a radius.
887 @arg center1: Center of the 1st sphere (C{Cartesian}, L{Vector3d},
888 C{Vector3Tuple} or C{Vector4Tuple}).
889 @arg radius1: Radius of the 1st sphere (same C{units} as C{x}, C{y}
890 and C{z}).
891 @arg center2: Center of the 2nd sphere (C{Cartesian}, L{Vector3d},
892 C{Vector3Tuple} or C{Vector4Tuple}).
893 @arg radius2: Radius of this sphere (same C{units} as C{x}, C{y}
894 and C{z}).
895 @arg center3: Center of the 3rd sphere (C{Cartesian}, L{Vector3d},
896 C{Vector3Tuple} or C{Vector4Tuple}).
897 @arg radius3: Radius of the 3rd sphere (same C{units} as C{x}, C{y}
898 and C{z}).
899 @kwarg eps: Pertubation tolerance (C{scalar}), same units as C{x},
900 C{y} and C{z} or C{None} for no pertubations.
901 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the
902 trilateration and optional, additional B{C{Vector}}
903 keyword arguments, otherwise B{C{center1}}'s
904 (sub-)class.
906 @return: 2-Tuple with two trilaterated points, each a B{C{Vector}}
907 instance. Both points are the same instance if all three
908 spheres abut/intersect in a single point.
910 @raise ImportError: Package C{numpy} not found, not installed or
911 older than version 1.10.
913 @raise IntersectionError: Near-concentric, -colinear, too distant or
914 non-intersecting spheres.
916 @raise NumPyError: Some C{numpy} issue.
918 @raise TypeError: Invalid B{C{center1}}, B{C{center2}} or B{C{center3}}.
920 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}.
922 @see: Norrdine, A. U{I{An Algebraic Solution to the Multilateration
923 Problem}<https://www.ResearchGate.net/publication/
924 275027725_An_Algebraic_Solution_to_the_Multilateration_Problem>},
925 the U{I{implementation}<https://www.ResearchGate.net/publication/
926 288825016_Trilateration_Matlab_Code>} and function
927 L{pygeodesy.trilaterate2d2}.
928 '''
929 try:
930 return _MODS.vector2d._trilaterate3d2(_otherV3d(center1=center1, NN_OK=False),
931 Radius_(radius1=radius1, low=eps),
932 center2, radius2, center3, radius3, eps=eps,
933 clas=center1.classof, **Vector_and_kwds)
934 except (AssertionError, TypeError, ValueError) as x:
935 raise _xError(x, center1=center1, radius1=radius1,
936 center2=center2, radius2=radius2,
937 center3=center3, radius3=radius3)
940def _xyzhdn3(xyz, height, datum, ll): # in .cartesianBase, .nvectorBase
941 '''(INTERNAL) Get a C{(h, d, name)} 3-tuple.
942 '''
943 h = height or _xattr(xyz, height=None) \
944 or _xattr(xyz, h=None) \
945 or _xattr(ll, height=None)
947 d = datum or _xattr(xyz, datum=None) \
948 or _xattr(ll, datum=None)
950 return h, d, _xattr(xyz, name=NN)
953__all__ += _ALL_DOCS(intersections2, sumOf, Vector3dBase)
955# **) MIT License
956#
957# Copyright (C) 2016-2023 -- mrJean1 at Gmail -- All Rights Reserved.
958#
959# Permission is hereby granted, free of charge, to any person obtaining a
960# copy of this software and associated documentation files (the "Software"),
961# to deal in the Software without restriction, including without limitation
962# the rights to use, copy, modify, merge, publish, distribute, sublicense,
963# and/or sell copies of the Software, and to permit persons to whom the
964# Software is furnished to do so, subject to the following conditions:
965#
966# The above copyright notice and this permission notice shall be included
967# in all copies or substantial portions of the Software.
968#
969# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
970# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
971# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
972# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
973# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
974# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
975# OTHER DEALINGS IN THE SOFTWARE.