1 2# -*- coding: utf-8 -*- 3 4u'''2- or 3-D vectorial functions L{circin6}, L{circum3}, L{circum4_}, 5L{iscolinearWith}, L{meeus2}, L{nearestOn}, L{radii11}, L{soddy4}, \ 6L{trilaterate2d2} and L{trilaterate3d2}. 7''' 8 9from pygeodesy.basics import isnear0, len2, map1, map2, _xnumpy 10from pygeodesy.errors import _and, _AssertionError, IntersectionError, NumPyError, \ 11 PointsError, _xError, _xkwds 12from pygeodesy.fmath import fdot, fsum_, fsum1_, hypot, hypot2_ 13from pygeodesy.interns import EPS, EPS0, EPS02, EPS4, INF, NN, \ 14 _EPS4e8, _and_, _center_, _coincident_, _colinear_, \ 15 _concentric_, _COMMASPACE_, _few_, _intersection_, \ 16 _invalid_, _near_, _no_, _radius_, _s_, _SPACE_, \ 17 _too_, _0_0, _0_5, _1_0, _1_0_T, _2_0, _4_0 18from pygeodesy.lazily import _ALL_LAZY 19from pygeodesy.named import Fmt, _NamedTuple, _Pass 20from pygeodesy.namedTuples import LatLon3Tuple, Vector2Tuple 21# from pygeodesy.streprs import Fmt # from .named 22from pygeodesy.units import Float, Int, Meter, Radius, Radius_ 23from pygeodesy.vector3d import iscolinearWith, nearestOn, _nVc, _otherV3d, \ 24 trilaterate2d2, trilaterate3d2, Vector3d # PYCHOK unused 25 26from contextlib import contextmanager 27from math import sqrt 28 29__all__ = _ALL_LAZY.vector2d 30__version__ = '21.09.06' 31 32_cA_ = 'cA' 33_cB_ = 'cB' 34_cC_ = 'cC' 35_deltas_ = 'deltas' 36_numpy_1_10 = None # PYCHOK import numpy once 37_of_ = 'of' 38_outer_ = 'outer' 39_raise_ = 'raise' # PYCHOK used! 40_rank_ = 'rank' 41_residuals_ = 'residuals' 42_Type_ = 'Type' 43_with_ = 'with' 44 45 46class Circin6Tuple(_NamedTuple): 47 '''6-Tuple C{(radius, center, deltas, cA, cB, cC)} with the C{radius}, the 48 trilaterated C{center} and contact points of the I{inscribed} aka I{In- 49 circle} of a triangle. The C{center} is I{un}ambiguous if C{deltas} is 50 C{None}, otherwise C{center} is the mean and C{deltas} the differences of 51 the L{trilaterate3d2} results. Contact points C{cA}, C{cB} and C{cC} are 52 the points of tangency, aka the corners of the U{Contact Triangle 53 <https://MathWorld.Wolfram.com/ContactTriangle.html>}. 54 ''' 55 _Names_ = (_radius_, _center_, _deltas_, _cA_, _cB_, _cC_) 56 _Units_ = ( Radius, _Pass, _Pass, _Pass, _Pass, _Pass) 57 58 59class Circum3Tuple(_NamedTuple): # in .latlonBase 60 '''3-Tuple C{(radius, center, deltas)} with the C{circumradius} and trilaterated 61 C{circumcenter} of the C{circumcircle} through 3 points (aka {Meeus}' Type II 62 circle) or the C{radius} and C{center} of the smallest I{Meeus}' Type I circle. 63 The C{center} is I{un}ambiguous if C{deltas} is C{None}, otherwise C{center} 64 is the mean and C{deltas} the differences of the L{trilaterate3d2} results. 65 ''' 66 _Names_ = (_radius_, _center_, _deltas_) 67 _Units_ = ( Radius, _Pass, _Pass) 68 69 70class Circum4Tuple(_NamedTuple): 71 '''4-Tuple C{(radius, center, rank, residuals)} with C{radius} and C{center} 72 of a sphere I{least-squares} fitted through given points and the C{rank} 73 and C{residuals} -if any- from U{numpy.linalg.lstsq 74 <https://NumPy.org/doc/stable/reference/generated/numpy.linalg.lstsq.html>}. 75 ''' 76 _Names_ = (_radius_, _center_, _rank_, _residuals_) 77 _Units_ = ( Radius, _Pass, Int, _Pass) 78 79 80class Meeus2Tuple(_NamedTuple): 81 '''2-Tuple C{(radius, Type)} with C{radius} and I{Meeus}' C{Type} of the smallest 82 circle I{containing} 3 points. C{Type} is C{None} for a I{Meeus}' Type II 83 C{circumcircle} passing through all 3 points. Otherwise C{Type} is the center 84 of a I{Meeus}' Type I circle with 2 points on (a diameter of) and 1 point 85 inside the circle. 86 ''' 87 _Names_ = (_radius_, _Type_) 88 _Units_ = ( Radius, _Pass) 89 90 91class Radii11Tuple(_NamedTuple): 92 '''11-Tuple C{(rA, rB, rC, cR, rIn, riS, roS, a, b, c, s)} with the C{Tangent} 93 circle radii C{rA}, C{rB} and C{rC}, the C{circumradius} C{cR}, the C{Incircle} 94 radius C{rIn} aka C{inradius}, the inner and outer I{Soddy} circle radii C{riS} 95 and C{roS} and the sides C{a}, C{b} and C{c} and semi-perimeter C{s} of a 96 triangle, all in C{meter} conventionally. 97 98 @note: C{Circumradius} C{cR} and outer I{Soddy} radius C{roS} may be C{INF}. 99 ''' 100 _Names_ = ('rA', 'rB', 'rC', 'cR', 'rIn', 'riS', 'roS', 'a', 'b', 'c', _s_) 101 _Units_ = ( Meter,) * len(_Names_) 102 103 104class Soddy4Tuple(_NamedTuple): 105 '''4-Tuple C{(radius, center, deltas, outer)} with C{radius} and trilaterated 106 C{center} of the I{inner} I{Soddy} circle and the radius of the C{outer} 107 I{Soddy} circle. The C{center} is I{un}ambiguous if C{deltas} is C{None}, 108 otherwise C{center} is the mean and C{deltas} the differences of the 109 L{trilaterate3d2} results. 110 111 @note: The outer I{Soddy} radius C{outer} may be C{INF}. 112 ''' 113 _Names_ = (_radius_, _center_, _deltas_, _outer_) 114 _Units_ = ( Radius, _Pass, _Pass, Radius) 115 116 117def circin6(point1, point2, point3, eps=EPS4, useZ=True): 118 '''Return the radius and center of the I{inscribed} aka I{In- circle} 119 of a (2- or 3-D) triangle. 120 121 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 122 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 123 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 124 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 125 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 126 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 127 @kwarg eps: Tolerance for function L{trilaterate3d2} if C{B{useZ} is True} 128 else L{trilaterate2d2}. 129 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=0} (C{bool}). 130 131 @return: L{Circin6Tuple}C{(radius, center, deltas, cA, cB, cC)}. The 132 C{center} and contact points C{cA}, C{cB} and C{cC}, each an 133 instance of B{C{point1}}'s (sub-)class, are co-planar with 134 the three given points. 135 136 @raise ImportError: Package C{numpy} not found, not installed or older 137 than version 1.10 and C{B{useZ} is True}. 138 139 @raise IntersectionError: Near-coincident or colinear points or 140 a trilateration or C{numpy} issue. 141 142 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}. 143 144 @see: Functions L{radii11} and L{circum3}, U{Incircle 145 <https://MathWorld.Wolfram.com/Incircle.html>} and U{Contact Triangle 146 <https://MathWorld.Wolfram.com/ContactTriangle.html>}. 147 ''' 148 try: 149 return _circin6(point1, point2, point3, eps=eps, useZ=useZ) 150 except (AssertionError, TypeError, ValueError) as x: 151 raise _xError(x, point1=point1, point2=point2, point3=point3) 152 153 154def _circin6(point1, point2, point3, eps=EPS4, useZ=True, dLL3=False, **Vector_kwds): 155 # (INTERNAL) Radius, center, deltas, 3 contact points 156 157 def _fraction(r, a): 158 return (r / a) if a > EPS0 else _0_5 159 160 def _contact2(a, p2, r2, p3, r3, V, V_kwds): 161 c = p2.intermediateTo(p3, _fraction(r2, a)) if r2 > r3 else \ 162 p3.intermediateTo(p2, _fraction(r3, a)) 163 C = V(c.x, c.y, c.z, **V_kwds) 164 return c, C 165 166 t, p1, p2, p3 = _radii11ABC(point1, point2, point3, useZ=useZ) 167 V, r1, r2, r3 = point1.classof, t.rA, t.rB, t.rC 168 169 c1, cA = _contact2(t.a, p2, r2, p3, r3, V, _xkwds(Vector_kwds, name=_cA_)) 170 c2, cB = _contact2(t.b, p3, r3, p1, r1, V, _xkwds(Vector_kwds, name=_cB_)) 171 c3, cC = _contact2(t.c, p1, r1, p2, r2, V, _xkwds(Vector_kwds, name=_cC_)) 172 173 r = t.rIn 174 c, d = _tricenter3d2(c1, r, c2, r, c3, r, eps=eps, useZ=useZ, dLL3=dLL3, 175 **_xkwds(Vector_kwds, Vector=V, 176 name=circin6.__name__)) 177 return Circin6Tuple(r, c, d, cA, cB, cC) 178 179 180def circum3(point1, point2, point3, circum=True, eps=EPS4, useZ=True): 181 '''Return the radius and center of the smallest circle I{through} or I{containing} 182 three (2- or 3-D) points. 183 184 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or 185 C{Vector4Tuple}). 186 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or 187 C{Vector4Tuple}). 188 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or 189 C{Vector4Tuple}). 190 @kwarg circum: If C{True} return the C{circumradius} and C{circumcenter} 191 always, ignoring the I{Meeus}' Type I case (C{bool}). 192 @kwarg eps: Tolerance for function L{trilaterate3d2} if C{B{useZ} is True} 193 else L{trilaterate2d2}. 194 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=0} (C{bool}). 195 196 @return: A L{Circum3Tuple}C{(radius, center, deltas)}. The C{center}, an 197 instance of B{C{point1}}'s (sub-)class, is co-planar with the three 198 given points. 199 200 @raise ImportError: Package C{numpy} not found, not installed or older 201 than version 1.10 and C{B{useZ} is True}. 202 203 @raise IntersectionError: Near-coincident or colinear points or 204 a trilateration or C{numpy} issue. 205 206 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}. 207 208 @see: U{Jean Meeus, "Astronomical Algorithms", 2nd Ed. 1998, page 127ff 209 <http://www.Agopax.IT/Libri_astronomia/pdf/Astronomical%20Algorithms.pdf>}, 210 U{circumradius<https://MathWorld.Wolfram.com/Circumradius.html>}, 211 U{circumcircle<https://MathWorld.Wolfram.com/Circumcircle.html>} and 212 functions L{pygeodesy.circum4_} and L{pygeodesy.meeus2}. 213 ''' 214 try: 215 p1 = _otherV3d(useZ=useZ, point1=point1) 216 return _circum3(p1, point2, point3, circum=circum, eps=eps, useZ=useZ, 217 clas=point1.classof) 218 except (AssertionError, TypeError, ValueError) as x: 219 raise _xError(x, point1=point1, point2=point2, point3=point3, circum=circum) 220 221 222def _circum3(p1, point2, point3, circum=True, eps=EPS4, useZ=True, dLL3=False, 223 clas=Vector3d, **clas_kwds): # in .latlonBase 224 # (INTERNAL) Radius, center, deltas 225 r, d, p2, p3 = _meeus4(p1, point2, point3, circum=circum, useZ=useZ, 226 clas=clas, **clas_kwds) 227 if d is None: # Meeus' Type II or circum=True 228 kwds = _xkwds(clas_kwds, eps=eps, Vector=clas, name=circum3.__name__) 229 c, d = _tricenter3d2(p1, r, p2, r, p3, r, useZ=useZ, dLL3=dLL3, **kwds) 230 else: # Meeus' Type I 231 c, d = d, None 232 return Circum3Tuple(r, c, d) 233 234 235def circum4_(*points, **Vector_and_kwds): 236 '''Best-fit a sphere through three or more (3-D) points. 237 238 @arg points: The points (each a C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 239 or C{Vector4Tuple}). 240 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the center 241 and optional, additional B{C{Vector}} keyword arguments, 242 otherwise the first B{C{points}}' (sub-)class. 243 244 @return: L{Circum4Tuple}C{(radius, center, rank, residuals)} with C{center} an 245 instance of C{B{points}[0]}' (sub-)class or B{C{Vector}} if specified. 246 247 @raise ImportError: Package C{numpy} not found, not installed or older than 248 version 1.10. 249 250 @raise NumPyError: Some C{numpy} issue. 251 252 @raise PointsError: Too few B{C{points}}. 253 254 @raise TypeError: One of the B{C{points}} is invalid. 255 256 @see: U{Charles F. Jekel, "Least Squares Sphere Fit", Sep 13, 2015 257 <https://Jekel.me/2015/Least-Squares-Sphere-Fit/>} and U{Appendix A 258 <https://hdl.handle.net/10019.1/98627>}, U{numpy.linalg.lstsq 259 <https://NumPy.org/doc/stable/reference/generated/numpy.linalg.lstsq.html>}, 260 U{Eberly 6<https://www.sci.Utah.EDU/~balling/FEtools/doc_files/LeastSquaresFitting.pdf>} 261 and functions L{pygeodesy.circum3} and L{pygeodesy.meeus2}. 262 ''' 263 n, ps = len2(points) 264 if n < 3: 265 raise PointsError(points=n, txt=_too_(_few_)) 266 267 A, b = [], [] 268 for i, p in enumerate(ps): 269 v = _otherV3d(useZ=True, i=i, points=p) 270 A.append(v.times(_2_0).xyz + _1_0_T) 271 b.append(v.length2) 272 273 with _numpy(n, circum4_) as np: 274 A = np.array(A).reshape((n, 4)) 275 b = np.array(b).reshape((n, 1)) 276 C, R, rk, _ = np.linalg.lstsq(A, b, rcond=None) # to silence warning 277 C = map1(float, *C) 278 R = map1(float, *R) # empty if rk < 4 or n <= 4 279 280 n = circum4_.__name__ 281 c = Vector3d(*C[:3], name=n) 282 r = Radius(sqrt(fsum_(C[3], *c.x2y2z2)), name=n) 283 284 c = _nVc(c, **_xkwds(Vector_and_kwds, clas=ps[0].classof, name=n)) 285 return Circum4Tuple(r, c, rk, R) 286 287 288def _iscolinearWith(p, point1, point2, eps=EPS, useZ=True): 289 # (INTERNAL) Check colinear, see L{iscolinearWith} above, 290 # separated to allow callers to embellish any exceptions 291 p1 = _otherV3d(useZ=useZ, point1=point1) 292 p2 = _otherV3d(useZ=useZ, point2=point2) 293 n = _nearestOn(p, p1, p2, within=False, eps=eps) 294 return n is p1 or n.minus(p).length2 < eps 295 296 297def meeus2(point1, point2, point3, circum=False, useZ=True): 298 '''Return the radius and I{Meeus}' Type of the smallest circle I{through} 299 or I{containing} three (2- or 3-D) points. 300 301 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 302 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 303 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 304 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 305 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 306 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 307 @kwarg circum: If C{True} return the non-zero C{circumradius} always, 308 ignoring the I{Meeus}' Type I case (C{bool}). 309 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=0} (C{bool}). 310 311 @return: L{Meeus2Tuple}C{(radius, Type)}. 312 313 @raise IntersectionError: Near-coincident or colinear points, iff C{B{circum}=True}. 314 315 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}. 316 317 @see: U{Jean Meeus, "Astronomical Algorithms", 2nd Ed. 1998, page 127ff 318 <http://www.Agopax.IT/Libri_astronomia/pdf/Astronomical%20Algorithms.pdf>}, 319 U{circumradius<https://MathWorld.Wolfram.com/Circumradius.html>}, 320 U{circumcircle<https://MathWorld.Wolfram.com/Circumcircle.html>} and 321 functions L{pygeodesy.circum3} and L{pygeodesy.circum4_}. 322 ''' 323 try: 324 A = _otherV3d(useZ=useZ, point1=point1) 325 r, t, _, _ = _meeus4(A, point2, point3, circum=circum, useZ=useZ, 326 clas=point1.classof) 327 except (TypeError, ValueError) as x: 328 raise _xError(x, point1=point1, point2=point2, point3=point3, circum=circum) 329 return Meeus2Tuple(r, t) 330 331 332def _meeus4(A, point2, point3, circum=False, useZ=True, clas=None, **clas_kwds): 333 # (INTERNAL) Radius and Meeus' Type 334 B = p2 = _otherV3d(useZ=useZ, point2=point2) 335 C = p3 = _otherV3d(useZ=useZ, point3=point3) 336 337 a = B.minus(C).length2 338 b = C.minus(A).length2 339 c = A.minus(B).length2 340 if a < b: 341 a, b, A, B = b, a, B, A 342 if a < c: 343 a, c, A, C = c, a, C, A 344 345 if a > EPS02 and (circum or a < (b + c)): # circumradius 346 b = sqrt(b / a) 347 c = sqrt(c / a) 348 r = fsum1_(_1_0, b, c) * fsum1_(_1_0, b, -c) * fsum1_(-_1_0, b, c) * fsum1_(_1_0, -b, c) 349 if r < EPS02: 350 raise IntersectionError(_coincident_ if b < EPS0 or c < EPS0 else ( 351 _colinear_ if _iscolinearWith(A, B, C) else _invalid_)) 352 r = sqrt(a / r) * b * c 353 t = None # Meeus' Type II 354 else: # obtuse or right angle 355 r = 0 if a < EPS02 else sqrt(a) * _0_5 356 t = B.plus(C).times(_0_5) # Meeus' Type I 357 if clas is not None: 358 t = clas(t.x, t.y, t.z, **_xkwds(clas_kwds, name=meeus2.__name__)) 359 return r, t, p2, p3 360 361 362def _nearestOn(p0, p1, p2, within=True, eps=EPS): 363 # (INTERNAL) Get closest point, see L{nearestOn} above, 364 # separated to allow callers to embellish any exceptions 365 p21 = p2.minus(p1) 366 d2 = p21.length2 367 if d2 < eps: # coincident 368 p = p1 # ~= p2 369 else: 370 t = p0.minus(p1).dot(p21) / d2 371 p = p1 if (within and t < eps) else ( 372 p2 if (within and t > (_1_0 - eps)) else 373 p1.plus(p21.times(t))) 374 return p 375 376 377def _null_space2(numpy, A, eps): 378 # (INTERNAL) Return the nullspace and rank of matrix A 379 # @see: <https://SciPy-Cookbook.ReadTheDocs.io/items/RankNullspace.html>, 380 # <https://NumPy.org/doc/stable/reference/generated/numpy.linalg.svd.html>, 381 # <https://StackOverflow.com/questions/19820921>, 382 # <https://StackOverflow.com/questions/2992947> and 383 # <https://StackOverflow.com/questions/5889142> 384 A = numpy.array(A) 385 m = max(numpy.shape(A)) 386 if m != 4: # for this usage 387 raise _AssertionError(shape=m, txt=_null_space2.__name__) 388 # if needed, square A, pad with zeros 389 A = numpy.resize(A, m * m).reshape(m, m) 390# try: # no numpy.linalg.null_space <https://docs.SciPy.org/doc/> 391# return scipy.linalg.null_space(A) # XXX no scipy.linalg? 392# except AttributeError: 393# pass 394 _, s, v = numpy.linalg.svd(A) 395 t = max(eps, eps * s[0]) # tol, s[0] is largest singular 396 r = numpy.sum(s > t) # rank 397 if r == 3: # get null_space 398 n = numpy.transpose(v[r:]) 399 s = numpy.shape(n) 400 if s != (m, 1): # bad null_space shape 401 raise _AssertionError(shape=s, txt=_null_space2.__name__) 402 e = float(numpy.max(numpy.abs(numpy.dot(A, n)))) 403 if e > t: # residual not near-zero 404 raise _AssertionError(res=e, tol=t, txt=_null_space2.__name__) 405 else: # coincident, colinear, concentric centers, ambiguous, etc. 406 n = None 407 # del A, s, vh # release numpy 408 return n, r 409 410 411@contextmanager # <https://www.python.org/dev/peps/pep-0343/> Examples 412def _numpy(arg, where): 413 # (INTERNAL) Yield numpy with any errors raised as NumPyError 414 global _numpy_1_10 415 np = _numpy_1_10 416 if np is None: 417 _numpy_1_10 = np = _xnumpy(where, 1, 10) 418 419 try: # <https://NumPy.org/doc/stable/reference/generated/numpy.seterr.html> 420 e = np.seterr(all=_raise_) # throw FloatingPointError for numpy errors 421 yield np 422 except Exception as x: # mostly FloatingPointError? 423 raise NumPyError(x.__class__.__name__, arg, txt=str(x)) 424 finally: # restore numpy error handling 425 np.seterr(**e) 426 427 428def radii11(point1, point2, point3, useZ=True): 429 '''Return the radii of the C{In-}, I{Soddy} and C{Tangent} circles of a 430 (2- or 3-D) triangle. 431 432 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 433 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 434 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 435 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 436 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 437 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 438 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=0} (C{bool}). 439 440 @return: L{Radii11Tuple}C{(rA, rB, rC, cR, rIn, riS, roS, a, b, c, s)}. 441 442 @raise IntersectionError: Near-coincident or colinear points. 443 444 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}. 445 446 @see: U{Circumradius<https://MathWorld.Wolfram.com/Circumradius.html>}, 447 U{Incircle<https://MathWorld.Wolfram.com/Incircle.html>}, U{Soddy 448 Circles<https://MathWorld.Wolfram.com/SoddyCircles.html>} and 449 U{Tangent Circles<https://MathWorld.Wolfram.com/TangentCircles.html>}. 450 ''' 451 try: 452 return _radii11ABC(point1, point2, point3, useZ=useZ)[0] 453 except (TypeError, ValueError) as x: 454 raise _xError(x, point1=point1, point2=point2, point3=point3) 455 456 457def _radii11ABC(point1, point2, point3, useZ=True): 458 # (INTERNAL) Tangent, Circum, Incircle, Soddy radii, sides and semi-perimeter 459 A = _otherV3d(useZ=useZ, point1=point1, NN_OK=False) 460 B = _otherV3d(useZ=useZ, point2=point2, NN_OK=False) 461 C = _otherV3d(useZ=useZ, point3=point3, NN_OK=False) 462 463 a = B.minus(C).length 464 b = C.minus(A).length 465 c = A.minus(B).length 466 467 s = fsum1_(a, b, c) * _0_5 # semi-perimeter 468 if s > EPS0: 469 rs = (s - a), (s - b), (s - c) 470 r3, r2, r1 = sorted(rs) # r3 <= r2 <= r1 471 if r3 > EPS0: # and r2 > EPS0 and r1 > EPS0 472 r3_r1 = r3 / r1 473 r3_r2 = r3 / r2 474 # t = r1 * r2 * r3 * (r1 + r2 + r3) 475 # = r1**2 * r2 * r3 * (1 + r2 / r1 + r3 / r1) 476 # = (r1 * r2)**2 * (r3 / r2) * (1 + r2 / r1 + r3 / r1) 477 t = r3_r2 * fsum1_(_1_0, r2 / r1, r3_r1) # * (r1 * r2)**2 478 if t > EPS02: 479 t = sqrt(t) * _2_0 # * r1 * r2 480 # d = r1 * r2 + r2 * r3 + r3 * r1 481 # = r1 * (r2 + r2 * r3 / r1 + r3) 482 # = r1 * r2 * (1 + r3 / r1 + r3 / r2) 483 d = fsum1_(_1_0, r3_r1, r3_r2) # * r1 * r2 484 # si/o = r1 * r2 * r3 / (r1 * r2 * (d +/- t)) 485 # = r3 / (d +/- t) 486 si = r3 / (d + t) 487 so = (r3 / (d - t)) if d > t else INF 488 # ci = sqrt(r1 * r2 * r3 / s) 489 # = r1 * sqrt(r2 * r3 / r1 / s) 490 ci = r1 * sqrt(r2 * r3_r1 / s) 491 # co = a * b * c / (4 * ci * s) 492 t = _4_0 * s * ci 493 co = (a * b * c / t) if t > EPS0 else INF 494 r1, r2, r3 = rs # original order 495 t = Radii11Tuple(r1, r2, r3, co, ci, si, so, a, b, c, s) 496 return t, A, B, C 497 498 raise IntersectionError(_near_(_coincident_) if min(a, b, c) < EPS0 else ( 499 _colinear_ if _iscolinearWith(A, B, C) else _invalid_)) 500 501 502def soddy4(point1, point2, point3, eps=EPS4, useZ=True): 503 '''Return the radius and center of the C{inner} I{Soddy} circle of a 504 (2- or 3-D) triangle. 505 506 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 507 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 508 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 509 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 510 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 511 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 512 @kwarg eps: Tolerance for function L{trilaterate3d2} if C{B{useZ} is True} 513 else L{trilaterate2d2}. 514 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=0} (C{bool}). 515 516 @return: L{Soddy4Tuple}C{(radius, center, deltas, outer)}. The C{center}, 517 an instance of B{C{point1}}'s (sub-)class, is co-planar with the 518 three given points. 519 520 @raise ImportError: Package C{numpy} not found, not installed or older 521 than version 1.10 and C{B{useZ} is True}. 522 523 @raise IntersectionError: Near-coincident or colinear points or 524 a trilateration or C{numpy} issue. 525 526 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}. 527 528 @see: Functions L{radii11} and L{circum3}. 529 ''' 530 t, p1, p2, p3 = _radii11ABC(point1, point2, point3, useZ=useZ) 531 532 r = t.riS 533 c, d = _tricenter3d2(p1, t.rA + r, 534 p2, t.rB + r, 535 p3, t.rC + r, eps=eps, useZ=useZ, 536 Vector=point1.classof, name=soddy4.__name__) 537 return Soddy4Tuple(r, c, d, t.roS) 538 539 540def _tricenter3d2(p1, r1, p2, r2, p3, r3, eps=EPS4, useZ=True, dLL3=False, **kwds): 541 # (INTERNAL) Trilaterate and disambiguate the 3-D center 542 d, kwds = None, _xkwds(kwds, eps=eps, coin=True) 543 if useZ and p1.z != p2.z != p3.z: # ignore z if all match 544 a, b = _trilaterate3d2(p1, r1, p2, r2, p3, r3, **kwds) 545 if a is b: # no unambiguity 546 c = a # == b 547 else: 548 c = a.plus(b).times(_0_5) # mean 549 if not a.isconjugateTo(b, minum=0, eps=eps): 550 if dLL3: # deltas as (lat, lon, height) 551 a = a.toLatLon() 552 b = b.toLatLon() 553 d = LatLon3Tuple(b.lat - a.lat, 554 b.lon - a.lon, 555 b.height - a.height, name=_deltas_) 556 else: 557 d = b.minus(a) # vectorial deltas 558 else: 559 if useZ: # pass z to Vector if given 560 kwds = _xkwds(kwds, z=p1.z) 561 c = _trilaterate2d2(p1.x, p1.y, r1, 562 p2.x, p2.y, r2, 563 p3.x, p3.y, r3, **kwds) 564 return c, d 565 566 567def _trilaterate2d2(x1, y1, radius1, x2, y2, radius2, x3, y3, radius3, 568 coin=False, eps=None, 569 Vector=None, **Vector_kwds): 570 # (INTERNAL) Trilaterate three circles, see L{trilaterate2d2} above 571 572 def _abct4(x1, y1, r1, x2, y2, r2): 573 a = x2 - x1 574 b = y2 - y1 575 t = _trinear2far(r1, r2, hypot(a, b), coin) 576 c = _0_0 if t else (hypot2_(r1, x2, y2) - hypot2_(r2, x1, y1)) 577 return a, b, c, t 578 579 def _astr(**kwds): # kwds as (name=value, ...) strings 580 return Fmt.PAREN(_COMMASPACE_(*(Fmt.EQUAL(*t) for t in kwds.items()))) 581 582 r1 = Radius_(radius1=radius1) 583 r2 = Radius_(radius2=radius2) 584 r3 = Radius_(radius3=radius3) 585 586 a, b, c, t = _abct4(x1, y1, r1, x2, y2, r2) 587 if t: 588 raise IntersectionError(_and(_astr(x1=x1, y1=y1, radius1=r1), 589 _astr(x2=x2, y2=y2, radius2=r2)), txt=t) 590 591 d, e, f, t = _abct4(x2, y2, r2, x3, y3, r3) 592 if t: 593 raise IntersectionError(_and(_astr(x2=x2, y2=y2, radius2=r2), 594 _astr(x3=x3, y3=y3, radius3=r3)), txt=t) 595 596 _, _, _, t = _abct4(x3, y3, r3, x1, y1, r1) 597 if t: 598 raise IntersectionError(_and(_astr(x3=x3, y3=y3, radius3=r3), 599 _astr(x1=x1, y1=y1, radius1=r1)), txt=t) 600 601 q = (a * e - b * d) * _2_0 602 if isnear0(q): 603 t = _no_(_intersection_) 604 raise IntersectionError(_and(_astr(x1=x1, y1=y1, radius1=r1), 605 _astr(x2=x2, y2=y2, radius2=r2), 606 _astr(x3=x3, y3=y3, radius3=r3)), txt=t) 607 t = Vector2Tuple((c * e - b * f) / q, 608 (a * f - c * d) / q, name=trilaterate2d2.__name__) 609 610 if eps and eps > 0: 611 for x, y, r in ((x1, y1, r1), (x2, y2, r2), (x3, y3, r3)): 612 d = hypot(x - t.x, y - t.y) 613 e = abs(d - r) 614 if e > eps: 615 t = _and(Float(delta=e).toRepr(), r.toRepr(), 616 Float(distance=d).toRepr(), t.toRepr()) 617 raise IntersectionError(t, txt=Fmt.exceeds_eps(eps)) 618 619 if Vector is not None: 620 t = Vector(t.x, t.y, **_xkwds(Vector_kwds, name=t.name)) 621 return t 622 623 624def _trilaterate3d2(c1, r1, c2, r2, c3, r3, eps=EPS, coin=False, # MCCABE 14 625 **clas_Vector_and_kwds): 626 # (INTERNAL) Intersect three spheres or circles, see L{trilaterate3d2} 627 # above, separated to allow callers to embellish any exceptions, like 628 # C{FloatingPointError}s from C{numpy} 629 630 def _F3d2(F): 631 # map numpy 4-vector to floats tuple and Vector3d 632 T = map2(float, F) 633 return T, Vector3d(*T[1:]) 634 635 def _N3(t01, x, z): 636 # compute x, y and z and return as B{C{clas}} or B{C{Vector}} 637 v = x.plus(z.times(t01)) 638 n = trilaterate3d2.__name__ 639 return _nVc(v, **_xkwds(clas_Vector_and_kwds, name=n)) 640 641 def _perturbe5(eps, r): 642 # perturbe radii to handle corner cases like this 643 # <https://GitHub.com/mrJean1/PyGeodesy/issues/49> 644 yield _0_0 645 if eps and eps > 0: 646 p = max(eps, EPS) 647 yield p 648 yield -min(p, r) 649 q = max(eps, _EPS4e8) 650 if q > p: 651 yield q 652 q = min(q, r) 653 if q != min(p, r): 654 yield -q 655 656 def _roots(numpy, *coeffs): 657 # only real, non-complex roots of a polynomial, if any 658 rs = numpy.polynomial.polynomial.polyroots(coeffs) 659 return tuple(float(r) for r in rs if not numpy.iscomplex(r)) 660 661 c2 = _otherV3d(center2=c2, NN_OK=False) 662 c3 = _otherV3d(center3=c3, NN_OK=False) 663 rs = [r1, Radius_(radius2=r2, low=eps), 664 Radius_(radius3=r3, low=eps)] 665 666 # get null_space Z, pseudo-inverse A and vector B, once 667 A = [(_1_0_T + c.times(-_2_0).xyz) for c in (c1, c2, c3)] # 3 x 4 668 with _numpy(None, trilaterate3d2) as np: 669 Z, _ = _null_space2(np, A, eps) 670 A = np.linalg.pinv(A) # Moore-Penrose pseudo-inverse 671 if Z is None: # coincident, colinear, concentric, etc. 672 raise _trilaterror(c1, r1, c2, r2, c3, r3, eps, coin) 673 Z, z = _F3d2(Z) 674 z2 = z.length2 675 bs = [c.length2 for c in (c1, c2, c3)] 676 677 for p in _perturbe5(eps, min(rs)): 678 b = [((r + p)**2 - b) for r, b in zip(rs, bs)] # 1 x 3 or 3 x 1 679 with _numpy(p, trilaterate3d2) as np: 680 X, x = _F3d2(np.dot(A, b)) 681 # quadratic polynomial coefficients, ordered (^0, ^1, ^2) 682 t = _roots(np, fdot(X, -_1_0, *x.xyz), 683 (fdot(Z, -_0_5, *x.xyz) * _2_0), z2) 684 if t: 685 break 686 else: # coincident, concentric, colinear, too distant, no intersection, etc. 687 raise _trilaterror(c1, r1, c2, r2, c3, r3, eps, coin) 688 689 v = _N3(t[0], x, z) 690 if len(t) < 2: # one intersection 691 t = v, v 692 elif abs(t[0] - t[1]) < eps: # abutting 693 t = v, v 694 else: # "lowest" intersection first (to avoid test failures) 695 u = _N3(t[1], x, z) 696 t = (u, v) if u.x < v.x else (v, u) 697 return t 698 699 700def _trilaterror(c1, r1, c2, r2, c3, r3, eps, coin): 701 # return IntersectionError with the cause of the error 702 703 def _no_intersection(): 704 t = _no_(_intersection_) 705 if coin: 706 def _reprs(*crs): 707 return _and(*map(repr, crs)) 708 709 r = repr(r1) if r1 == r2 == r3 else _reprs(r1, r2, r3) 710 t = _SPACE_(t, _of_, _reprs(c1, c2, c3), _with_, _radius_, r) 711 return t 712 713 def _txt(c1, r1, c2, r2): 714 t = _trinear2far(r1, r2, c1.minus(c2).length, coin) 715 return _SPACE_(c1.name, _and_, c2.name, t) if t else t 716 717 t = _txt(c1, r1, c2, r2) or \ 718 _txt(c1, r1, c3, r3) or \ 719 _txt(c2, r2, c3, r3) or ( 720 _colinear_ if _iscolinearWith(c1, c2, c3, eps=eps) else 721 _no_intersection()) 722 return IntersectionError(t, txt=None) 723 724 725def _trinear2far(r1, r2, h, coin): 726 # check for near-coincident/-concentric or too distant spheres/circles 727 return _too_(Fmt.distant(h)) if h > (r1 + r2) else (_near_( 728 _coincident_ if coin else _concentric_) if h < abs(r1 - r2) else NN) 729 730# **) MIT License 731# 732# Copyright (C) 2016-2021 -- mrJean1 at Gmail -- All Rights Reserved. 733# 734# Permission is hereby granted, free of charge, to any person obtaining a 735# copy of this software and associated documentation files (the "Software"), 736# to deal in the Software without restriction, including without limitation 737# the rights to use, copy, modify, merge, publish, distribute, sublicense, 738# and/or sell copies of the Software, and to permit persons to whom the 739# Software is furnished to do so, subject to the following conditions: 740# 741# The above copyright notice and this permission notice shall be included 742# in all copies or substantial portions of the Software. 743# 744# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 745# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 746# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 747# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR 748# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 749# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 750# OTHER DEALINGS IN THE SOFTWARE. 751