Coverage for pygeodesy/vector2d.py: 98%
340 statements
« prev ^ index » next coverage.py v7.6.1, created at 2025-01-10 16:55 -0500
« prev ^ index » next coverage.py v7.6.1, created at 2025-01-10 16:55 -0500
2# -*- coding: utf-8 -*-
4u'''2- or 3-D vectorial functions L{circin6}, L{circum3}, L{circum4_},
5L{iscolinearWith}, L{meeus2}, L{nearestOn}, L{radii11}, L{soddy4} and
6L{trilaterate2d2}.
7'''
9from pygeodesy.basics import len2, map2, _xnumpy
10from pygeodesy.constants import EPS, EPS0, EPS02, EPS4, INF, INT0, \
11 _EPS4e8, isnear0, _0_0, _0_25, _0_5, _N_0_5, \
12 _1_0, _1_0_1T, _N_1_0, _2_0, _N_2_0, _4_0
13from pygeodesy.errors import _and, _AssertionError, IntersectionError, NumPyError, \
14 PointsError, TriangleError, _xError, _xkwds
15from pygeodesy.fmath import fabs, fdot, fdot_, hypot, hypot2_, sqrt
16from pygeodesy.fsums import _Fsumf_, fsumf_, fsum1f_
17from pygeodesy.interns import NN, _a_, _and_, _b_, _c_, _center_, _coincident_, \
18 _colinear_, _COMMASPACE_, _concentric_, _few_, \
19 _intersection_, _invalid_, _near_, _no_, _of_, \
20 _radius_, _rIn_, _s_, _SPACE_, _too_, _with_
21# from pygeodesy.lazily import _ALL_LAZY # from .named
22from pygeodesy.named import _ALL_LAZY, _NamedTuple, _Pass, Property_RO
23from pygeodesy.namedTuples import LatLon3Tuple, Vector2Tuple
24# from pygeodesy.props import Property_RO # from .named
25from pygeodesy.streprs import Fmt, unstr
26from pygeodesy.units import Float, Int, Meter, Radius, Radius_
27from pygeodesy.vector3d import iscolinearWith, nearestOn, _nearestOn2, _nVc, \
28 _otherV3d, trilaterate3d2, Vector3d # PYCHOK unused
30from contextlib import contextmanager
31# from math import fabs, sqrt # from .fmath
33__all__ = _ALL_LAZY.vector2d
34__version__ = '24.11.21'
36_cA_ = 'cA'
37_cB_ = 'cB'
38_cC_ = 'cC'
39_deltas_ = 'deltas'
40_outer_ = 'outer'
41_raise_ = 'raise' # PYCHOK used!
42_rank_ = 'rank'
43_residuals_ = 'residuals'
44_Type_ = 'Type'
47class Circin6Tuple(_NamedTuple):
48 '''6-Tuple C{(radius, center, deltas, cA, cB, cC)} with the C{radius}, the
49 trilaterated C{center} and contact points of the I{inscribed} aka I{In-
50 circle} of a triangle. The C{center} is I{un}ambiguous if C{deltas} is
51 C{None}, otherwise C{center} is the mean and C{deltas} the differences of
52 the L{pygeodesy.trilaterate3d2} results. Contact points C{cA}, C{cB} and
53 C{cC} are the points of tangency, aka the corners of the U{Contact Triangle
54 <https://MathWorld.Wolfram.com/ContactTriangle.html>}.
55 '''
56 _Names_ = (_radius_, _center_, _deltas_, _cA_, _cB_, _cC_)
57 _Units_ = ( Radius, _Pass, _Pass, _Pass, _Pass, _Pass)
60class Circum3Tuple(_NamedTuple): # in .latlonBase
61 '''3-Tuple C{(radius, center, deltas)} with the C{circumradius} and trilaterated
62 C{circumcenter} of the C{circumcircle} through 3 points (aka {Meeus}' Type II
63 circle) or the C{radius} and C{center} of the smallest I{Meeus}' Type I circle.
64 The C{center} is I{un}ambiguous if C{deltas} is C{None}, otherwise C{center}
65 is the mean and C{deltas} the differences of the L{pygeodesy.trilaterate3d2}
66 results.
67 '''
68 _Names_ = (_radius_, _center_, _deltas_)
69 _Units_ = ( Radius, _Pass, _Pass)
72class Circum4Tuple(_NamedTuple):
73 '''4-Tuple C{(radius, center, rank, residuals)} with C{radius} and C{center}
74 of a sphere I{least-squares} fitted through given points and the C{rank}
75 and C{residuals} -if any- from U{numpy.linalg.lstsq
76 <https://NumPy.org/doc/stable/reference/generated/numpy.linalg.lstsq.html>}.
77 '''
78 _Names_ = (_radius_, _center_, _rank_, _residuals_)
79 _Units_ = ( Radius, _Pass, Int, _Pass)
82class Meeus2Tuple(_NamedTuple):
83 '''2-Tuple C{(radius, Type)} with C{radius} and I{Meeus}' C{Type} of the smallest
84 circle I{containing} 3 points. C{Type} is C{None} for a I{Meeus}' Type II
85 C{circumcircle} passing through all 3 points. Otherwise C{Type} is the center
86 of a I{Meeus}' Type I circle with 2 points on (a diameter of) and 1 point
87 inside the circle.
88 '''
89 _Names_ = (_radius_, _Type_)
90 _Units_ = ( Radius, _Pass)
93class Radii11Tuple(_NamedTuple):
94 '''11-Tuple C{(rA, rB, rC, cR, rIn, riS, roS, a, b, c, s)} with the C{Tangent}
95 circle radii C{rA}, C{rB} and C{rC}, the C{circumradius} C{cR}, the C{Incircle}
96 radius C{rIn} aka C{inradius}, the inner and outer I{Soddy} circle radii C{riS}
97 and C{roS}, the sides C{a}, C{b} and C{c} and semi-perimeter C{s} of a triangle,
98 all in C{meter} conventionally.
100 @note: C{Circumradius} C{cR} and outer I{Soddy} radius C{roS} may be C{INF}.
101 '''
102 _Names_ = ('rA', 'rB', 'rC', 'cR', _rIn_, 'riS', 'roS', _a_, _b_, _c_, _s_)
103 _Units_ = ( Meter,) * len(_Names_)
106class Soddy4Tuple(_NamedTuple):
107 '''4-Tuple C{(radius, center, deltas, outer)} with C{radius} and (trilaterated)
108 C{center} of the I{inner} I{Soddy} circle and the radius of the C{outer}
109 I{Soddy} circle. The C{center} is I{un}ambiguous if C{deltas} is C{None},
110 otherwise C{center} is the mean and C{deltas} the differences of the
111 L{pygeodesy.trilaterate3d2} results.
113 @note: The outer I{Soddy} radius C{outer} may be C{INF}.
114 '''
115 _Names_ = (_radius_, _center_, _deltas_, _outer_)
116 _Units_ = ( Radius, _Pass, _Pass, Radius)
119class Triaxum5Tuple(_NamedTuple):
120 '''5-Tuple C{(a, b, c, rank, residuals)} with the (unordered) triaxial radii
121 C{a}, C{b} and C{c} of an ellipsoid I{least-squares} fitted through given
122 points and the C{rank} and C{residuals} -if any- from U{numpy.linalg.lstsq
123 <https://NumPy.org/doc/stable/reference/generated/numpy.linalg.lstsq.html>}.
124 '''
125 _Names_ = (_a_, _b_, _c_, _rank_, _residuals_)
126 _Units_ = ( Radius, Radius, Radius, Int, _Pass)
129def circin6(point1, point2, point3, eps=EPS4, useZ=True):
130 '''Return the radius and center of the I{inscribed} aka I{Incircle} of
131 a (2- or 3-D) triangle.
133 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
134 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
135 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
136 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
137 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
138 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
139 @kwarg eps: Tolerance for function L{pygeodesy.trilaterate3d2} if
140 C{B{useZ} is True} else L{pygeodesy.trilaterate2d2}.
141 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).
143 @return: L{Circin6Tuple}C{(radius, center, deltas, cA, cB, cC)}. The
144 C{center} and contact points C{cA}, C{cB} and C{cC}, each an
145 instance of B{C{point1}}'s (sub-)class, are co-planar with
146 the three given points.
148 @raise ImportError: Package C{numpy} not found, not installed or older
149 than version 1.10 and C{B{useZ} is True}.
151 @raise IntersectionError: Near-coincident or -colinear points or
152 a trilateration or C{numpy} issue.
154 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}.
156 @see: Functions L{radii11} and L{circum3}, U{Contact Triangle
157 <https://MathWorld.Wolfram.com/ContactTriangle.html>} and
158 U{Incircle<https://MathWorld.Wolfram.com/Incircle.html>}.
159 '''
160 try:
161 return _circin6(point1, point2, point3, eps=eps, useZ=useZ)
162 except (AssertionError, TypeError, ValueError) as x:
163 raise _xError(x, point1=point1, point2=point2, point3=point3)
166def _circin6(point1, point2, point3, eps=EPS4, useZ=True, dLL3=False, **Vector_kwds):
167 # (INTERNAL) Radius, center, deltas, 3 contact points
169 def _fraction(r, a):
170 return (r / a) if a > EPS0 else _0_5
172 def _contact2(a, p2, r2, p3, r3, V, V_kwds):
173 c = p2.intermediateTo(p3, _fraction(r2, a)) if r2 > r3 else \
174 p3.intermediateTo(p2, _fraction(r3, a))
175 C = V(c.x, c.y, c.z, **V_kwds)
176 return c, C
178 t, p1, p2, p3 = _radii11ABC4(point1, point2, point3, useZ=useZ)
179 V, r1, r2, r3 = point1.classof, t.rA, t.rB, t.rC
181 c1, cA = _contact2(t.a, p2, r2, p3, r3, V, _xkwds(Vector_kwds, name=_cA_))
182 c2, cB = _contact2(t.b, p3, r3, p1, r1, V, _xkwds(Vector_kwds, name=_cB_))
183 c3, cC = _contact2(t.c, p1, r1, p2, r2, V, _xkwds(Vector_kwds, name=_cC_))
185 r = t.rIn
186 c, d = _tricenter3d2(c1, r, c2, r, c3, r, eps=eps, useZ=useZ, dLL3=dLL3,
187 **_xkwds(Vector_kwds, Vector=V, name=circin6.__name__))
188 return Circin6Tuple(r, c, d, cA, cB, cC)
191def circum3(point1, point2, point3, circum=True, eps=EPS4, useZ=True):
192 '''Return the radius and center of the smallest circle I{through} or
193 I{containing} three (2- or 3-D) points.
195 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or
196 C{Vector4Tuple}).
197 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or
198 C{Vector4Tuple}).
199 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or
200 C{Vector4Tuple}).
201 @kwarg circum: If C{True}, return the C{circumradius} and C{circumcenter}
202 always, ignoring the I{Meeus}' Type I case (C{bool}).
203 @kwarg eps: Tolerance for function L{pygeodesy.trilaterate3d2} if C{B{useZ}
204 is True} else L{pygeodesy.trilaterate2d2}.
205 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).
207 @return: A L{Circum3Tuple}C{(radius, center, deltas)}. The C{center}, an
208 instance of B{C{point1}}'s (sub-)class, is co-planar with the three
209 given points.
211 @raise ImportError: Package C{numpy} not found, not installed or older
212 than version 1.10 and C{B{useZ} is True}.
214 @raise IntersectionError: Near-coincident or -colinear points or
215 a trilateration or C{numpy} issue.
217 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}.
219 @see: Functions L{pygeodesy.circum4_} and L{pygeodesy.meeus2} and Meeus, J.
220 U{I{Astronomical Algorithms}<http://www.Agopax.IT/Libri_astronomia/pdf/
221 Astronomical%20Algorithms.pdf>}, 2nd Ed. 1998, page 127ff, U{circumradius
222 <https://MathWorld.Wolfram.com/Circumradius.html>} and U{circumcircle
223 <https://MathWorld.Wolfram.com/Circumcircle.html>}.
224 '''
225 try:
226 p1 = _otherV3d(useZ=useZ, point1=point1)
227 return _circum3(p1, point2, point3, circum=circum, eps=eps, useZ=useZ,
228 clas=point1.classof)
229 except (AssertionError, TypeError, ValueError) as x:
230 raise _xError(x, point1=point1, point2=point2, point3=point3, circum=circum)
233def _circum3(p1, point2, point3, circum=True, eps=EPS4, useZ=True, dLL3=False,
234 clas=Vector3d, **clas_kwds): # in .latlonBase
235 # (INTERNAL) Radius, center, deltas
236 r, d, p2, p3 = _meeus4(p1, point2, point3, circum=circum, useZ=useZ,
237 clas=clas, **clas_kwds)
238 if d is None: # Meeus' Type II or circum=True
239 kwds = _xkwds(clas_kwds, eps=eps, Vector=clas, name=circum3.__name__)
240 c, d = _tricenter3d2(p1, r, p2, r, p3, r, useZ=useZ, dLL3=dLL3, **kwds)
241 else: # Meeus' Type I
242 c, d = d, None
243 return Circum3Tuple(r, c, d)
246def circum4(points, useZ=True, **Vector_and_kwds):
247 '''Best-fit a sphere through three or more (3-D) points.
249 @arg points: Iterable of points (each a C{Cartesian}, L{Vector3d}, C{Vector3Tuple}
250 or C{Vector4Tuple}).
251 @kwarg useZ: If C{True}, use the points' Z component, otherwise force C{z=INT0}
252 (C{bool}).
253 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the center point
254 and optionally, additional B{C{Vector}} keyword arguments, otherwise
255 the B{C{points}}' (sub-)class.
257 @return: L{Circum4Tuple}C{(radius, center, rank, residuals)} with C{center} an
258 instance of C{B{points}[0]}' (sub-)class or B{C{Vector}} if specified.
260 @raise ImportError: Package C{numpy} not found, not installed or older than
261 version 1.10.
263 @raise NumPyError: Some C{numpy} issue.
265 @raise PointsError: Too few B{C{points}}.
267 @raise TypeError: One of the B{C{points}} is invalid.
269 @see: Functions L{pygeodesy.circum3} and L{pygeodesy.meeus2}, I{Charles Jekel}'s
270 U{"Least Squares Sphere Fit"<https://Jekel.me/2015/Least-Squares-Sphere-Fit/>},
271 U{Appendix A<https://hdl.handle.net/10019.1/98627>}, U{numpy.linalg.lstsq
272 <https://NumPy.org/doc/stable/reference/generated/numpy.linalg.lstsq.html>} and U{Eberly
273 6<https://www.sci.Utah.EDU/~balling/FEtools/doc_files/LeastSquaresFitting.pdf>}.
274 '''
275 n, ps = len2(points)
276 if n < 3:
277 raise PointsError(points=n, txt=_too_(_few_))
279 A, b = [], []
280 for i, p in enumerate(ps):
281 v = _otherV3d(useZ=useZ, i=i, points=p)
282 A.append(v.times(_2_0).xyz3 + _1_0_1T)
283 b.append(v.length2)
285 with _numpy(circum4, n=n) as _np:
286 A = _np.array(A).reshape((n, 4))
287 b = _np.array(b).reshape((n, 1))
288 C, R, rk = _np.least_squares3(A, b)
290 c = Vector3d(*C[:3], name__=circum4) # .__name__
291 r = Radius(sqrt(fsumf_(C[3], *c.x2y2z2)), name=c.name)
293 c = _nVc(c, **_xkwds(Vector_and_kwds, clas=ps[0].classof, name=c.name))
294 return Circum4Tuple(r, c, rk, R)
297def circum4_(*points, **useZ_Vector_and_kwds):
298 '''Best-fit a sphere through three or more (3-D) positional points.
300 @arg points: The points (each a C{Cartesian}, L{Vector3d}, C{Vector3Tuple}
301 or C{Vector4Tuple}), all positional.
302 @kwarg useZ_Vector_and_kwds: Keyword arguments C{B{useZ}=True} and
303 C{B{Vector}=None}, see function L{circum4}.
305 @see: Function L{circum4} for further details.
306 '''
307 return circum4(points, **useZ_Vector_and_kwds)
310def _iscolinearWith(p, point1, point2, eps=EPS, useZ=True):
311 # (INTERNAL) Check colinear, see L{iscolinearWith} above,
312 # separated to allow callers to embellish any exceptions
313 p1 = _otherV3d(useZ=useZ, point1=point1)
314 p2 = _otherV3d(useZ=useZ, point2=point2)
315 n, _ = _nearestOn2(p, p1, p2, within=False, eps=eps)
316 return n is p1 or n.minus(p).length2 < eps
319def meeus2(point1, point2, point3, circum=False, useZ=True):
320 '''Return the radius and I{Meeus}' Type of the smallest circle I{through}
321 or I{containing} three (2- or 3-D) points.
323 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
324 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
325 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
326 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
327 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
328 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
329 @kwarg circum: If C{True}, return the C{circumradius} and C{circumcenter}
330 always, overriding I{Meeus}' Type II case (C{bool}).
331 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).
333 @return: L{Meeus2Tuple}C{(radius, Type)}, with C{Type} the C{circumcenter}
334 iff C{B{circum}=True}.
336 @raise IntersectionError: Near-coincident or -colinear points, iff C{B{circum}=True}.
338 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}.
340 @see: Functions L{pygeodesy.circum3} and L{pygeodesy.circum4_} and Meeus, J.
341 U{I{Astronomical Algorithms}<http://www.Agopax.IT/Libri_astronomia/pdf/
342 Astronomical%20Algorithms.pdf>}, 2nd Ed. 1998, page 127ff, U{circumradius
343 <https://MathWorld.Wolfram.com/Circumradius.html>} and U{circumcircle
344 <https://MathWorld.Wolfram.com/Circumcircle.html>}.
345 '''
346 try:
347 A = _otherV3d(useZ=useZ, point1=point1)
348 return _meeus2(A, point2, point3, circum, useZ=useZ, clas=point1.classof)
349 except (TypeError, ValueError) as x:
350 raise _xError(x, point1=point1, point2=point2, point3=point3, circum=circum)
353def _meeus2(A, point2, point3, circum, useZ=True, **clas_and_kwds): # in .vector3d
354 # (INTERNAL) Radius and center or Meeus' Type
355 f = _circum3 if circum else _meeus4
356 t = f(A, point2, point3, circum=circum, useZ=useZ, **clas_and_kwds)[:2]
357 return Meeus2Tuple(t)
360def _meeus4(A, point2, point3, circum=False, useZ=True, clas=None, **clas_kwds):
361 # (INTERNAL) Radius and Meeus' Type
362 B = p2 = _otherV3d(useZ=useZ, point2=point2)
363 C = p3 = _otherV3d(useZ=useZ, point3=point3)
365 a = B.minus(C).length2
366 b = C.minus(A).length2
367 c = A.minus(B).length2
368 if a < b:
369 a, b, A, B = b, a, B, A
370 if a < c:
371 a, c, A, C = c, a, C, A
373 if a > EPS02 and (circum or a < (b + c)): # circumradius
374 b = sqrt(b / a)
375 c = sqrt(c / a)
376 R = _Fsumf_(_1_0, b, c) * _Fsumf_(_1_0, b, -c) * \
377 _Fsumf_(_1_0, -b, c) * _Fsumf_(_N_1_0, b, c)
378 r = R.fover(a)
379 if r < EPS02:
380 t = _coincident_ if b < EPS0 or c < EPS0 else (
381 _colinear_ if _iscolinearWith(A, B, C) else _invalid_)
382 raise IntersectionError(t)
383 r = b * c / sqrt(r)
384 t = None # Meeus' Type II
385 else: # obtuse or right angle at A
386 r = sqrt(a * _0_25) if a > EPS02 else INT0
387 t = B.plus(C).times(_0_5) # Meeus' Type I
388 if clas is not None:
389 t = clas(t.x, t.y, t.z, **_xkwds(clas_kwds, name=meeus2.__name__))
390 return r, t, p2, p3
393class _numpy(object): # see also .formy._idllmn6, .geodesicw._wargs, .latlonBase._toCartesian3
394 '''(INTERNAL) Partial C{NumPy} wrapper.
395 '''
396 @contextmanager # <https://www.Python.org/dev/peps/pep-0343/> Examples
397 def __call__(self, where, *args, **kwds):
398 '''(INTERNAL) Yield self with any errors raised as L{NumPyError}
399 embellished with all B{C{args}} and B{C{kwds}}.
400 '''
401 np = self.np
402 try: # <https://NumPy.org/doc/stable/reference/generated/numpy.seterr.html>
403 e = np.seterr(all=_raise_) # throw FloatingPointError for numpy errors
404 yield self
405 except Exception as x: # mostly FloatingPointError?
406 t = unstr(where, *args, **kwds)
407 raise NumPyError(t, cause=x) # _xError2?
408 finally: # restore numpy error handling
409 np.seterr(**e)
411 @Property_RO
412 def array(self):
413 return self.np.array
415 def least_squares3(self, A, b):
416 '''Linear least-squares function.
417 '''
418 C, R, rk, _ = self.np.linalg.lstsq(A, b, rcond=None) # to silence warning
419 C = map2(float, C)
420 R = map2(float, R) # empty if rk < 4 or n <= 4
421 return C, R, int(rk)
423 @Property_RO
424 def np(self):
425 '''Import numpy 1.10+ once.
426 '''
427 return _xnumpy(self.__class__, 1, 10)
429 def null_space2(self, A, rcond=None):
430 '''Return the C{null_space} and C{rank} of matrix B{C{A}}.
432 @see: U{Source<https://docs.SciPy.org/doc/scipy/reference/generated/scipy.linalg.null_space.html>}
433 U{SciPY Cookbook<https://SciPy-Cookbook.ReadTheDocs.io/items/RankNullspace.html>}, U{here
434 <https://NumPy.org/doc/stable/reference/generated/numpy.linalg.svd.html>}, U{here
435 <https://StackOverflow.com/questions/19820921>}, U{here
436 <https://StackOverflow.com/questions/2992947>} and U{here
437 <https://StackOverflow.com/questions/5889142>}.
438 '''
439 def _Error(**kwds):
440 return _AssertionError(txt__=self.null_space2, **kwds)
442 np = self.np
443 A = np.array(A)
444 m = max(A.shape)
445 if m != 4: # for this usage
446 raise _Error(shape=m)
447 # if needed, square A, pad with zeros
448 A = np.resize(A, m * m).reshape(m, m)
449# try: # no np.linalg.null_space <https://docs.SciPy.org/doc/>
450# Z = scipy.linalg.null_space(A) # XXX no scipy.linalg?
451# return Z, ...
452# except AttributeError:
453# pass
454 U, S, V = np.linalg.svd(A)
455 s = max(EPS, rcond) if rcond else (EPS * max(U.shape[0], V.shape[1]))
456 t = max(EPS, float(np.max(S) * s)) # abs_tol, rel_tol * largest singular
457 r = int(np.sum(S > t)) # rank
458 if r == 3: # get null_space
459 Z = np.transpose(V[r:])
460 s = map2(int, Z.shape)
461 if s != (m, 1): # bad null_space shape
462 raise _Error(shape=s, m=m)
463 D = A.dot(Z) # near-zeros-vector
464 n = float(np.linalg.norm(D, INF)) # INF = max(fabs(D)), 2 = hypot_(*D)
465 if n > t: # largest exceed tol
466 raise _Error(dot=tuple(D.ravel()), norm=n, tol=t)
467 else: # coincident, colinear, concentric centers, ambiguous, etc.
468 Z = None
469 # del A, S, U, V # release numpy
470 return Z, r
472 @Property_RO
473 def pseudo_inverse(self):
474 '''Moore-Penrose pseudo-inverse function.
475 '''
476 return self.np.linalg.pinv
478 def real_roots(self, *coeffs):
479 '''Compute the real, non-complex roots of a polynomial.
480 '''
481 np = self.np
482 rs = np.polynomial.polynomial.polyroots(coeffs)
483 return tuple(float(r) for r in rs if not np.iscomplex(r))
485_numpy = _numpy() # PYCHOK singleton
488def radii11(point1, point2, point3, useZ=True):
489 '''Return the radii of the C{In-}, I{Soddy} and C{Tangent} circles of a
490 (2- or 3-D) triangle.
492 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
493 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
494 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
495 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
496 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
497 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
498 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).
500 @return: L{Radii11Tuple}C{(rA, rB, rC, cR, rIn, riS, roS, a, b, c, s)}.
502 @raise TriangleError: Near-coincident or -colinear points.
504 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}.
506 @see: U{Circumradius<https://MathWorld.Wolfram.com/Circumradius.html>},
507 U{Incircle<https://MathWorld.Wolfram.com/Incircle.html>}, U{Soddy
508 Circles<https://MathWorld.Wolfram.com/SoddyCircles.html>} and
509 U{Tangent Circles<https://MathWorld.Wolfram.com/TangentCircles.html>}.
510 '''
511 try:
512 return _radii11ABC4(point1, point2, point3, useZ=useZ)[0]
513 except (TypeError, ValueError) as x:
514 raise _xError(x, point1=point1, point2=point2, point3=point3)
517def _radii11ABC4(point1, point2, point3, useZ=True):
518 # (INTERNAL) Tangent, Circum, Incircle, Soddy radii, sides and semi-perimeter
519 A = _otherV3d(useZ=useZ, point1=point1, NN_OK=False)
520 B = _otherV3d(useZ=useZ, point2=point2, NN_OK=False)
521 C = _otherV3d(useZ=useZ, point3=point3, NN_OK=False)
523 a = B.minus(C).length
524 b = C.minus(A).length
525 c = A.minus(B).length
527 S = _Fsumf_(a, b, c) * _0_5
528 s = float(S) # semi-perimeter
529 if s > EPS0:
530 rs = float(S - a), float(S - b), float(S - c)
531 r3, r2, r1 = sorted(rs) # r3 <= r2 <= r1
532 if r3 > EPS0: # and r2 > EPS0 and r1 > EPS0
533 r3_r1 = r3 / r1
534 r3_r2 = r3 / r2
535 # t = r1 * r2 * r3 * (r1 + r2 + r3)
536 # = r1**2 * r2 * r3 * (1 + r2 / r1 + r3 / r1)
537 # = (r1 * r2)**2 * (r3 / r2) * (1 + r2 / r1 + r3 / r1)
538 t = r3_r2 * fsum1f_(_1_0, r2 / r1, r3_r1) # * (r1 * r2)**2
539 if t > EPS02:
540 t = sqrt(t) * _2_0 # * r1 * r2
541 # d = r1 * r2 + r2 * r3 + r3 * r1
542 # = r1 * (r2 + r2 * r3 / r1 + r3)
543 # = r1 * r2 * (1 + r3 / r1 + r3 / r2)
544 d = fsum1f_(_1_0, r3_r1, r3_r2) # * r1 * r2
545 # si/o = r1 * r2 * r3 / (r1 * r2 * (d +/- t))
546 # = r3 / (d +/- t)
547 si = r3 / (d + t)
548 so = (r3 / (d - t)) if d > t else INF
549 # ci = sqrt(r1 * r2 * r3 / s)
550 # = r1 * sqrt(r2 * r3 / r1 / s)
551 ci = r1 * sqrt(r2 * r3_r1 / s)
552 # co = a * b * c / (4 * ci * s)
553 t = ci * s * _4_0
554 co = (a * b * c / t) if t > EPS0 else INF
555 r1, r2, r3 = rs # original order
556 t = Radii11Tuple(r1, r2, r3, co, ci, si, so, a, b, c, s)
557 return t, A, B, C
559 raise TriangleError(_near_(_coincident_) if min(a, b, c) < EPS0 else (
560 _colinear_ if _iscolinearWith(A, B, C) else _invalid_))
563def soddy4(point1, point2, point3, eps=EPS4, useZ=True):
564 '''Return the radius and center of the C{inner} I{Soddy} circle of a
565 (2- or 3-D) triangle.
567 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
568 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
569 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
570 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
571 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
572 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
573 @kwarg eps: Tolerance for function L{pygeodesy.trilaterate3d2} if
574 C{B{useZ} is True} otherwise L{pygeodesy.trilaterate2d2}.
575 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=INT0} (C{bool}).
577 @return: L{Soddy4Tuple}C{(radius, center, deltas, outer)}. The C{center},
578 an instance of B{C{point1}}'s (sub-)class, is co-planar with the
579 three given points. The C{outer} I{Soddy} radius may be C{INF}.
581 @raise ImportError: Package C{numpy} not found, not installed or older
582 than version 1.10 and C{B{useZ} is True}.
584 @raise IntersectionError: Near-coincident or -colinear points or
585 a trilateration or C{numpy} issue.
587 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}.
589 @see: Functions L{radii11} and L{circum3} and U{Soddy Circles
590 <https://MathWorld.Wolfram.com/SoddyCircles.html>}.
591 '''
592 t, p1, p2, p3 = _radii11ABC4(point1, point2, point3, useZ=useZ)
594 r = t.riS
595 c, d = _tricenter3d2(p1, t.rA + r,
596 p2, t.rB + r,
597 p3, t.rC + r, eps=eps, useZ=useZ,
598 Vector=point1.classof, name=soddy4.__name__)
599 return Soddy4Tuple(r, c, d, t.roS)
602def triaxum5(points, useZ=True):
603 '''Best-fit a triaxial ellipsoid through three or more (3-D) points.
605 @arg points: Iterable of points (each a C{Cartesian}, L{Vector3d}, C{Vector3Tuple}
606 or C{Vector4Tuple}).
607 @kwarg useZ: If C{True}, use the points' Z component, otherwise force C{z=INT0}
608 (C{bool}).
610 @return: L{Triaxum5Tuple}C{(a, b, c, rank, residuals)} with the unordered triaxial
611 radii C{a}, C{b} and C{c} in C{meter}, same units as the points' coordinates.
613 @raise ImportError: Package C{numpy} not found, not installed or older than version 1.10.
615 @raise NumPyError: Some C{numpy} issue.
617 @raise PointsError: Too few B{C{points}}.
619 @raise TypeError: One of the B{C{points}} is invalid.
621 @see: I{Charles Jekel}'s U{"Least Squares Ellipsoid Fit"<https://Jekel.me/2020/Least-Squares-Ellipsoid-Fit/>}
622 and U{numpy.linalg.lstsq<https://NumPy.org/doc/stable/reference/generated/numpy.linalg.lstsq.html>}.
623 '''
624 n, ps = len2(points)
625 if n < 3:
626 raise PointsError(points=n, txt=_too_(_few_))
628 A = []
629 for i, p in enumerate(ps):
630 v = _otherV3d(useZ=useZ, i=i, points=p)
631 A.append(v.x2y2z2)
633 with _numpy(triaxum5, n=n) as _np:
634 A = _np.array(A)
635 b = _1_0_1T * n
636 T, R, rk = _np.least_squares3(A, b)
638 def _1_sqrt(x):
639 return sqrt(_1_0 / x) if x else _0_0 # INF
641 a, b, c = map(_1_sqrt, T)
642 return Triaxum5Tuple(a, b, c, rk, R)
645def _tricenter3d2(p1, r1, p2, r2, p3, r3, eps=EPS4, useZ=True, dLL3=False, **kwds):
646 # (INTERNAL) Trilaterate and disambiguate the 3-D center
647 d, kwds = None, _xkwds(kwds, eps=eps, coin=True)
648 if useZ and p1.z != p2.z != p3.z: # ignore z if all match
649 a, b = _trilaterate3d2(p1, r1, p2, r2, p3, r3, **kwds)
650 if a is b: # no unambiguity
651 c = a # == b
652 else:
653 c = a.plus(b).times(_0_5) # mean
654 if not a.isconjugateTo(b, minum=0, eps=eps):
655 if dLL3: # deltas as (lat, lon, height)
656 a = a.toLatLon()
657 b = b.toLatLon()
658 d = LatLon3Tuple(b.lat - a.lat,
659 b.lon - a.lon,
660 b.height - a.height, name=_deltas_)
661 else:
662 d = b.minus(a) # vectorial deltas
663 else:
664 if useZ: # pass z to Vector if given
665 kwds = _xkwds(kwds, z=p1.z)
666 c = _trilaterate2d2(p1.x, p1.y, r1,
667 p2.x, p2.y, r2,
668 p3.x, p3.y, r3, **kwds)
669 return c, d
672def trilaterate2d2(x1, y1, radius1, x2, y2, radius2, x3, y3, radius3,
673 eps=None, **Vector_and_kwds):
674 '''Trilaterate three circles, each given as a (2-D) center and a radius.
676 @arg x1: Center C{x} coordinate of the 1st circle (C{scalar}).
677 @arg y1: Center C{y} coordinate of the 1st circle (C{scalar}).
678 @arg radius1: Radius of the 1st circle (C{scalar}).
679 @arg x2: Center C{x} coordinate of the 2nd circle (C{scalar}).
680 @arg y2: Center C{y} coordinate of the 2nd circle (C{scalar}).
681 @arg radius2: Radius of the 2nd circle (C{scalar}).
682 @arg x3: Center C{x} coordinate of the 3rd circle (C{scalar}).
683 @arg y3: Center C{y} coordinate of the 3rd circle (C{scalar}).
684 @arg radius3: Radius of the 3rd circle (C{scalar}).
685 @kwarg eps: Tolerance to check the trilaterated point I{delta} on
686 all 3 circles (C{scalar}) or C{None} for no checking.
687 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return
688 the trilateration and optionally, additional B{C{Vector}}
689 keyword arguments).
691 @return: Trilaterated point as C{B{Vector}(x, y, **B{Vector_kwds})}
692 or L{Vector2Tuple}C{(x, y)} if C{B{Vector} is None}.
694 @raise IntersectionError: No intersection, near-concentric or -colinear
695 centers, trilateration failed some other way
696 or the trilaterated point is off one circle
697 by more than B{C{eps}}.
699 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}.
701 @see: U{Issue #49<https://GitHub.com/mrJean1/PyGeodesy/issues/49>},
702 U{Find X location using 3 known (X,Y) location using trilateration
703 <https://math.StackExchange.com/questions/884807>} and function
704 L{pygeodesy.trilaterate3d2}.
705 '''
706 return _trilaterate2d2(x1, y1, radius1,
707 x2, y2, radius2,
708 x3, y3, radius3, eps=eps, **Vector_and_kwds)
711def _trilaterate2d2(x1, y1, radius1, x2, y2, radius2, x3, y3, radius3,
712 coin=False, eps=None,
713 Vector=None, **Vector_kwds):
714 # (INTERNAL) Trilaterate three circles, see L{pygeodesy.trilaterate2d2}
716 def _abct4(x1, y1, r1, x2, y2, r2):
717 a = x2 - x1
718 b = y2 - y1
719 t = _tri3near2far(r1, r2, hypot(a, b), coin)
720 c = _0_0 if t else (hypot2_(r1, x2, y2) - hypot2_(r2, x1, y1))
721 return a, b, c, t
723 def _astr(**kwds): # kwds as (name=value, ...) strings
724 return Fmt.PAREN(_COMMASPACE_(*(Fmt.EQUALg(*t) for t in kwds.items())))
726 r1 = Radius_(radius1=radius1)
727 r2 = Radius_(radius2=radius2)
728 r3 = Radius_(radius3=radius3)
730 a, b, c, t = _abct4(x1, y1, r1, x2, y2, r2)
731 if t:
732 raise IntersectionError(_and(_astr(x1=x1, y1=y1, radius1=r1),
733 _astr(x2=x2, y2=y2, radius2=r2)), txt=t)
735 d, e, f, t = _abct4(x2, y2, r2, x3, y3, r3)
736 if t:
737 raise IntersectionError(_and(_astr(x2=x2, y2=y2, radius2=r2),
738 _astr(x3=x3, y3=y3, radius3=r3)), txt=t)
740 _, _, _, t = _abct4(x3, y3, r3, x1, y1, r1)
741 if t:
742 raise IntersectionError(_and(_astr(x3=x3, y3=y3, radius3=r3),
743 _astr(x1=x1, y1=y1, radius1=r1)), txt=t)
745 q = fdot_(a, e, -b, d) * _2_0
746 if isnear0(q):
747 t = _no_(_intersection_)
748 raise IntersectionError(_and(_astr(x1=x1, y1=y1, radius1=r1),
749 _astr(x2=x2, y2=y2, radius2=r2),
750 _astr(x3=x3, y3=y3, radius3=r3)), txt=t)
751 t = Vector2Tuple(fdot_(c, e, -b, f) / q,
752 fdot_(a, f, -c, d) / q, name=trilaterate2d2.__name__)
754 if eps and eps > 0: # check distances to center vs radius
755 for x, y, r in ((x1, y1, r1), (x2, y2, r2), (x3, y3, r3)):
756 d = hypot(x - t.x, y - t.y)
757 e = fabs(d - r)
758 if e > eps:
759 t = _and(Float(delta=e).toRepr(), r.toRepr(),
760 Float(distance=d).toRepr(), t.toRepr())
761 raise IntersectionError(t, txt=Fmt.exceeds_eps(eps))
763 if Vector is not None:
764 t = Vector(t.x, t.y, **_xkwds(Vector_kwds, name=t.name))
765 return t
768def _trilaterate3d2(c1, r1, c2, r2, c3, r3, eps=EPS4, coin=False, # MCCABE 13
769 **clas_Vector_and_kwds):
770 # (INTERNAL) Intersect three spheres or circles, see function
771 # L{pygeodesy.trilaterate3d2}, separated to allow callers to
772 # embellish exceptions, like C{FloatingPointError}s from C{numpy}
774 def _Arow4(c):
775 # make a row for matrix A (1, -2x, -2y, -2z)
776 return _1_0_1T + c.times(_N_2_0).xyz3
778 def _F4d3(F):
779 # map numpy 4-vector to floats and xyz3
780 T = map2(float, F)
781 t = T[1:]
782 return T, t, Vector3d(*t)
784 def _N3(t01, x, z):
785 # compute x, y and z and return as B{C{clas}} or B{C{Vector}}
786 v = x.plus(z.times(t01))
787 n = trilaterate3d2.__name__
788 return _nVc(v, **_xkwds(clas_Vector_and_kwds, name=n))
790 c2 = _otherV3d(center2=c2, NN_OK=False)
791 c3 = _otherV3d(center3=c3, NN_OK=False)
792 rs = (r1, Radius_(radius2=r2, low=EPS),
793 Radius_(radius3=r3, low=EPS))
795 # get matrix A[3 x 4], its null_space Z and pseudo-inverse
796 A = [_Arow4(c) for c in (c1, c2, c3)]
797 with _numpy(trilaterate3d2, A=A, eps=eps) as _np:
798 Z, _ = _np.null_space2(A, eps)
799 if Z is not None:
800 Z, _, z = _F4d3(Z) # [4 x 1]
801 z2 = z.length2
802 A = _np.pseudo_inverse(A) # [4 x 3]
803 bs = [c.length2 for c in (c1, c2, c3)]
804 # perturb radii slightly by eps and eps * 4
805 for p in _tri5perturbs(eps, min(rs)):
806 b = [((r + p)**2 - b) for r, b in zip(rs, bs)] # [3 x 1]
807 X, t, x = _F4d3(A.dot(b)) # [4 * 1]
808 # quadratic polynomial, coefficients order (^0, ^1, ^2)
809 t = _np.real_roots(fdot(X, _N_1_0, *t),
810 fdot(Z, _N_0_5, *t) * _2_0, z2)
811 if t:
812 v = _N3(t[0], x, z)
813 if len(t) < 2: # one intersection
814 t = v, v
815 elif fabs(t[0] - t[1]) < eps: # abutting
816 t = v, v
817 else: # "lowest" intersection first (to avoid test failures)
818 u = _N3(t[1], x, z)
819 t = (u, v) if u.x < v.x else (v, u)
820 return t
822 def _no_itersection(coin, Z):
823 t = _no_(_intersection_)
824 if coin:
825 def _reprs(*crs):
826 return _and(*map(repr, crs))
828 r = repr(r1) if r1 == r2 == r3 else _reprs(r1, r2, r3)
829 t = _SPACE_(t, _of_, _reprs(c1, c2, c3), _with_, _radius_, r)
830 elif Z is None:
831 t = _COMMASPACE_(t, _no_(_numpy.null_space2.__name__))
832 return t
834 # coincident, concentric, colinear, too distant, no intersection:
835 # create the explanation and and throw an IntersectionError
836 t = _tri4near2far(c1, r1, c2, r2, coin) or \
837 _tri4near2far(c1, r1, c3, r3, coin) or \
838 _tri4near2far(c2, r2, c3, r3, coin) or (
839 _colinear_ if _iscolinearWith(c1, c2, c3, eps=eps) else
840 _no_itersection(coin, Z))
841 raise IntersectionError(t, txt=None)
844def _tri3near2far(r1, r2, h, coin):
845 # check for near-coincident/-concentric or too distant spheres/circles
846 return _too_(Fmt.distant(h)) if h > (r1 + r2) else (_near_(
847 _coincident_ if coin else _concentric_) if h < fabs(r1 - r2) else NN)
850def _tri4near2far(c1, r1, c2, r2, coin):
851 # check for near-coincident/-concentric or too distant spheres/circles
852 t = _tri3near2far(r1, r2, c1.minus(c2).length, coin)
853 return _SPACE_(c1.name, _and_, c2.name, t) if t else NN
856def _tri5perturbs(eps, r):
857 # perturb the radii to handle this corner case
858 # <https://GitHub.com/mrJean1/PyGeodesy/issues/49>
859 yield _0_0
860 if eps and eps > 0:
861 p = max(eps, EPS)
862 yield p
863 m = min(p, r)
864 yield -m
865 q = max(eps * _4_0, _EPS4e8)
866 if q > p:
867 yield q
868 q = min(q, r)
869 if q > m:
870 yield -q
872# **) MIT License
873#
874# Copyright (C) 2016-2025 -- mrJean1 at Gmail -- All Rights Reserved.
875#
876# Permission is hereby granted, free of charge, to any person obtaining a
877# copy of this software and associated documentation files (the "Software"),
878# to deal in the Software without restriction, including without limitation
879# the rights to use, copy, modify, merge, publish, distribute, sublicense,
880# and/or sell copies of the Software, and to permit persons to whom the
881# Software is furnished to do so, subject to the following conditions:
882#
883# The above copyright notice and this permission notice shall be included
884# in all copies or substantial portions of the Software.
885#
886# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
887# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
888# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
889# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
890# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
891# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
892# OTHER DEALINGS IN THE SOFTWARE.