1 2# -*- coding: utf-8 -*- 3 4u'''Extended 3-D vector class L{Vector3d} and functions. 5 6Function L{intersection3d3}, L{intersections2}, L{parse3d} and L{sumOf}. 7''' 8 9from pygeodesy.basics import len2, map2 10from pygeodesy.errors import IntersectionError, _TypeError, _ValueError, \ 11 VectorError, _xError, _xkwds, _xkwds_popitem 12from pygeodesy.fmath import fdot, fsum, fsum1_ 13from pygeodesy.formy import _radical2 14from pygeodesy.interns import EPS, EPS0, EPS1, EPS4, MISSING, NN, _COMMA_, \ 15 _concentric_, _datum_, _h_, _height_, \ 16 _intersection_, _name_, _near_, _negative_, \ 17 _no_, _too_, _xyz_, _y_, _z_, _0_0, _1_0 18from pygeodesy.lazily import _ALL_DOCS, _ALL_LAZY 19from pygeodesy.named import _xnamed, _xotherError 20from pygeodesy.namedTuples import Intersection3Tuple, Vector3Tuple # Vector4Tuple 21from pygeodesy.streprs import Fmt 22from pygeodesy.units import Radius, Radius_ 23from pygeodesy.vector3dBase import Vector3dBase 24 25from math import sqrt 26 27__all__ = _ALL_LAZY.vector3d 28__version__ = '21.09.07' 29 30 31class Vector3d(Vector3dBase): 32 '''Extended 3-D vector. 33 34 In a geodesy context, these may be used to represent: 35 - n-vector representing a normal to a point on earth's surface 36 - earth-centered, earth-fixed cartesian (ECEF) 37 - great circle normal to vector 38 - motion vector on earth's surface 39 - etc. 40 ''' 41 42 def circin6(self, point2, point3, eps=EPS4): 43 '''Return the radius and center of the I{inscribed} aka I{In- circle} 44 of a (3-D) triangle formed by this and two other points. 45 46 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 47 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 48 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 49 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 50 @kwarg eps: Tolerance for function L{trilaterate3d2} if C{B{useZ} is True} 51 else L{trilaterate2d2}. 52 53 @return: L{Circin6Tuple}C{(radius, center, deltas, cA, cB, cC)}. The 54 C{center} and contact points C{cA}, C{cB} and C{cC}, each an 55 instance of this (sub-)class, are co-planar with this and the 56 two given points. 57 58 @raise ImportError: Package C{numpy} not found, not installed or older 59 than version 1.10. 60 61 @raise IntersectionError: Near-coincident or colinear points or 62 a trilateration or C{numpy} issue. 63 64 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}. 65 66 @see: Function L{pygeodesy.circin6}, U{Incircle 67 <https://MathWorld.Wolfram.com/Incircle.html>} and U{Contact 68 Triangle<https://MathWorld.Wolfram.com/ContactTriangle.html>}. 69 ''' 70 from pygeodesy.vector2d import _circin6 71 try: 72 return _circin6(self, point2, point3, eps=eps, useZ=True) 73 except (AssertionError, TypeError, ValueError) as x: 74 raise _xError(x, point=self, point2=point2, point3=point3) 75 76 def circum3(self, point2, point3, circum=True, eps=EPS4): 77 '''Return the radius and center of the smallest circle I{through} or 78 I{containing} this and two other (3-D) points. 79 80 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 81 or C{Vector4Tuple}). 82 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 83 or C{Vector4Tuple}). 84 @kwarg circum: If C{True} return the C{circumradius} and C{circumcenter}, 85 always, ignoring the I{Meeus}' Type I case (C{bool}). 86 @kwarg eps: Tolerance passed to function L{trilaterate3d2}. 87 88 @return: A L{Circum3Tuple}C{(radius, center, deltas)}. The C{center}, an 89 instance of this (sub-)class, is co-planar with this and the two 90 given points. 91 92 @raise ImportError: Package C{numpy} not found, not installed or older than 93 version 1.10. 94 95 @raise IntersectionError: Near-concentric, coincident or colinear points or 96 a trilateration or C{numpy} issue. 97 98 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}. 99 100 @see: Function L{pygeodesy.circum3} and methods L{circum4_} and L{meeus2}. 101 ''' 102 from pygeodesy.vector2d import _circum3 103 try: 104 return _circum3(self, point2, point3, circum=circum, eps=eps, useZ=True, 105 clas=self.classof) 106 except (AssertionError, TypeError, ValueError) as x: 107 raise _xError(x, point=self, point2=point2, point3=point3, circum=circum) 108 109 def circum4_(self, *points): 110 '''Best-fit a sphere through this and two or more other (3-D) points. 111 112 @arg points: Other points (each a C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 113 or C{Vector4Tuple}). 114 115 @return: L{Circum4Tuple}C{(radius, center, rank, residuals)} with C{center} 116 an instance if this (sub-)class. 117 118 @raise ImportError: Package C{numpy} not found, not installed or 119 older than version 1.10. 120 121 @raise NumPyError: Some C{numpy} issue. 122 123 @raise PointsError: Too few B{C{points}}. 124 125 @raise TypeError: One of the B{C{points}} invalid. 126 127 @see: Function L{pygeodesy.circum4_} and methods L{circum3} and L{meeus2}. 128 ''' 129 from pygeodesy.vector2d import circum4_ 130 return circum4_(self, *points, Vector=self.classof) 131 132 def iscolinearWith(self, point1, point2, eps=EPS): 133 '''Check whether this and two other (3-D) points are colinear. 134 135 @arg point1: One point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 136 or C{Vector4Tuple}). 137 @arg point2: An other point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 138 or C{Vector4Tuple}). 139 @kwarg eps: Tolerance (C{scalar}), same units as C{x}, 140 C{y}, and C{z}. 141 142 @return: C{True} if this point is colinear with B{C{point1}} and 143 B{C{point2}}, C{False} otherwise. 144 145 @raise TypeError: Invalid B{C{point1}} or B{C{point2}}. 146 147 @see: Method L{nearestOn}. 148 ''' 149 from pygeodesy.vector2d import _iscolinearWith 150 v = self if self.name else _otherV3d(NN_OK=False, this=self) 151 return _iscolinearWith(v, point1, point2, eps=eps) 152 153 def meeus2(self, point2, point3, circum=False): 154 '''Return the radius and I{Meeus}' Type of the smallest circle 155 I{through} or I{containing} this and two other (3-D) points. 156 157 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 158 or C{Vector4Tuple}). 159 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 160 or C{Vector4Tuple}). 161 @kwarg circum: If C{True} return the C{circumradius} and C{circumcenter} 162 always, ignoring the I{Meeus}' Type I case (C{bool}). 163 164 @return: L{Meeus2Tuple}C{(radius, Type)}. 165 166 @raise IntersectionError: Coincident or colinear points, iff C{B{circum}=True}. 167 168 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}. 169 170 @see: Function L{pygeodesy.meeus2} and methods L{circum3} and L{circum4_}. 171 ''' 172 from pygeodesy.vector2d import _meeus4, Meeus2Tuple 173 try: 174 r, t, _, _ = _meeus4(self, point2, point3, circum=circum, clas=self.classof) 175 except (TypeError, ValueError) as x: 176 raise _xError(x, point=self, point2=point2, point3=point3, circum=circum) 177 return Meeus2Tuple(r, t) 178 179 def nearestOn(self, point1, point2, within=True): 180 '''Locate the point between two points closest to this point. 181 182 @arg point1: Start point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 183 or C{Vector4Tuple}). 184 @arg point2: End point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 185 or C{Vector4Tuple}). 186 @kwarg within: If C{True} return the closest point between 187 the given points, otherwise the closest 188 point on the extended line through both 189 points (C{bool}). 190 191 @return: Closest point, either B{C{point1}} or B{C{point2}} or an 192 instance of this (sub-)class. 193 194 @raise TypeError: Invalid B{C{point1}} or B{C{point2}}. 195 196 @see: Method L{sphericalTrigonometry.LatLon.nearestOn3} and 197 U{3-D Point-Line Distance<https://MathWorld.Wolfram.com/ 198 Point-LineDistance3-Dimensional.html>}. 199 ''' 200 from pygeodesy.vector2d import _nearestOn 201 return _nearestOn(self, point1, point2, within=within) 202 203 def parse(self, str3d, sep=_COMMA_, name=NN): 204 '''Parse an C{"x, y, z"} string to a L{Vector3d} instance. 205 206 @arg str3d: X, y and z string (C{str}), see function L{parse3d}. 207 @kwarg sep: Optional separator (C{str}). 208 @kwarg name: Optional instance name (C{str}), overriding this name. 209 210 @return: The instance (L{Vector3d}). 211 212 @raise VectorError: Invalid B{C{str3d}}. 213 ''' 214 return parse3d(str3d, sep=sep, Vector=self.classof, name=name or self.name) 215 216 def radii11(self, point2, point3): 217 '''Return the radii of the C{Circum-}, C{In-}, I{Soddy} and C{Tangent} 218 circles of a (3-D) triangle. 219 220 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 221 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 222 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 223 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 224 225 @return: L{Radii11Tuple}C{(rA, rB, rC, cR, rIn, riS, roS, a, b, c, s)}. 226 227 @raise IntersectionError: Near-coincident or colinear points. 228 229 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}. 230 231 @see: Function L{pygeodesy.radii11}, U{Incircle 232 <https://MathWorld.Wolfram.com/Incircle.html>}, U{Soddy Circles 233 <https://MathWorld.Wolfram.com/SoddyCircles.html>} and U{Tangent 234 Circles<https://MathWorld.Wolfram.com/TangentCircles.html>}. 235 ''' 236 from pygeodesy.vector2d import _radii11ABC 237 try: 238 return _radii11ABC(self, point2, point3, useZ=True)[0] 239 except (TypeError, ValueError) as x: 240 raise _xError(x, point=self, point2=point2, point3=point3) 241 242 def soddy4(self, point2, point3, eps=EPS4): 243 '''Return the radius and center of the C{inner} I{Soddy} circle of a 244 (3-D) triangle. 245 246 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 247 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 248 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 249 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 250 @kwarg eps: Tolerance for function L{trilaterate3d2} if C{B{useZ} is True} 251 else L{trilaterate2d2}. 252 253 @return: L{Soddy4Tuple}C{(radius, center, deltas, outer)}. The C{center}, 254 an instance of B{C{point1}}'s (sub-)class, is co-planar with the 255 three given points. 256 257 @raise ImportError: Package C{numpy} not found, not installed or older 258 than version 1.10. 259 260 @raise IntersectionError: Near-coincident or colinear points or 261 a trilateration or C{numpy} issue. 262 263 @raise TypeError: Invalid B{C{point2}} or B{C{point3}}. 264 265 @see: Function L{pygeodesy.soddy4}. 266 ''' 267 from pygeodesy.vector2d import soddy4 268 return soddy4(self, point2, point3, eps=eps, useZ=True) 269 270 def trilaterate2d2(self, radius, center2, radius2, center3, radius3, eps=EPS, z=0): 271 '''Trilaterate this and two other circles, each given as a (2-D) center 272 and a radius. 273 274 @arg radius: Radius of this circle (same C{units} as this C{x} and C{y}. 275 @arg center2: Center of the 2nd circle (C{Cartesian}, L{Vector3d}, 276 C{Vector2Tuple}, C{Vector3Tuple} or C{Vector4Tuple}). 277 @arg radius2: Radius of this circle (same C{units} as this C{x} and C{y}. 278 @arg center3: Center of the 3rd circle (C{Cartesian}, L{Vector3d}, 279 C{Vector2Tuple}, C{Vector3Tuple} or C{Vector4Tuple}). 280 @arg radius3: Radius of the 3rd circle (same C{units} as this C{x} and C{y}. 281 @kwarg eps: Check the trilaterated point I{delta} on all 3 circles (C{scalar}) 282 or C{None}. 283 @kwarg z: Optional Z component of the trilaterated point (C{scalar}). 284 285 @return: Trilaterated point, an instance of this (sub-)class with C{z=B{z}}. 286 287 @raise IntersectionError: No intersection, colinear or near-concentric 288 centers, trilateration failed some other way 289 or the trilaterated point is off one circle 290 by more than B{C{eps}}. 291 292 @raise TypeError: Invalid B{C{center2}} or B{C{center3}}. 293 294 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}. 295 296 @see: Function L{pygeodesy.trilaterate2d2}. 297 ''' 298 from pygeodesy.vector2d import _trilaterate2d2 299 300 def _xyr3(r, **name_v): 301 v = _otherV3d(useZ=False, **name_v) 302 return v.x, v.y, r 303 304 try: 305 return _trilaterate2d2(*(_xyr3(radius, center=self) + 306 _xyr3(radius2, center2=center2) + 307 _xyr3(radius3, center3=center3)), 308 eps=eps, Vector=self.classof, z=z) 309 except (AssertionError, TypeError, ValueError) as x: 310 raise _xError(x, center=self, radius=radius, 311 center2=center2, radius2=radius2, 312 center3=center3, radius3=radius3) 313 314 def trilaterate3d2(self, radius, center2, radius2, center3, radius3, eps=EPS): 315 '''Trilaterate this and two other spheres, each given as a (3-D) center 316 and a radius. 317 318 @arg radius: Radius of this sphere (same C{units} as this C{x}, C{y} 319 and C{z}). 320 @arg center2: Center of the 2nd sphere (C{Cartesian}, L{Vector3d}, 321 C{Vector3Tuple} or C{Vector4Tuple}). 322 @arg radius2: Radius of this sphere (same C{units} as this C{x}, C{y} 323 and C{z}). 324 @arg center3: Center of the 3rd sphere (C{Cartesian}, , L{Vector3d}, 325 C{Vector3Tuple} or C{Vector4Tuple}). 326 @arg radius3: Radius of the 3rd sphere (same C{units} as this C{x}, C{y} 327 and C{z}). 328 @kwarg eps: Tolerance (C{scalar}), same units as C{x}, C{y}, and C{z}. 329 330 @return: 2-Tuple with two trilaterated points, each an instance of this 331 (sub-)class. Both points are the same instance if all three 332 spheres intersect or abut in a single point. 333 334 @raise ImportError: Package C{numpy} not found, not installed or 335 older than version 1.10. 336 337 @raise IntersectionError: Near-concentric, colinear, too distant or 338 non-intersecting spheres or C{numpy} issue. 339 340 @raise TypeError: Invalid B{C{center2}} or B{C{center3}}. 341 342 @raise UnitError: Invalid B{C{radius}}, B{C{radius2}} or B{C{radius3}}. 343 344 @note: Package U{numpy<https://pypi.org/project/numpy>} is required, 345 version 1.10 or later. 346 347 @see: Norrdine, A. U{I{An Algebraic Solution to the Multilateration 348 Problem}<https://www.ResearchGate.net/publication/ 349 275027725_An_Algebraic_Solution_to_the_Multilateration_Problem>} 350 and U{I{implementation}<https://www.ResearchGate.net/publication/ 351 288825016_Trilateration_Matlab_Code>}. 352 ''' 353 from pygeodesy.vector2d import _trilaterate3d2 354 try: 355 return _trilaterate3d2(_otherV3d(center=self, NN_OK=False), 356 Radius_(radius, low=eps), 357 center2, radius2, center3, radius3, 358 eps=eps, clas=self.classof) 359 except (AssertionError, TypeError, ValueError) as x: 360 raise _xError(x, center=self, radius=radius, 361 center2=center2, radius2=radius2, 362 center3=center3, radius3=radius3) 363 364 365def _intersect3d3(start1, end1, start2, end2, eps=EPS, useZ=False): 366 # (INTERNAL) Intersect two lines, see L{intersection} above, 367 # separated to allow callers to embellish any exceptions 368 369 def _outside(t, d2): # -1 before start#, +1 after end# 370 return -1 if t < 0 else (+1 if t > d2 else 0) # XXX d2 + eps? 371 372 s1 = _otherV3d(useZ=useZ, start1=start1) 373 e1 = _otherV3d(useZ=useZ, end1=end1) 374 s2 = _otherV3d(useZ=useZ, start2=start2) 375 e2 = _otherV3d(useZ=useZ, end2=end2) 376 377 a = e1.minus(s1) 378 b = e2.minus(s2) 379 c = s2.minus(s1) 380 381 ab = a.cross(b) 382 d = abs(c.dot(ab)) 383 e = max(EPS0, eps or _0_0) 384 if d > EPS0 and ab.length > e: 385 d = d / ab.length 386 if d > e: # argonic, skew lines distance 387 raise IntersectionError(skew_d=d, txt=_no_(_intersection_)) 388 389 # co-planar, non-skew lines 390 ab2 = ab.length2 391 if ab2 < e: # colinear, parallel or null line(s) 392 x = a.length2 > b.length2 393 if x: # make a shortest 394 a, b = b, a 395 s1, s2 = s2, s1 396 e1, e2 = e2, e1 397 if b.length2 < e: # both null 398 if c.length < e: 399 return s1, 0, 0 400 elif e2.minus(e1).length < e: 401 return e1, 0, 0 402 elif a.length2 < e: # null (s1, e1), non-null (s2, e2) 403 # like _nearestOn(s1, s2, e2, within=False, eps=e) 404 t = s1.minus(s2).dot(b) 405 v = s2.plus(b.times(t / b.length2)) 406 if s1.minus(v).length < e: 407 o = _outside(t, b.length2) 408 return (v, o, 0) if x else (v, 0, (o * 2)) 409 raise IntersectionError(length2=ab2, txt=_no_(_intersection_)) 410 411 cb = c.cross(b) 412 t = cb.dot(ab) 413 o1 = _outside(t, ab2) 414 v = s1.plus(a.times(t / ab2)) 415 o2 = _outside(v.minus(s2).dot(b), b.length2) * 2 416 return v, o1, o2 417 418 419def intersection3d3(start1, end1, start2, end2, eps=EPS, useZ=True, 420 **Vector_and_kwds): 421 '''Compute the intersection point of two lines, each defined by or 422 through a start and end point (3-D). 423 424 @arg start1: Start point of the first line (C{Cartesian}, L{Vector3d}, 425 C{Vector3Tuple} or C{Vector4Tuple}). 426 @arg end1: End point of the first line (C{Cartesian}, L{Vector3d}, 427 C{Vector3Tuple} or C{Vector4Tuple}). 428 @arg start2: Start point of the second line (C{Cartesian}, L{Vector3d}, 429 C{Vector3Tuple} or C{Vector4Tuple}). 430 @arg end2: End point of the second line (C{Cartesian}, L{Vector3d}, 431 C{Vector3Tuple} or C{Vector4Tuple}). 432 @kwarg eps: Tolerance for skew line distance and length (C{EPS}). 433 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=0} (C{bool}). 434 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the 435 intersection points and optional, additional B{C{Vector}} 436 keyword arguments, otherwise B{C{start1}}'s (sub-)class. 437 438 @return: An L{Intersection3Tuple}C{(point, outside1, outside2)} with 439 C{point} an instance of B{C{Vector}} or B{C{start1}}'s (sub-)class. 440 441 @raise IntersectionError: Invalid, skew, non-co-planar or otherwise 442 non-intersecting lines. 443 444 @see: U{Line-line intersection<https://MathWorld.Wolfram.com/Line-LineIntersection.html>} 445 and U{line-line distance<https://MathWorld.Wolfram.com/Line-LineDistance.html>}, 446 U{skew lines<https://MathWorld.Wolfram.com/SkewLines.html>} and U{point-line 447 distance<https://MathWorld.Wolfram.com/Point-LineDistance3-Dimensional.html>}. 448 ''' 449 try: 450 v, o1, o2 = _intersect3d3(start1, end1, start2, end2, eps=eps, useZ=useZ) 451 except (TypeError, ValueError) as x: 452 raise _xError(x, start1=start1, end1=end1, start2=start2, end2=end2) 453 v = _nVc(v, **_xkwds(Vector_and_kwds, clas=start1.classof, 454 name=intersection3d3.__name__)) 455 return Intersection3Tuple(v, o1, o2) 456 457 458def intersections2(center1, radius1, center2, radius2, sphere=True, **Vector_and_kwds): 459 '''Compute the intersection of two spheres or circles, each defined by a 460 (3-D) center point and a radius. 461 462 @arg center1: Center of the first sphere or circle (C{Cartesian}, L{Vector3d}, 463 C{Vector3Tuple} or C{Vector4Tuple}). 464 @arg radius1: Radius of the first sphere or circle (same units as the 465 B{C{center1}} coordinates). 466 @arg center2: Center of the second sphere or circle (C{Cartesian}, L{Vector3d}, 467 C{Vector3Tuple} or C{Vector4Tuple}). 468 @arg radius2: Radius of the second sphere or circle (same units as the 469 B{C{center1}} and B{C{center2}} coordinates). 470 @kwarg sphere: If C{True} compute the center and radius of the intersection of 471 two spheres. If C{False}, ignore the C{z}-component and compute 472 the intersection of two circles (C{bool}). 473 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the 474 intersection points and optional, additional B{C{Vector}} 475 keyword arguments, otherwise B{C{center1}}'s (sub-)class. 476 477 @return: If B{C{sphere}} is C{True}, a 2-tuple of the C{center} and C{radius} 478 of the intersection of the I{spheres}. The C{radius} is C{0.0} for 479 abutting spheres (and the C{center} is aka I{radical center}). 480 481 If B{C{sphere}} is C{False}, a 2-tuple with the two intersection 482 points of the I{circles}. For abutting circles, both points are 483 the same instance, aka I{radical center}. 484 485 @raise IntersectionError: Concentric, invalid or non-intersecting spheres 486 or circles. 487 488 @raise TypeError: Invalid B{C{center1}} or B{C{center2}}. 489 490 @raise UnitError: Invalid B{C{radius1}} or B{C{radius2}}. 491 492 @see: U{Sphere-Sphere<https://MathWorld.Wolfram.com/Sphere-SphereIntersection.html>} and 493 U{Circle-Circle<https://MathWorld.Wolfram.com/Circle-CircleIntersection.html>} 494 Intersection. 495 ''' 496 try: 497 return _intersects2(center1, Radius_(radius1=radius1), 498 center2, Radius_(radius2=radius2), sphere=sphere, 499 clas=center1.classof, **Vector_and_kwds) 500 except (TypeError, ValueError) as x: 501 raise _xError(x, center1=center1, radius1=radius1, center2=center2, radius2=radius2) 502 503 504def _intersects2(center1, r1, center2, r2, sphere=True, too_d=None, # in CartesianEllipsoidalBase.intersections2, 505 **clas_Vector_and_kwds): # .ellipsoidalBaseDI._intersections2 506 # (INTERNAL) Intersect two spheres or circles, see L{intersections2} 507 # above, separated to allow callers to embellish any exceptions 508 509 def _nV3(x, y, z): 510 v = Vector3d(x, y, z) 511 n = intersections2.__name__ 512 return _nVc(v, **_xkwds(clas_Vector_and_kwds, name=n)) 513 514 def _xV3(c1, u, x, y): 515 xy1 = x, y, _1_0 # transform to original space 516 return _nV3(fdot(xy1, u.x, -u.y, c1.x), 517 fdot(xy1, u.y, u.x, c1.y), _0_0) 518 519 c1 = _otherV3d(useZ=sphere, center1=center1) 520 c2 = _otherV3d(useZ=sphere, center2=center2) 521 522 if r1 < r2: # r1, r2 == R, r 523 c1, c2 = c2, c1 524 r1, r2 = r2, r1 525 526 m = c2.minus(c1) 527 d = m.length 528 if d < max(r2 - r1, EPS): 529 raise IntersectionError(_near_(_concentric_)) 530 531 o = fsum1_(-d, r1, r2) # overlap == -(d - (r1 + r2)) 532 # compute intersections with c1 at (0, 0) and c2 at (d, 0), like 533 # <https://MathWorld.Wolfram.com/Circle-CircleIntersection.html> 534 if o > EPS: # overlapping, r1, r2 == R, r 535 x = _radical2(d, r1, r2).xline 536 y = _1_0 - (x / r1)**2 537 if y > EPS: 538 y = r1 * sqrt(y) # y == a / 2 539 elif y < 0: 540 raise IntersectionError(_negative_) 541 else: # abutting 542 y = _0_0 543 elif o < 0: 544 t = d if too_d is None else too_d 545 raise IntersectionError(_too_(Fmt.distant(t))) 546 else: # abutting 547 x, y = r1, _0_0 548 549 u = m.unit() 550 if sphere: # sphere center and radius 551 c = c1 if x < EPS else ( 552 c2 if x > EPS1 else c1.plus(u.times(x))) 553 t = _nV3(c.x, c.y, c.z), Radius(y) 554 555 elif y > 0: # intersecting circles 556 t = _xV3(c1, u, x, y), _xV3(c1, u, x, -y) 557 else: # abutting circles 558 t = _xV3(c1, u, x, 0) 559 t = t, t 560 return t 561 562 563def iscolinearWith(point, point1, point2, eps=EPS, useZ=True): 564 '''Check whether a point is colinear with two other (2- or 3-D) points. 565 566 @arg point: The point (L{Vector3d}, C{Vector3Tuple} or 567 C{Vector4Tuple}). 568 @arg point1: First point (L{Vector3d}, C{Vector3Tuple} 569 or C{Vector4Tuple}). 570 @arg point2: Second point (L{Vector3d}, C{Vector3Tuple} 571 or C{Vector4Tuple}). 572 @kwarg eps: Tolerance (C{scalar}), same units as C{x}, 573 C{y} and C{z}. 574 @kwarg useZ: If C{True}, use the Z components, otherwise 575 force C{z=0} (C{bool}). 576 577 @return: C{True} if B{C{point}} is colinear B{C{point1}} 578 and B{C{point2}}, C{False} otherwise. 579 580 @raise TypeError: Invalid B{C{point}}, B{C{point1}} or 581 B{C{point2}}. 582 583 @see: Function L{vector2d.nearestOn}. 584 ''' 585 p = _otherV3d(useZ=useZ, point=point) 586 587 from pygeodesy.vector2d import _iscolinearWith 588 return _iscolinearWith(p, point1, point2, eps=eps, useZ=useZ) 589 590 591def nearestOn(point, point1, point2, within=True, useZ=True, Vector=None, **Vector_kwds): 592 '''Locate the point between two points closest to a reference (2- or 3-D). 593 594 @arg point: Reference point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} 595 or C{Vector4Tuple}). 596 @arg point1: Start point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or 597 C{Vector4Tuple}). 598 @arg point2: End point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple} or 599 C{Vector4Tuple}). 600 @kwarg within: If C{True} return the closest point between both given 601 points, otherwise the closest point on the extended line 602 through both points (C{bool}). 603 @kwarg useZ: If C{True}, use the Z components, otherwise force C{z=0} (C{bool}). 604 @kwarg Vector: Class to return closest point (C{Cartesian}, L{Vector3d} 605 or C{Vector3Tuple}) or C{None}. 606 @kwarg Vector_kwds: Optional, additional B{C{Vector}} keyword arguments, 607 ignored if C{B{Vector} is None}. 608 609 @return: Closest point, either B{C{point1}} or B{C{point2}} or an instance 610 of the B{C{point}}'s (sub-)class or B{C{Vector}} if not C{None}. 611 612 @raise TypeError: Invalid B{C{point}}, B{C{point1}} or B{C{point2}}. 613 614 @see: U{3-D Point-Line Distance<https://MathWorld.Wolfram.com/Point-LineDistance3-Dimensional.html>}, 615 C{Cartesian} and C{LatLon} methods C{nearestOn}, method L{sphericalTrigonometry.LatLon.nearestOn3} 616 and function L{sphericalTrigonometry.nearestOn3}. 617 ''' 618 p0 = _otherV3d(useZ=useZ, point =point) 619 p1 = _otherV3d(useZ=useZ, point1=point1) 620 p2 = _otherV3d(useZ=useZ, point2=point2) 621 622 from pygeodesy.vector2d import _nearestOn 623 p = _nearestOn(p0, p1, p2, within=within) 624 if Vector is not None: 625 p = Vector(p.x, p.y, **_xkwds(Vector_kwds, z=p.z, name=nearestOn.__name__)) 626 elif p is p1: 627 p = point1 628 elif p is p2: 629 p = point2 630 else: # ignore Vector_kwds 631 p = point.classof(p.x, p.y, Vector_kwds.get(_z_, p.z), name=nearestOn.__name__) 632 return p 633 634 635def _nVc(v, clas=None, name=NN, Vector=None, **Vector_kwds): # in .vector2d 636 # return a named C{Vector} or C{clas} instance 637 if Vector is not None: 638 v = Vector(v.x, v.y, v.z, **Vector_kwds) 639 elif clas is not None: 640 v = clas(v.x, v.y, v.z) # ignore Vector_kwds 641 return _xnamed(v, name) if name else v 642 643 644def _otherV3d(useZ=True, NN_OK=True, i=None, **name_v): # in .CartesianEllipsoidalBase.intersections2, 645 # check named vector instance, return Vector3d .Ellipsoid.height4, .formy.hartzell, .vector2d 646 def _name_i(name, i): 647 return name if i is None else Fmt.SQUARE(name, i) 648 649 name, v = _xkwds_popitem(name_v) 650 if useZ and isinstance(v, Vector3dBase): 651 return v if NN_OK or v.name else v.copy(name=_name_i(name, i)) 652 try: 653 return Vector3d(v.x, v.y, (v.z if useZ else _0_0), name=_name_i(name, i)) 654 except AttributeError: # no _x_ or _y_ attr 655 pass 656 raise _xotherError(Vector3d(0, 0, 0), v, name=_name_i(name, i), up=2) 657 658 659def parse3d(str3d, sep=_COMMA_, Vector=Vector3d, **Vector_kwds): 660 '''Parse an C{"x, y, z"} string. 661 662 @arg str3d: X, y and z values (C{str}). 663 @kwarg sep: Optional separator (C{str}). 664 @kwarg Vector: Optional class (L{Vector3d}). 665 @kwarg Vector_kwds: Optional B{C{Vector}} keyword arguments, 666 ignored if C{B{Vector} is None}. 667 668 @return: New B{C{Vector}} or if B{C{Vector}} is C{None}, 669 a named L{Vector3Tuple}C{(x, y, z)}. 670 671 @raise VectorError: Invalid B{C{str3d}}. 672 ''' 673 try: 674 v = [float(v.strip()) for v in str3d.split(sep)] 675 n = len(v) 676 if n != 3: 677 raise _ValueError(len=n) 678 except (TypeError, ValueError) as x: 679 raise VectorError(str3d=str3d, txt=str(x)) 680 return _xnamed((Vector3Tuple(*v) if Vector is None else 681 Vector(*v, **Vector_kwds)), parse3d.__name__) 682 683 684def sumOf(vectors, Vector=Vector3d, **Vector_kwds): 685 '''Compute the vectorial sum of several vectors. 686 687 @arg vectors: Vectors to be added (L{Vector3d}[]). 688 @kwarg Vector: Optional class for the vectorial sum (L{Vector3d}). 689 @kwarg Vector_kwds: Optional B{C{Vector}} keyword arguments, 690 ignored if C{B{Vector} is None}. 691 692 @return: Vectorial sum as B{C{Vector}} or if B{C{Vector}} is 693 C{None}, a named L{Vector3Tuple}C{(x, y, z)}. 694 695 @raise VectorError: No B{C{vectors}}. 696 ''' 697 n, vectors = len2(vectors) 698 if n < 1: 699 raise VectorError(vectors=n, txt=MISSING) 700 701 v = Vector3Tuple(fsum(v.x for v in vectors), 702 fsum(v.y for v in vectors), 703 fsum(v.z for v in vectors)) 704 return _xnamed((v if Vector is None else 705 Vector(*v, **Vector_kwds)), sumOf.__name__) 706 707 708def trilaterate2d2(x1, y1, radius1, x2, y2, radius2, x3, y3, radius3, 709 eps=None, **Vector_and_kwds): 710 '''Trilaterate three circles, each given as a (2-D) center and a radius. 711 712 @arg x1: Center C{x} coordinate of the 1st circle (C{scalar}). 713 @arg y1: Center C{y} coordinate of the 1st circle (C{scalar}). 714 @arg radius1: Radius of the 1st circle (C{scalar}). 715 @arg x2: Center C{x} coordinate of the 2nd circle (C{scalar}). 716 @arg y2: Center C{y} coordinate of the 2nd circle (C{scalar}). 717 @arg radius2: Radius of the 2nd circle (C{scalar}). 718 @arg x3: Center C{x} coordinate of the 3rd circle (C{scalar}). 719 @arg y3: Center C{y} coordinate of the 3rd circle (C{scalar}). 720 @arg radius3: Radius of the 3rd circle (C{scalar}). 721 @kwarg eps: Check the trilaterated point I{delta} on all 3 722 circles (C{scalar}) or C{None}. 723 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the 724 trilateration and optional, additional B{C{Vector}} 725 keyword arguments, otherwise (L{Vector3d}). 726 727 @return: Trilaterated point as C{B{Vector}(x, y, **B{Vector_kwds})} 728 or L{Vector2Tuple}C{(x, y)} if C{B{Vector} is None}.. 729 730 @raise IntersectionError: No intersection, colinear or near-concentric 731 centers, trilateration failed some other way 732 or the trilaterated point is off one circle 733 by more than B{C{eps}}. 734 735 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}. 736 737 @see: U{Issue #49<https://GitHub.com/mrJean1/PyGeodesy/issues/49>}, 738 U{Find X location using 3 known (X,Y) location using trilateration 739 <https://math.StackExchange.com/questions/884807>} and L{trilaterate3d2}. 740 ''' 741 from pygeodesy.vector2d import _trilaterate2d2 742 return _trilaterate2d2(x1, y1, radius1, x2, y2, radius2, x3, y3, radius3, 743 eps=eps, **Vector_and_kwds) 744 745 746def trilaterate3d2(center1, radius1, center2, radius2, center3, radius3, 747 eps=EPS, **Vector_and_kwds): 748 '''Trilaterate three spheres, each given as a (3-D) center and a radius. 749 750 @arg center1: Center of the 1st sphere (C{Cartesian}, L{Vector3d}, 751 C{Vector3Tuple} or C{Vector4Tuple}). 752 @arg radius1: Radius of the 1st sphere (same C{units} as C{x}, C{y} 753 and C{z}). 754 @arg center2: Center of the 2nd sphere (C{Cartesian}, L{Vector3d}, 755 C{Vector3Tuple} or C{Vector4Tuple}). 756 @arg radius2: Radius of this sphere (same C{units} as C{x}, C{y} 757 and C{z}). 758 @arg center3: Center of the 3rd sphere (C{Cartesian}, L{Vector3d}, 759 C{Vector3Tuple} or C{Vector4Tuple}). 760 @arg radius3: Radius of the 3rd sphere (same C{units} as C{x}, C{y} 761 and C{z}). 762 @kwarg eps: Tolerance (C{scalar}), same units as C{x}, C{y}, and C{z}. 763 @kwarg Vector_and_kwds: Optional class C{B{Vector}=None} to return the 764 trilateration and optional, additional B{C{Vector}} 765 keyword arguments, otherwise B{C{center1}}'s 766 (sub-)class. 767 768 @return: 2-Tuple with two trilaterated points, each a B{C{Vector}} 769 instance. Both points are the same instance if all three 770 spheres abut/intersect in a single point. 771 772 @raise ImportError: Package C{numpy} not found, not installed or 773 older than version 1.10. 774 775 @raise IntersectionError: Near-concentric, colinear, too distant or 776 non-intersecting spheres. 777 778 @raise NumPyError: Some C{numpy} issue. 779 780 @raise TypeError: Invalid B{C{center1}}, B{C{center2}} or B{C{center3}}. 781 782 @raise UnitError: Invalid B{C{radius1}}, B{C{radius2}} or B{C{radius3}}. 783 784 @see: Norrdine, A. U{I{An Algebraic Solution to the Multilateration 785 Problem}<https://www.ResearchGate.net/publication/ 786 275027725_An_Algebraic_Solution_to_the_Multilateration_Problem>}, 787 the U{I{implementation}<https://www.ResearchGate.net/publication/ 788 288825016_Trilateration_Matlab_Code>} and L{trilaterate2d2}. 789 ''' 790 from pygeodesy.vector2d import _trilaterate3d2 791 try: 792 return _trilaterate3d2(_otherV3d(center1=center1, NN_OK=False), 793 Radius_(radius1=radius1, low=eps), 794 center2, radius2, center3, radius3, eps=eps, 795 clas=center1.classof, **Vector_and_kwds) 796 except (AssertionError, TypeError, ValueError) as x: 797 raise _xError(x, center1=center1, radius1=radius1, 798 center2=center2, radius2=radius2, 799 center3=center3, radius3=radius3) 800 801 802def _xyzn4(xyz, y, z, Error=_TypeError): # see: .ecef 803 '''(INTERNAL) Get an C{(x, y, z, name)} 4-tuple. 804 ''' 805 try: 806 t = xyz.x, xyz.y, xyz.z 807 n = getattr(xyz, _name_, NN) 808 except AttributeError: 809 t = xyz, y, z 810 n = NN 811 try: 812 x, y, z = map2(float, t) 813 except (TypeError, ValueError) as x: 814 d = dict(zip((_xyz_, _y_, _z_), t)) 815 raise Error(txt=str(x), **d) 816 817 return x, y, z, n 818 819 820def _xyzhdn6(xyz, y, z, height, datum, ll, Error=_TypeError): # by .cartesianBase, .nvectorBase 821 '''(INTERNAL) Get an C{(x, y, z, h, d, name)} 6-tuple. 822 ''' 823 x, y, z, n = _xyzn4(xyz, y, z, Error=Error) 824 825 h = height or getattr(xyz, _height_, None) \ 826 or getattr(xyz, _h_, None) \ 827 or getattr(ll, _height_, None) 828 829 d = datum or getattr(xyz, _datum_, None) \ 830 or getattr(ll, _datum_, None) 831 832 return x, y, z, h, d, n 833 834 835__all__ += _ALL_DOCS(intersections2, sumOf, Vector3dBase) 836 837# **) MIT License 838# 839# Copyright (C) 2016-2021 -- mrJean1 at Gmail -- All Rights Reserved. 840# 841# Permission is hereby granted, free of charge, to any person obtaining a 842# copy of this software and associated documentation files (the "Software"), 843# to deal in the Software without restriction, including without limitation 844# the rights to use, copy, modify, merge, publish, distribute, sublicense, 845# and/or sell copies of the Software, and to permit persons to whom the 846# Software is furnished to do so, subject to the following conditions: 847# 848# The above copyright notice and this permission notice shall be included 849# in all copies or substantial portions of the Software. 850# 851# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 852# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 853# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 854# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR 855# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 856# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 857# OTHER DEALINGS IN THE SOFTWARE. 858