Coverage for pygeodesy/resections.py: 99%
313 statements
« prev ^ index » next coverage.py v7.2.2, created at 2023-04-12 11:45 -0400
« prev ^ index » next coverage.py v7.2.2, created at 2023-04-12 11:45 -0400
2# -*- coding: utf-8 -*-
4u'''3-Point resection functions L{cassini}, L{collins5}, L{pierlot} and L{tienstra7},
5survey functions L{snellius3} and L{wildberger3} and triangle functions L{triAngle},
6L{triAngle4}, L{triSide}, L{triSide2} and L{triSide4}.
8@note: Function L{pierlot} is transcoded with permission from U{triangulationPierlot
9 <http://www.Telecom.ULg.ac.Be/triangulation/doc/total_8c.html>} and U{Pierlot
10 <http://www.Telecom.ULg.ac.Be/publi/publications/pierlot/Pierlot2014ANewThree>}.
11'''
12# make sure int/int division yields float quotient
13from __future__ import division as _; del _ # PYCHOK semicolon
15from pygeodesy.basics import map1, _ALL_LAZY
16from pygeodesy.constants import EPS, EPS0, EPS02, INT0, PI, PI2, PI_2, PI_4, isnear0, \
17 _0_0, _0_5, _1_0, _N_1_0, _2_0, _N_2_0, _4_0, _360_0
18from pygeodesy.errors import _and, _or, TriangleError, _ValueError, _xkwds
19from pygeodesy.fmath import favg, Fdot, fidw, fmean, hypot, hypot2_
20from pygeodesy.fsums import Fsum, fsum_, fsum1, fsum1_
21from pygeodesy.interns import _a_, _A_, _b_, _B_, _c_, _C_, _coincident_, _colinear_, \
22 _d_, _invalid_, _negative_, _not_, _rIn_, _SPACE_
23# from pygeodesy.lazily import _ALL_LAZY # from .basics
24from pygeodesy.named import Fmt, _NamedTuple, _Pass
25# from pygeodesy.streprs import Fmt # from .named
26from pygeodesy.units import Degrees, Distance, Radians
27from pygeodesy.utily import acos1, asin1, sincos2, sincos2_, sincos2d, sincos2d_
28from pygeodesy.vector3d import _otherV3d, Vector3d
30from math import cos, atan2, degrees, fabs, radians, sin, sqrt
32__all__ = _ALL_LAZY.resections
33__version__ = '23.04.09'
35_concyclic_ = 'concyclic'
36_PA_ = 'PA'
37_PB_ = 'PB'
38_PC_ = 'PC'
39_pointH_ = 'pointH'
40_pointP_ = 'pointP'
41_R3__ = 'R3 '
42_radA_ = 'radA'
43_radB_ = 'radB'
44_radC_ = 'radC'
47class Collins5Tuple(_NamedTuple):
48 '''5-Tuple C{(pointP, pointH, a, b, c)} with survey C{pointP}, auxiliary
49 C{pointH}, each an instance of B{C{pointA}}'s (sub-)class and triangle
50 sides C{a}, C{b} and C{c} in C{meter}, conventionally.
51 '''
52 _Names_ = (_pointP_, _pointH_, _a_, _b_, _c_)
53 _Units_ = (_Pass, _Pass, Distance, Distance, Distance)
56class ResectionError(_ValueError):
57 '''Error raised for issues in L{pygeodesy.resections}.
58 '''
59 pass
62class Survey3Tuple(_NamedTuple):
63 '''3-Tuple C{(PA, PB, PC)} with distance from survey point C{P} to each of
64 the triangle corners C{A}, C{B} and C{C} in C{meter}, conventionally.
65 '''
66 _Names_ = (_PA_, _PB_, _PC_)
67 _Units_ = ( Distance, Distance, Distance)
70class Tienstra7Tuple(_NamedTuple):
71 '''7-Tuple C{(pointP, A, B, C, a, b, c)} with survey C{pointP}, interior
72 triangle angles C{A}, C{B} and C{C} in C{degrees} and triangle sides
73 C{a}, C{b} and C{c} in C{meter}, conventionally.
74 '''
75 _Names_ = (_pointP_, _A_, _B_, _C_, _a_, _b_, _c_)
76 _Units_ = (_Pass, Degrees, Degrees, Degrees, Distance, Distance, Distance)
79class TriAngle4Tuple(_NamedTuple):
80 '''4-Tuple C{(radA, radB, radC, rIn)} with the interior angles at triangle
81 corners C{A}, C{B} and C{C} in C{radians} and the C{InCircle} radius
82 C{rIn} aka C{inradius} in C{meter}, conventionally.
83 '''
84 _Names_ = (_radA_, _radB_, _radC_, _rIn_)
85 _Units_ = ( Radians, Radians, Radians, Distance)
88class TriSide2Tuple(_NamedTuple):
89 '''2-Tuple C{(a, radA)} with triangle side C{a} in C{meter}, conventionally
90 and angle C{radA} at the opposite triangle corner in C{radians}.
91 '''
92 _Names_ = (_a_, _radA_)
93 _Units_ = ( Distance, Radians)
96class TriSide4Tuple(_NamedTuple):
97 '''4-Tuple C{(a, b, radC, d)} with interior angle C{radC} at triangle corner
98 C{C} in C{radians}with the length of triangle sides C{a} and C{b} and
99 with triangle height C{d} perpendicular to triangle side C{c}, in the
100 same units as triangle sides C{a} and C{b}.
101 '''
102 _Names_ = (_a_, _b_, _radC_, _d_)
103 _Units_ = ( Distance, Distance, Radians, Distance)
106def cassini(pointA, pointB, pointC, alpha, beta, useZ=False, Clas=None, **Clas_kwds):
107 '''3-Point resection using U{Cassini<https://NL.WikiPedia.org/wiki/Achterwaartse_insnijding>}'s method.
109 @arg pointA: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
110 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
111 @arg pointB: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
112 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
113 @arg pointC: Center point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
114 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
115 @arg alpha: Angle subtended by triangle side B{C{pointA}} to B{C{pointC}}
116 (C{degrees}, non-negative).
117 @arg beta: Angle subtended by triangle side B{C{pointB}} to B{C{pointC}}
118 (C{degrees}, non-negative).
119 @kwarg useZ: If C{True}, use and interpolate the Z component, otherwise
120 force C{z=INT0} (C{bool}).
121 @kwarg Clas: Optional class to return the survey point or C{None} for
122 B{C{pointA}}'s (sub-)class.
123 @kwarg Clas_kwds: Optional additional keyword argument for the survey
124 point instance.
126 @note: Typically, B{C{pointC}} is between B{C{pointA}} and B{C{pointB}}.
128 @return: The survey point, an instance of B{C{Clas}} or if C{B{Clas} is
129 None} of B{C{pointA}}'s (sub-)class.
131 @raise ResectionError: Near-coincident, -colinear or -concyclic points
132 or negative or invalid B{C{alpha}} or B{C{beta}}.
134 @raise TypeError: Invalid B{C{pointA}}, B{C{pointB}} or B{C{pointM}}.
136 @see: U{Three Point Resection Problem<https://Dokumen.tips/documents/
137 three-point-resection-problem-introduction-kaestner-burkhardt-method.html>}
138 and functions L{pygeodesy.collins5}, L{pygeodesy.pierlot} and
139 L{pygeodesy.tienstra7}.
140 '''
142 def _H(A, C, sa):
143 s, c = sincos2d(sa)
144 if isnear0(s):
145 raise ValueError(_or(_coincident_, _colinear_))
146 t = s, c, c
147 x = Fdot(t, A.x, C.y, -A.y).fover(s)
148 y = Fdot(t, A.y, -C.x, A.x).fover(s)
149 return Vector3d(x, y, 0)
151 A = _otherV3d(useZ=useZ, pointA=pointA)
152 B = _otherV3d(useZ=useZ, pointB=pointB)
153 C = _otherV3d(useZ=useZ, pointC=pointC)
155 try:
156 sa, sb = map1(float, alpha, beta)
157 if min(sa, sb) < 0:
158 raise ValueError(_negative_)
159 if fsum_(_360_0, -sa, -sb) < EPS0:
160 raise ValueError()
162 H1 = _H(A, C, sa)
163 H2 = _H(B, C, -sb)
165 x = H1.x - H2.x
166 y = H1.y - H2.y
167 # x, y, _ = H1.minus(H2).xyz
168 if isnear0(x) or isnear0(y):
169 raise ValueError(_SPACE_(_concyclic_, (x, y)))
171 m = y / x
172 n = x / y
173 N = n + m
174 if isnear0(N):
175 raise ValueError(_SPACE_(_concyclic_, (m, n, N)))
177 t = n, m, _1_0, _N_1_0
178 x = Fdot(t, C.x, H1.x, C.y, H1.y).fover(N)
179 y = Fdot(t, H1.y, C.y, C.x, H1.x).fover(N)
180 z = _zidw(A, B, C, x, y) if useZ else INT0
182 clas = Clas or pointA.classof
183 return clas(x, y, z, **_xkwds(Clas_kwds, name=cassini.__name__))
185 except (TypeError, ValueError) as x:
186 raise ResectionError(pointA=pointA, pointB=pointB, pointC=pointC,
187 alpha=alpha, beta=beta, cause=x)
190def collins5(pointA, pointB, pointC, alpha, beta, useZ=False, Clas=None, **Clas_kwds):
191 '''3-Point resection using U{Collins<https://Dokumen.tips/documents/
192 three-point-resection-problem-introduction-kaestner-burkhardt-method.html>}' method.
194 @arg pointA: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
195 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
196 @arg pointB: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
197 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
198 @arg pointC: Center point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
199 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
200 @arg alpha: Angle subtended by triangle side C{b} from B{C{pointA}} to
201 B{C{pointC}} (C{degrees}, non-negative).
202 @arg beta: Angle subtended by triangle side C{a} from B{C{pointB}} to
203 B{C{pointC}} (C{degrees}, non-negative).
204 @kwarg useZ: If C{True}, use and interpolate the Z component, otherwise
205 force C{z=INT0} (C{bool}).
206 @kwarg Clas: Optional class to return the survey and auxiliary point
207 or C{None} for B{C{pointA}}'s (sub-)class.
208 @kwarg Clas_kwds: Optional additional keyword argument for the survey
209 and auxiliary point instance.
211 @note: Typically, B{C{pointC}} is between B{C{pointA}} and B{C{pointB}}.
213 @return: L{Collins5Tuple}C{(pointP, pointH, a, b, c)} with survey C{pointP},
214 auxiliary C{pointH}, each an instance of B{C{Clas}} or if C{B{Clas}
215 is None} of B{C{pointA}}'s (sub-)class and triangle sides C{a},
216 C{b} and C{c} in C{meter}, conventionally.
218 @raise ResectionError: Near-coincident, -colinear or -concyclic points
219 or negative or invalid B{C{alpha}} or B{C{beta}}.
221 @raise TypeError: Invalid B{C{pointA}}, B{C{pointB}} or B{C{pointM}}.
223 @see: U{Collins' methode<https://NL.WikiPedia.org/wiki/Achterwaartse_insnijding>}
224 and functions L{pygeodesy.cassini}, L{pygeodesy.pierlot} and
225 L{pygeodesy.tienstra7}.
226 '''
228 def _azi_len2(A, B, pi2):
229 v = B.minus(A)
230 r = atan2(v.x, v.y)
231 if pi2 and r < 0:
232 r += pi2
233 return r, v.length
235 def _cV3(d, r, A, B, C, useZ, V3, **kwds):
236 s, c = sincos2(r)
237 x = A.x + d * s
238 y = A.y + d * c
239 z = _zidw(A, B, C, x, y) if useZ else INT0
240 return V3(x, y, z, **kwds)
242 A = _otherV3d(useZ=useZ, pointA=pointA)
243 B = _otherV3d(useZ=useZ, pointB=pointB)
244 C = _otherV3d(useZ=useZ, pointC=pointC)
246 try:
247 ra, rb = radians(alpha), radians(beta)
248 if min(ra, rb) < 0:
249 raise ValueError(_negative_)
251 sra, srH = sin(ra), sin(ra + rb - PI) # rH = PI - ((PI - ra) + (PI - rb))
252 if isnear0(sra) or isnear0(srH):
253 raise ValueError(_or(_coincident_, _colinear_, _concyclic_))
255 clas = Clas or pointA.classof
256 kwds = _xkwds(Clas_kwds, name=collins5.__name__)
258# za, a = _azi_len2(C, B, PI2)
259 zb, b = _azi_len2(C, A, PI2)
260 zc, c = _azi_len2(A, B, 0)
262# d = c * sin(PI - rb) / srH # B.minus(H).length
263 d = c * sin(PI - ra) / srH # A.minus(H).length
264 r = zc + PI - rb # zh = zc + (PI - rb)
265 H = _cV3(d, r, A, B, C, useZ, Vector3d)
267 zh, _ = _azi_len2(C, H, PI2)
269# d = a * sin(za - zh) / sin(rb) # B.minus(P).length
270 d = b * sin(zb - zh) / sra # A.minus(P).length
271 r = zh - ra # zb - PI + (PI - ra - (zb - zh))
272 P = _cV3(d, r, A, B, C, useZ, clas, **kwds)
274 H = clas(H.x, H.y, H.z, **kwds)
275 a = B.minus(C).length
277 return Collins5Tuple(P, H, a, b, c, name=collins5.__name__)
279 except (TypeError, ValueError) as x:
280 raise ResectionError(pointA=pointA, pointB=pointB, pointC=pointC,
281 alpha=alpha, beta=beta, cause=x)
284def pierlot(point1, point2, point3, alpha12, alpha23, useZ=False, Clas=None, **Clas_kwds):
285 '''3-Point resection using U{Pierlot<http://www.Telecom.ULg.ac.Be/publi/publications/
286 pierlot/Pierlot2014ANewThree>}'s method C{ToTal}.
288 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
289 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
290 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
291 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
292 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple},
293 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}).
294 @arg alpha12: Angle subtended from B{C{point1}} to B{C{point2}} (C{degrees}).
295 @arg alpha23: Angle subtended from B{C{point2}} to B{C{point3}} (C{degrees}).
296 @kwarg useZ: If C{True}, interpolate the Z component, otherwise use C{z=INT0}
297 (C{bool}).
298 @kwarg Clas: Optional class to return the survey point or C{None} for
299 B{C{point1}}'s (sub-)class.
300 @kwarg Clas_kwds: Optional additional keyword arguments for the survey
301 point instance.
303 @note: Points B{C{point1}}, B{C{point2}} and B{C{point3}} are ordered
304 counter-clockwise, typically.
306 @return: The survey (or robot) point, an instance of B{C{Clas}} or if
307 C{B{Clas} is None} of B{C{point1}}'s (sub-)class.
309 @raise ResectionError: Near-coincident, -colinear or -concyclic points
310 or invalid B{C{alpha12}} or B{C{alpha23}}.
312 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}.
314 @see: U{V. Pierlot, M. Van Droogenbroeck, "A New Three Object Triangulation
315 Algorithm for Mobile Robot Positioning"<https://ORBi.ULiege.Be/
316 bitstream/2268/157469/1/Pierlot2014ANewThree.pdf>}, U{Vincent Pierlot,
317 Marc Van Droogenbroeck, "18 Triangulation Algorithms for 2D Positioning
318 (also known as the Resection Problem)"<http://www.Telecom.ULg.ac.Be/
319 triangulation>} and functions L{pygeodesy.cassini}, L{pygeodesy.collins5}
320 and L{pygeodesy.tienstra7}.
321 '''
322 B1 = _otherV3d(useZ=useZ, point1=point1)
323 B2 = _otherV3d(useZ=useZ, point2=point2)
324 B3 = _otherV3d(useZ=useZ, point3=point3)
326 try: # (INTERNAL) Raises error for (pseudo-)singularities
327 s12, c12, s23, c23 = sincos2d_(alpha12, alpha23)
328 if isnear0(s12) or isnear0(s23):
329 raise ValueError(_or(_coincident_, _colinear_))
330 cot12 = c12 / s12
331 cot23 = c23 / s23
332# cot31 = (1 - cot12 * cot23) / (cot12 + cot32)
333 d = fsum1_(c12 * s23, s12 * c23)
334 if isnear0(d):
335 raise ValueError(_or(_colinear_, _coincident_))
336 cot31 = Fsum(_1_0, s12 * s23, -c12 * c23, _N_1_0).fover(d)
338 x1_, y1_, _ = B1.minus(B2).xyz
339 x3_, y3_, _ = B3.minus(B2).xyz
341# x23 = x3_ - cot23 * y3_
342# y23 = y3_ + cot23 * x3_
344 X12_23 = Fsum(x1_, cot12 * y1_, -x3_, cot23 * y3_)
345 Y12_23 = Fsum(y1_, -cot12 * x1_, -y3_, -cot23 * x3_)
347 X31_23 = Fsum(x1_, -cot31 * y1_, cot31 * y3_, cot23 * y3_)
348 Y31_23 = Fsum(y1_, cot31 * x1_, -cot31 * x3_, -cot23 * x3_)
350 d = float(X31_23 * Y12_23 - X12_23 * Y31_23)
351 if isnear0(d):
352 raise ValueError(_or(_coincident_, _colinear_, _concyclic_))
353 K = Fsum(x3_ * x1_, cot31 * (y3_ * x1_),
354 y3_ * y1_, -cot31 * (x3_ * y1_))
356 x = (B2.x * d + K * Y12_23).fover(d)
357 y = (B2.y * d - K * X12_23).fover(d)
358 z = _zidw(B1, B2, B3, x, y) if useZ else INT0
360 clas = Clas or point1.classof
361 return clas(x, y, z, **_xkwds(Clas_kwds, name=pierlot.__name__))
363 except (TypeError, ValueError) as x:
364 raise ResectionError(point1=point1, point2=point2, point3=point3,
365 alpha12=alpha12, alpha23=alpha23, cause=x)
368def snellius3(a, b, degC, alpha, beta):
369 '''Snellius' surveying using U{Snellius Pothenot<https://WikiPedia.org/wiki/Snellius–Pothenot_problem>}.
371 @arg a: Length of the triangle side between corners C{B} and C{C} and opposite of
372 triangle corner C{A} (C{scalar}, non-negative C{meter}, conventionally).
373 @arg b: Length of the triangle side between corners C{C} and C{A} and opposite of
374 triangle corner C{B} (C{scalar}, non-negative C{meter}, conventionally).
375 @arg degC: Angle at triangle corner C{C}, opposite triangle side C{c} (non-negative C{degrees}).
376 @arg alpha: Angle subtended by triangle side B{C{b}} (non-negative C{degrees}).
377 @arg beta: Angle subtended by triangle side B{C{a}} (non-negative C{degrees}).
379 @return: L{Survey3Tuple}C{(PA, PB, PC)} with distance from survey point C{P} to
380 each of the triangle corners C{A}, C{B} and C{C}, same units as triangle
381 sides B{C{a}}, B{C{b}} and B{C{c}}.
383 @raise TriangleError: Invalid B{C{a}}, B{C{b}} or B{C{degC}} or negative B{C{alpha}}
384 or B{C{beta}}.
386 @see: Function L{pygeodesy.wildberger3}.
387 '''
388 try:
389 a, b, degC, alpha, beta = map1(float, a, b, degC, alpha, beta)
390 ra, rb, rC = map1(radians, alpha, beta, degC)
391 if min(ra, rb, rC, a, b) < 0:
392 raise ValueError(_negative_)
394 r = fsum_(ra, rb, rC) * _0_5
395 k = PI - r
396 if min(k, r) < 0:
397 raise ValueError(_or(_coincident_, _colinear_))
399 sa, sb = sin(ra), sin(rb)
400 p = atan2(a * sa, b * sb)
401 sp, cp, sr, cr = sincos2_(PI_4 - p, r)
402 w = atan2(sp * sr, cp * cr)
403 x = k + w
404 y = k - w
406 s = fabs(sa)
407 if fabs(sb) > s:
408 pc = fabs(a * sin(y) / sb)
409 elif s:
410 pc = fabs(b * sin(x) / sa)
411 else:
412 raise ValueError(_or(_colinear_, _coincident_))
414 pa = _triSide(b, pc, fsum_(PI, -ra, -x))
415 pb = _triSide(a, pc, fsum_(PI, -rb, -y))
416 return Survey3Tuple(pa, pb, pc, name=snellius3.__name__)
418 except (TypeError, ValueError) as x:
419 raise TriangleError(a=a, b=b, degC=degC, alpha=alpha, beta=beta, cause=x)
422def tienstra7(pointA, pointB, pointC, alpha, beta=None, gamma=None,
423 useZ=False, Clas=None, **Clas_kwds):
424 '''3-Point resection using U{Tienstra<https://WikiPedia.org/wiki/Tienstra_formula>}'s formula.
426 @arg pointA: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, C{Vector4Tuple} or
427 C{Vector2Tuple} if C{B{useZ}=False}).
428 @arg pointB: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, C{Vector4Tuple} or
429 C{Vector2Tuple} if C{B{useZ}=False}).
430 @arg pointC: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, C{Vector4Tuple} or
431 C{Vector2Tuple} if C{B{useZ}=False}).
432 @arg alpha: Angle subtended by triangle side C{a} from B{C{pointB}} to B{C{pointC}}
433 (C{degrees}, non-negative).
434 @kwarg beta: Angle subtended by triangle side C{b} from B{C{pointA}} to B{C{pointC}}
435 (C{degrees}, non-negative) or C{None} if C{B{gamma} is not None}.
436 @kwarg gamma: Angle subtended by triangle side C{c} from B{C{pointA}} to B{C{pointB}}
437 (C{degrees}, non-negative) or C{None} if C{B{beta} is not None}.
438 @kwarg useZ: If C{True}, use and interpolate the Z component, otherwise force C{z=INT0}
439 (C{bool}).
440 @kwarg Clas: Optional class to return the survey point or C{None} for B{C{pointA}}'s
441 (sub-)class.
442 @kwarg Clas_kwds: Optional additional keyword arguments for the survey point instance.
444 @note: Points B{C{pointA}}, B{C{pointB}} and B{C{pointC}} are ordered clockwise.
446 @return: L{Tienstra7Tuple}C{(pointP, A, B, C, a, b, c)} with survey C{pointP}, an
447 instance of B{C{Clas}} or if C{B{Clas} is None} of B{C{pointA}}'s (sub-)class,
448 with triangle angles C{A} at B{C{pointA}}, C{B} at B{C{pointB}} and C{C} at
449 B{C{pointC}} in C{degrees} and with triangle sides C{a}, C{b} and C{c} in
450 C{meter}, conventionally.
452 @raise ResectionError: Near-coincident, -colinear or -concyclic points or sum of
453 B{C{alpha}}, B{C{beta}} and B{C{gamma}} not C{360} or
454 negative B{C{alpha}}, B{C{beta}} or B{C{gamma}}.
456 @raise TypeError: Invalid B{C{pointA}}, B{C{pointB}} or B{C{pointC}}.
458 @see: U{3-Point Resection Solver<http://MesaMike.org/geocache/GC1B0Q9/tienstra/>},
459 U{V. Pierlot, M. Van Droogenbroeck, "A New Three Object Triangulation..."
460 <http://www.Telecom.ULg.ac.Be/publi/publications/pierlot/Pierlot2014ANewThree/>},
461 U{18 Triangulation Algorithms...<http://www.Telecom.ULg.ac.Be/triangulation/>} and
462 functions L{pygeodesy.cassini}, L{pygeodesy.collins5} and L{pygeodesy.pierlot}.
463 '''
465 def _deg_ks(r, s, ks, N):
466 if isnear0(fsum_(PI, r, -s)): # r + (PI2 - s) == PI
467 raise ValueError(Fmt.PARENSPACED(concyclic=N))
468 # k = 1 / (cot(r) - cot(s))
469 # = 1 / (cos(r) / sin(r) - cos(s) / sin(s))
470 # = 1 / (cos(r) * sin(s) - cos(s) * sin(r)) / (sin(r) * sin(s))
471 # = sin(r) * sin(s) / (cos(r) * sin(s) - cos(s) * sin(r))
472 sr, cr, ss, cs = sincos2_(r, s)
473 c = cr * ss - cs * sr
474 if isnear0(c):
475 raise ValueError(Fmt.PARENSPACED(cotan=N))
476 ks.append(sr * ss / c)
477 return Degrees(degrees(r), name=N) # C degrees
479 A = _otherV3d(useZ=useZ, pointA=pointA)
480 B = _otherV3d(useZ=useZ, pointB=pointB)
481 C = _otherV3d(useZ=useZ, pointC=pointC)
483 try:
484 sa, sb, sc = map1(radians, alpha, (beta or 0), (gamma or 0))
485 if beta is None:
486 if gamma is None:
487 raise ValueError(_and(Fmt.EQUAL(beta=beta), Fmt.EQUAL(gamma=gamma)))
488 sb = fsum_(PI2, -sa, -sc)
489 elif gamma is None:
490 sc = fsum_(PI2, -sa, -sb)
491 else: # subtended angles must add to 360 degrees
492 r = fsum1_(sa, sb, sc)
493 if fabs(r - PI2) > EPS:
494 raise ValueError(Fmt.EQUAL(sum=degrees(r)))
495 if min(sa, sb, sc) < 0:
496 raise ValueError(_negative_)
498 # triangle sides
499 a = B.minus(C).length
500 b = A.minus(C).length
501 c = A.minus(B).length
503 ks = [] # 3 Ks and triangle angles
504 dA = _deg_ks(_triAngle(b, c, a), sa, ks, _A_)
505 dB = _deg_ks(_triAngle(a, c, b), sb, ks, _B_)
506 dC = _deg_ks(_triAngle(a, b, c), sc, ks, _C_)
508 k = fsum1(ks, floats=True)
509 if isnear0(k):
510 raise ValueError(Fmt.EQUAL(K=k))
511 x = Fdot(ks, A.x, B.x, C.x).fover(k)
512 y = Fdot(ks, A.y, B.y, C.y).fover(k)
513 z = _zidw(A, B, C, x, y) if useZ else INT0
515 n = tienstra7.__name__
516 clas = Clas or pointA.classof
517 P = clas(x, y, z, **_xkwds(Clas_kwds, name=n))
518 return Tienstra7Tuple(P, dA, dB, dC, a, b, c, name=n)
520 except (TypeError, ValueError) as x:
521 raise ResectionError(pointA=pointA, pointB=pointB, pointC=pointC,
522 alpha=alpha, beta=beta, gamma=gamma, cause=x)
525def triAngle(a, b, c):
526 '''Compute one angle of a triangle.
528 @arg a: Adjacent triangle side length (C{scalar}, non-negative
529 C{meter}, conventionally).
530 @arg b: Adjacent triangle side length (C{scalar}, non-negative
531 C{meter}, conventionally).
532 @arg c: Opposite triangle side length (C{scalar}, non-negative
533 C{meter}, conventionally).
535 @return: Angle in C{radians} at triangle corner C{C}, opposite
536 triangle side B{C{c}}.
538 @raise TriangleError: Invalid or negative B{C{a}}, B{C{b}} or B{C{c}}.
540 @see: Functions L{pygeodesy.triAngle4} and L{pygeodesy.triSide}.
541 '''
542 try:
543 return _triAngle(a, b, c)
544 except (TypeError, ValueError) as x:
545 raise TriangleError(a=a, b=b, c=c, cause=x)
548def _triAngle(a, b, c):
549 # (INTERNAL) To allow callers to embellish errors
550 a, b, c = map1(float, a, b, c)
551 if a < b:
552 a, b = b, a
553 if b < 0 or c < 0:
554 raise ValueError(_negative_)
555 if a < EPS0:
556 raise ValueError(_coincident_)
557 b_a = b / a
558 if b_a < EPS0:
559 raise ValueError(_coincident_)
560 return acos1(fsum_(_1_0, b_a**2, -(c / a)**2) / (b_a * _2_0))
563def triAngle4(a, b, c):
564 '''Compute the angles of a triangle.
566 @arg a: Length of the triangle side opposite of triangle corner C{A}
567 (C{scalar}, non-negative C{meter}, conventionally).
568 @arg b: Length of the triangle side opposite of triangle corner C{B}
569 (C{scalar}, non-negative C{meter}, conventionally).
570 @arg c: Length of the triangle side opposite of triangle corner C{C}
571 (C{scalar}, non-negative C{meter}, conventionally).
573 @return: L{TriAngle4Tuple}C{(radA, radB, radC, rIn)} with angles C{radA},
574 C{radB} and C{radC} at triangle corners C{A}, C{B} and C{C}, all
575 in C{radians} and the C{InCircle} radius C{rIn} aka C{inradius},
576 same units as triangle sides B{C{a}}, B{C{b}} and B{C{c}}.
578 @raise TriangleError: Invalid or negative B{C{a}}, B{C{b}} or B{C{c}}.
580 @see: Function L{pygeodesy.triAngle}.
581 '''
582 try:
583 a, b, c = map1(float, a, b, c)
584 ab = a < b
585 if ab:
586 a, b = b, a
587 bc = b < c
588 if bc:
589 b, c = c, b
591 if c > EPS0: # c = min(a, b, c)
592 s = float(Fsum(a, b, c) * _0_5)
593 if s < EPS0:
594 raise ValueError(_negative_)
595 sa, sb, sc = (s - a), (s - b), (s - c)
596 r = sa * sb * sc / s
597 if r < EPS02:
598 raise ValueError(_coincident_)
599 r = sqrt(r)
600 rA = atan2(r, sa) * _2_0
601 rB = atan2(r, sb) * _2_0
602 rC = fsum_(PI, -rA, -rB)
603 if min(rA, rB, rC) < 0:
604 raise ValueError(_colinear_)
605 elif c < 0:
606 raise ValueError(_negative_)
607 else: # 0 <= c <= EPS0
608 rA = rB = PI_2
609 rC = r = _0_0
611 if bc:
612 rB, rC = rC, rB
613 if ab:
614 rA, rB = rB, rA
615 return TriAngle4Tuple(rA, rB, rC, r, name=triAngle4.__name__)
617 except (TypeError, ValueError) as x:
618 raise TriangleError(a=a, b=b, c=c, cause=x)
621def triSide(a, b, radC):
622 '''Compute one side of a triangle.
624 @arg a: Adjacent triangle side length (C{scalar},
625 non-negative C{meter}, conventionally).
626 @arg b: Adjacent triangle side length (C{scalar},
627 non-negative C{meter}, conventionally).
628 @arg radC: Angle included by sides B{C{a}} and B{C{b}},
629 opposite triangle side C{c} (C{radians}).
631 @return: Length of triangle side C{c}, opposite triangle
632 corner C{C} and angle B{C{radC}}, same units as
633 B{C{a}} and B{C{b}}.
635 @raise TriangleError: Invalid B{C{a}}, B{C{b}} or B{C{radC}}.
637 @see: Functions L{pygeodesy.sqrt_a}, L{pygeodesy.triAngle},
638 L{pygeodesy.triSide2} and L{pygeodesy.triSide4}.
639 '''
640 try:
641 return _triSide(a, b, radC)
642 except (TypeError, ValueError) as x:
643 raise TriangleError(a=a, b=b, radC=radC, cause=x)
646def _triSide(a, b, radC):
647 # (INTERNAL) To allow callers to embellish errors
648 a, b, r = map1(float, a, b, radC)
649 if min(a, b, r) < 0:
650 raise ValueError(_negative_)
652 if a < b:
653 a, b = b, a
654 if a > EPS0:
655 ba = b / a
656 c2 = fsum_(_1_0, ba**2, _N_2_0 * ba * cos(r))
657 if c2 > EPS02:
658 return a * sqrt(c2)
659 elif c2 < 0:
660 raise ValueError(_invalid_)
661 return hypot(a, b)
664def triSide2(b, c, radB):
665 '''Compute one side and the opposite angle of a triangle.
667 @arg b: Adjacent triangle side length (C{scalar},
668 non-negative C{meter}, conventionally).
669 @arg c: Adjacent triangle side length (C{scalar},
670 non-negative C{meter}, conventionally).
671 @arg radB: Angle included by sides B{C{a}} and B{C{c}},
672 opposite triangle side C{b} (C{radians}).
674 @return: L{TriSide2Tuple}C{(a, radA)} with triangle angle
675 C{radA} in C{radians} and length of the opposite
676 triangle side C{a}, same units as B{C{b}} and B{C{c}}.
678 @raise TriangleError: Invalid B{C{b}} or B{C{c}} or either
679 B{C{b}} or B{C{radB}} near zero.
681 @see: Functions L{pygeodesy.sqrt_a}, L{pygeodesy.triSide}
682 and L{pygeodesy.triSide4}.
683 '''
684 try:
685 return _triSide2(b, c, radB)
686 except (TypeError, ValueError) as x:
687 raise TriangleError(b=b, c=c, radB=radB, cause=x)
690def _triSide2(b, c, radB):
691 # (INTERNAL) To allow callers to embellish errors
692 b, c, rB = map1(float, b, c, radB)
693 if min(b, c, rB) < 0:
694 raise ValueError(_negative_)
695 sB, cB = sincos2(rB)
696 if isnear0(sB):
697 if not isnear0(b):
698 raise ValueError(_invalid_)
699 if cB < 0:
700 a, rA = (b + c), PI
701 else:
702 a, rA = fabs(b - c), _0_0
703 elif isnear0(b):
704 raise ValueError(_invalid_)
705 else:
706 rA = fsum_(PI, -rB, -asin1(c * sB / b))
707 a = sin(rA) * b / sB
708 return TriSide2Tuple(a, rA, name=triSide2.__name__)
711def triSide4(radA, radB, c):
712 '''Compute two sides and the height of a triangle.
714 @arg radA: Angle at triangle corner C{A}, opposite triangle side C{a}
715 (non-negative C{radians}).
716 @arg radB: Angle at triangle corner C{B}, opposite triangle side C{b}
717 (non-negative C{radians}).
718 @arg c: Length of triangle side between triangle corners C{A} and C{B},
719 (C{scalar}, non-negative C{meter}, conventionally).
721 @return: L{TriSide4Tuple}C{(a, b, radC, d)} with triangle sides C{a} and
722 C{b} and triangle height C{d} perpendicular to triangle side
723 B{C{c}}, all in the same units as B{C{c}} and interior angle
724 C{radC} in C{radians} at triangle corner C{C}, opposite
725 triangle side B{C{c}}.
727 @raise TriangleError: Invalid or negative B{C{radA}}, B{C{radB}} or B{C{c}}.
729 @see: U{Triangulation, Surveying<https://WikiPedia.org/wiki/Triangulation_(surveying)>}
730 and functions L{pygeodesy.sqrt_a}, L{pygeodesy.triSide} and L{pygeodesy.triSide2}.
731 '''
732 try:
733 rA, rB, c = map1(float, radA, radB, c)
734 rC = fsum_(PI, -rA, -rB)
735 if min(rC, rA, rB, c) < 0:
736 raise ValueError(_negative_)
737 sa, ca, sb, cb = sincos2_(rA, rB)
738 sc = fsum1_(sa * cb, sb * ca)
739 if sc < EPS0 or min(sa, sb) < 0:
740 raise ValueError(_invalid_)
741 sc = c / sc
742 return TriSide4Tuple((sa * sc), (sb * sc), rC, (sa * sb * sc),
743 name=triSide4.__name__)
745 except (TypeError, ValueError) as x:
746 raise TriangleError(radA=radA, radB=radB, c=c, cause=x)
749def wildberger3(a, b, c, alpha, beta, R3=min):
750 '''Snellius' surveying using U{Rational Trigonometry<https://WikiPedia.org/wiki/Snellius–Pothenot_problem>}.
752 @arg a: Length of the triangle side between corners C{B} and C{C} and opposite of
753 triangle corner C{A} (C{scalar}, non-negative C{meter}, conventionally).
754 @arg b: Length of the triangle side between corners C{C} and C{A} and opposite of
755 triangle corner C{B} (C{scalar}, non-negative C{meter}, conventionally).
756 @arg c: Length of the triangle side between corners C{A} and C{B} and opposite of
757 triangle corner C{C} (C{scalar}, non-negative C{meter}, conventionally).
758 @arg alpha: Angle subtended by triangle side B{C{b}} (C{degrees}, non-negative).
759 @arg beta: Angle subtended by triangle side B{C{a}} (C{degrees}, non-negative).
760 @kwarg R3: Callable to determine C{R3} from C{(R3 - C)**2 = D}, typically standard
761 function C{min} or C{max}, invoked with 2 arguments.
763 @return: L{Survey3Tuple}C{(PA, PB, PC)} with distance from survey point C{P} to
764 each of the triangle corners C{A}, C{B} and C{C}, same units as B{C{a}},
765 B{C{b}} and B{C{c}}.
767 @raise TriangleError: Invalid B{C{a}}, B{C{b}} or B{C{c}} or negative B{C{alpha}} or
768 B{C{beta}} or B{C{R3}} not C{callable}.
770 @see: U{Wildberger, Norman J.<https://math.sc.Chula.ac.TH/cjm/content/
771 survey-article-greek-geometry-rational-trigonometry-and-snellius-–-pothenot-surveying>},
772 U{Devine Proportions, page 252<http://www.ms.LT/derlius/WildbergerDivineProportions.pdf>}
773 and function L{pygeodesy.snellius3}.
774 '''
775 def _s(x):
776 return sin(x)**2
778 def _vpa(r1, r3, q2, q3, s3):
779 r = r1 * r3 * _4_0
780 n = (r - Fsum(r1, r3, -q2).fpow(2)).fover(s3)
781 if n < 0 or isnear0(r):
782 raise ValueError(_coincident_)
783 return sqrt((n / r) * q3) if n else _0_0
785 try:
786 a, b, c, da, db = map1(float, a, b, c, alpha, beta)
787 if min(a, b, c, da, db) < 0:
788 raise ValueError(_negative_)
790 ra, rb = radians(da), radians(db)
791 s1, s2, s3 = s = map1(_s, rb, ra, ra + rb) # rb, ra!
792 if min(s) < EPS02:
793 raise ValueError(_or(_coincident_, _colinear_))
795 q1, q2, q3 = q = a**2, b**2, c**2
796 if min(q) < EPS02:
797 raise ValueError(_coincident_)
799 r1 = s2 * q3 / s3 # s2!
800 r2 = s1 * q3 / s3 # s1!
801 Qs = Fsum(*q) # == hypot2_(a, b, c)
802 Ss = Fsum(*s) # == fsum1(s, floats=True)
803 s += Qs * _0_5, # tuple!
804 C0 = Fdot(s, q1, q2, q3, -Ss)
805 r3 = C0.fover(-s3)
806 d0 = Qs.fpow(2).fsub_(hypot2_(*q) * _2_0).fmul(s1 * s2).fover(s3)
807 if d0 > EPS02: # > c0
808 d0 = sqrt(d0)
809 if not callable(R3):
810 raise ValueError(_R3__ + _not_(callable.__name__))
811 r3 = R3(float(C0 + d0), float(C0 - d0)) # XXX min or max
812 elif d0 < 0:
813 raise ValueError(_negative_)
815 pa = _vpa(r1, r3, q2, q3, s3)
816 pb = _vpa(r2, r3, q1, q3, s3)
817 pc = favg(_triSide2(b, pa, ra).a,
818 _triSide2(a, pb, rb).a)
819 return Survey3Tuple(pa, pb, pc, name=wildberger3.__name__)
821 except (TypeError, ValueError) as x:
822 raise TriangleError(a=a, b=b, c=c, alpha=alpha, beta=beta, R3=R3, cause=x)
825def _zidw(A, B, C, x, y):
826 # interpolate z or coplanar with A, B and C?
827 t = A.z, B.z, C.z
828 m = Vector3d(x, y, fmean(t)).minus
829 return fidw(t, (m(T).length for T in (A, B, C)))
831# **) MIT License
832#
833# Copyright (C) 2016-2023 -- mrJean1 at Gmail -- All Rights Reserved.
834#
835# Permission is hereby granted, free of charge, to any person obtaining a
836# copy of this software and associated documentation files (the "Software"),
837# to deal in the Software without restriction, including without limitation
838# the rights to use, copy, modify, merge, publish, distribute, sublicense,
839# and/or sell copies of the Software, and to permit persons to whom the
840# Software is furnished to do so, subject to the following conditions:
841#
842# The above copyright notice and this permission notice shall be included
843# in all copies or substantial portions of the Software.
844#
845# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
846# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
847# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
848# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
849# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
850# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
851# OTHER DEALINGS IN THE SOFTWARE.