Coverage for pygeodesy/resections.py: 97%

372 statements  

« prev     ^ index     » next       coverage.py v7.2.2, created at 2023-10-11 16:04 -0400

1 

2# -*- coding: utf-8 -*- 

3 

4u'''3-Point resection functions L{cassini}, L{collins5}, L{pierlot}, L{pierlotx} and 

5L{tienstra7}, survey functions L{snellius3} and L{wildberger3} and triangle functions 

6L{triAngle}, L{triAngle4}, L{triSide}, L{triSide2} and L{triSide4}. 

7 

8@note: Functions L{pierlot} and L{pierlotx} are transcoded to Python with permission from 

9 U{triangulationPierlot<http://www.Telecom.ULg.ac.BE/triangulation/doc/total_8c.html>} and 

10 U{Pierlot<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 

14 

15from pygeodesy.basics import map1, _ALL_LAZY 

16from pygeodesy.constants import EPS, EPS0, EPS02, INT0, NEG0, PI, PI2, PI_2, PI_4, \ 

17 _0_0, _0_5, _1_0, _N_1_0, _2_0, _N_2_0, _4_0, _16_0, \ 

18 _180_0, _360_0, isnear0, _over, _umod_360 

19from pygeodesy.errors import _and, _or, TriangleError, _ValueError, _xkwds 

20from pygeodesy.fmath import favg, Fdot, fidw, fmean, hypot, hypot2_ 

21from pygeodesy.fsums import Fsum, fsumf_, fsum1, fsum1f_ 

22from pygeodesy.interns import _a_, _A_, _area_, _b_, _B_, _c_, _C_, _coincident_, \ 

23 _colinear_, _d_, _eps_, _invalid_, _negative_, \ 

24 _not_, _rIn_, _SPACE_ 

25# from pygeodesy.lazily import _ALL_LAZY # from .basics 

26from pygeodesy.named import _NamedTuple, _Pass, Fmt 

27# from pygeodesy.streprs import Fmt # from .named 

28from pygeodesy.units import Degrees, Distance, Radians 

29from pygeodesy.utily import acos1, asin1, sincos2, sincos2_, sincos2d, sincos2d_ 

30from pygeodesy.vector3d import _otherV3d, Vector3d 

31 

32from math import cos, atan2, degrees, fabs, radians, sin, sqrt 

33 

34__all__ = _ALL_LAZY.resections 

35__version__ = '23.09.12' 

36 

37_concyclic_ = 'concyclic' 

38_PA_ = 'PA' 

39_PB_ = 'PB' 

40_PC_ = 'PC' 

41_pointH_ = 'pointH' 

42_pointP_ = 'pointP' 

43_positive_ = 'positive' 

44_R3__ = 'R3 ' 

45_radA_ = 'radA' 

46_radB_ = 'radB' 

47_radC_ = 'radC' 

48 

49 

50class Collins5Tuple(_NamedTuple): 

51 '''5-Tuple C{(pointP, pointH, a, b, c)} with survey C{pointP}, auxiliary 

52 C{pointH}, each an instance of B{C{pointA}}'s (sub-)class and triangle 

53 sides C{a}, C{b} and C{c} in C{meter}, conventionally. 

54 ''' 

55 _Names_ = (_pointP_, _pointH_, _a_, _b_, _c_) 

56 _Units_ = (_Pass, _Pass, Distance, Distance, Distance) 

57 

58 

59class ResectionError(_ValueError): 

60 '''Error raised for issues in L{pygeodesy.resections}. 

61 ''' 

62 pass 

63 

64 

65class Survey3Tuple(_NamedTuple): 

66 '''3-Tuple C{(PA, PB, PC)} with distance from survey point C{P} to each of 

67 the triangle corners C{A}, C{B} and C{C} in C{meter}, conventionally. 

68 ''' 

69 _Names_ = (_PA_, _PB_, _PC_) 

70 _Units_ = ( Distance, Distance, Distance) 

71 

72 

73class Tienstra7Tuple(_NamedTuple): 

74 '''7-Tuple C{(pointP, A, B, C, a, b, c)} with survey C{pointP}, interior 

75 triangle angles C{A}, C{B} and C{C} in C{degrees} and triangle sides 

76 C{a}, C{b} and C{c} in C{meter}, conventionally. 

77 ''' 

78 _Names_ = (_pointP_, _A_, _B_, _C_, _a_, _b_, _c_) 

79 _Units_ = (_Pass, Degrees, Degrees, Degrees, Distance, Distance, Distance) 

80 

81 

82class TriAngle5Tuple(_NamedTuple): 

83 '''5-Tuple C{(radA, radB, radC, rIn, area)} with the interior angles at 

84 triangle corners C{A}, C{B} and C{C} in C{radians}, the C{InCircle} 

85 radius C{rIn} aka C{inradius} in C{meter} and the triangle C{area} 

86 in C{meter} I{squared}, conventionally. 

87 ''' 

88 _Names_ = (_radA_, _radB_, _radC_, _rIn_, _area_) 

89 _Units_ = ( Radians, Radians, Radians, Distance, _Pass) 

90 

91 

92class TriSide2Tuple(_NamedTuple): 

93 '''2-Tuple C{(a, radA)} with triangle side C{a} in C{meter}, conventionally 

94 and angle C{radA} at the opposite triangle corner in C{radians}. 

95 ''' 

96 _Names_ = (_a_, _radA_) 

97 _Units_ = ( Distance, Radians) 

98 

99 

100class TriSide4Tuple(_NamedTuple): 

101 '''4-Tuple C{(a, b, radC, d)} with interior angle C{radC} at triangle corner 

102 C{C} in C{radians}with the length of triangle sides C{a} and C{b} and 

103 with triangle height C{d} perpendicular to triangle side C{c}, in the 

104 same units as triangle sides C{a} and C{b}. 

105 ''' 

106 _Names_ = (_a_, _b_, _radC_, _d_) 

107 _Units_ = ( Distance, Distance, Radians, Distance) 

108 

109 

110def _ABC3(useZ, pointA, pointB, pointC): 

111 '''(INTERNAL) Helper for L{cassini} and L{tienstra7}. 

112 ''' 

113 return (_otherV3d(useZ=useZ, pointA=pointA), 

114 _otherV3d(useZ=useZ, pointB=pointB), 

115 _otherV3d(useZ=useZ, pointC=pointC)) 

116 

117 

118def _B3(useZ, point1, point2, point3): 

119 '''(INTERNAL) Helper for L{pierlot} and L{pierlotx}. 

120 ''' 

121 return (_otherV3d(useZ=useZ, point1=point1), 

122 _otherV3d(useZ=useZ, point2=point2), 

123 _otherV3d(useZ=useZ, point3=point3)) 

124 

125 

126def cassini(pointA, pointB, pointC, alpha, beta, useZ=False, Clas=None, **Clas_kwds): 

127 '''3-Point resection using U{Cassini<https://NL.WikiPedia.org/wiki/Achterwaartse_insnijding>}'s method. 

128 

129 @arg pointA: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

130 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

131 @arg pointB: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

132 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

133 @arg pointC: Center point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

134 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

135 @arg alpha: Angle subtended by triangle side B{C{pointA}} to B{C{pointC}} 

136 (C{degrees}, non-negative). 

137 @arg beta: Angle subtended by triangle side B{C{pointB}} to B{C{pointC}} 

138 (C{degrees}, non-negative). 

139 @kwarg useZ: If C{True}, use and interpolate the Z component, otherwise 

140 force C{z=INT0} (C{bool}). 

141 @kwarg Clas: Optional class to return the survey point or C{None} for 

142 B{C{pointA}}'s (sub-)class. 

143 @kwarg Clas_kwds: Optional, additional keyword argument for the survey 

144 point instance. 

145 

146 @note: Typically, B{C{pointC}} is between B{C{pointA}} and B{C{pointB}}. 

147 

148 @return: The survey point, an instance of B{C{Clas}} or if C{B{Clas} is 

149 None} an instance of B{C{pointA}}'s (sub-)class. 

150 

151 @raise ResectionError: Near-coincident, -colinear or -concyclic points 

152 or negative or invalid B{C{alpha}} or B{C{beta}}. 

153 

154 @raise TypeError: Invalid B{C{pointA}}, B{C{pointB}} or B{C{pointM}}. 

155 

156 @see: U{Three Point Resection Problem<https://Dokumen.tips/documents/ 

157 three-point-resection-problem-introduction-kaestner-burkhardt-method.html>} 

158 and functions L{pygeodesy.collins5}, L{pygeodesy.pierlot} and 

159 L{pygeodesy.tienstra7}. 

160 ''' 

161 

162 def _H(A, C, sa): 

163 s, c = sincos2d(sa) 

164 if isnear0(s): 

165 raise ValueError(_or(_coincident_, _colinear_)) 

166 t = s, c, c 

167 x = Fdot(t, A.x, C.y, -A.y).fover(s) 

168 y = Fdot(t, A.y, -C.x, A.x).fover(s) 

169 return x, y 

170 

171 A, B, C = _ABC3(useZ, pointA, pointB, pointC) 

172 try: 

173 sa, sb = map1(float, alpha, beta) 

174 if min(sa, sb) < 0: 

175 raise ValueError(_negative_) 

176 if fsumf_(_360_0, -sa, -sb) < EPS0: 

177 raise ValueError() 

178 

179 x1, y1 = _H(A, C, sa) 

180 x2, y2 = _H(B, C, -sb) 

181 

182 x = x1 - x2 

183 y = y1 - y2 

184 if isnear0(x) or isnear0(y): 

185 raise ValueError(_SPACE_(_concyclic_, (x, y))) 

186 

187 m = y / x 

188 n = x / y 

189 N = n + m 

190 if isnear0(N): 

191 raise ValueError(_SPACE_(_concyclic_, (m, n, N))) 

192 

193 t = n, m, _1_0, _N_1_0 

194 x = Fdot(t, C.x, x1, C.y, y1).fover(N) 

195 y = Fdot(t, y1, C.y, C.x, x1).fover(N) 

196 z = _zidw(x, y, useZ, A, B, C) 

197 

198 clas = Clas or pointA.classof 

199 return clas(x, y, z, **_xkwds(Clas_kwds, name=cassini.__name__)) 

200 

201 except (TypeError, ValueError) as x: 

202 raise ResectionError(pointA=pointA, pointB=pointB, pointC=pointC, 

203 alpha=alpha, beta=beta, cause=x) 

204 

205 

206def collins5(pointA, pointB, pointC, alpha, beta, useZ=False, Clas=None, **Clas_kwds): 

207 '''3-Point resection using U{Collins<https://Dokumen.tips/documents/ 

208 three-point-resection-problem-introduction-kaestner-burkhardt-method.html>}' method. 

209 

210 @arg pointA: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

211 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

212 @arg pointB: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

213 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

214 @arg pointC: Center point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

215 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

216 @arg alpha: Angle subtended by triangle side C{b} from B{C{pointA}} to 

217 B{C{pointC}} (C{degrees}, non-negative). 

218 @arg beta: Angle subtended by triangle side C{a} from B{C{pointB}} to 

219 B{C{pointC}} (C{degrees}, non-negative). 

220 @kwarg useZ: If C{True}, use and interpolate the Z component, otherwise 

221 force C{z=INT0} (C{bool}). 

222 @kwarg Clas: Optional class to return the survey and auxiliary point 

223 or C{None} for B{C{pointA}}'s (sub-)class. 

224 @kwarg Clas_kwds: Optional, additional keyword argument for the survey 

225 and auxiliary point instance. 

226 

227 @note: Typically, B{C{pointC}} is between B{C{pointA}} and B{C{pointB}}. 

228 

229 @return: L{Collins5Tuple}C{(pointP, pointH, a, b, c)} with survey C{pointP}, 

230 auxiliary C{pointH}, each an instance of B{C{Clas}} or if C{B{Clas} 

231 is None} an instance of B{C{pointA}}'s (sub-)class and triangle 

232 sides C{a}, C{b} and C{c} in C{meter}, conventionally. 

233 

234 @raise ResectionError: Near-coincident, -colinear or -concyclic points 

235 or negative or invalid B{C{alpha}} or B{C{beta}}. 

236 

237 @raise TypeError: Invalid B{C{pointA}}, B{C{pointB}} or B{C{pointM}}. 

238 

239 @see: U{Collins' methode<https://NL.WikiPedia.org/wiki/Achterwaartse_insnijding>} 

240 and functions L{pygeodesy.cassini}, L{pygeodesy.pierlot} and 

241 L{pygeodesy.tienstra7}. 

242 ''' 

243 

244 def _azi_len2(A, B, pi2): 

245 v = B.minus(A) 

246 r = atan2(v.x, v.y) 

247 if pi2 and r < 0: 

248 r += pi2 

249 return r, v.length 

250 

251 def _cV3(d, r, A, B, C, useZ, V3, **kwds): 

252 s, c = sincos2(r) 

253 x = A.x + d * s 

254 y = A.y + d * c 

255 z = _zidw(x, y, useZ, A, B, C) 

256 return V3(x, y, z, **kwds) 

257 

258 A, B, C = _ABC3(useZ, pointA, pointB, pointC) 

259 try: 

260 ra, rb = radians(alpha), radians(beta) 

261 if min(ra, rb) < 0: 

262 raise ValueError(_negative_) 

263 

264 sra, srH = sin(ra), sin(ra + rb - PI) # rH = PI - ((PI - ra) + (PI - rb)) 

265 if isnear0(sra) or isnear0(srH): 

266 raise ValueError(_or(_coincident_, _colinear_, _concyclic_)) 

267 

268 clas = Clas or pointA.classof 

269 kwds = _xkwds(Clas_kwds, name=collins5.__name__) 

270 

271# za, a = _azi_len2(C, B, PI2) 

272 zb, b = _azi_len2(C, A, PI2) 

273 zc, c = _azi_len2(A, B, 0) 

274 

275# d = c * sin(PI - rb) / srH # B.minus(H).length 

276 d = c * sin(PI - ra) / srH # A.minus(H).length 

277 r = zc + PI - rb # zh = zc + (PI - rb) 

278 H = _cV3(d, r, A, B, C, useZ, Vector3d) 

279 

280 zh, _ = _azi_len2(C, H, PI2) 

281 

282# d = a * sin(za - zh) / sin(rb) # B.minus(P).length 

283 d = b * sin(zb - zh) / sra # A.minus(P).length 

284 r = zh - ra # zb - PI + (PI - ra - (zb - zh)) 

285 P = _cV3(d, r, A, B, C, useZ, clas, **kwds) 

286 

287 H = clas(H.x, H.y, H.z, **kwds) 

288 a = B.minus(C).length 

289 

290 return Collins5Tuple(P, H, a, b, c, name=collins5.__name__) 

291 

292 except (TypeError, ValueError) as x: 

293 raise ResectionError(pointA=pointA, pointB=pointB, pointC=pointC, 

294 alpha=alpha, beta=beta, cause=x) 

295 

296 

297def pierlot(point1, point2, point3, alpha12, alpha23, useZ=False, eps=EPS, 

298 Clas=None, **Clas_kwds): 

299 '''3-Point resection using U{Pierlot<http://www.Telecom.ULg.ac.BE/publi/publications/ 

300 pierlot/Pierlot2014ANewThree>}'s method C{ToTal} with I{approximate} limits for 

301 the (pseudo-)singularities. 

302 

303 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

304 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

305 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

306 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

307 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

308 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

309 @arg alpha12: Angle subtended from B{C{point1}} to B{C{point2}} or 

310 B{C{alpha2 - alpha1}} (C{degrees}). 

311 @arg alpha23: Angle subtended from B{C{point2}} to B{C{point3}} or 

312 B{C{alpha3 - alpha2}}(C{degrees}). 

313 @kwarg useZ: If C{True}, interpolate the survey point's Z component, 

314 otherwise use C{z=INT0} (C{bool}). 

315 @kwarg eps: Tolerance for C{cot} (pseudo-)singularities (C{float}). 

316 @kwarg Clas: Optional class to return the survey point, if C{None} use 

317 B{C{point1}}'s (sub-)class. 

318 @kwarg Clas_kwds: Optional, additional keyword arguments for the survey 

319 point instance. 

320 

321 @note: Typically, B{C{point1}}, B{C{point2}} and B{C{point3}} are ordered 

322 by angle, modulo 360, counter-clockwise. 

323 

324 @return: The survey (or robot) point, an instance of B{C{Clas}} or if 

325 C{B{Clas} is None} an instance of B{C{point1}}'s (sub-)class. 

326 

327 @raise ResectionError: Near-coincident, -colinear or -concyclic points 

328 or invalid B{C{alpha12}} or B{C{alpha23}} or 

329 non-positive B{C{eps}}. 

330 

331 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}. 

332 

333 @see: I{Pierlot's} C function U{triangulationPierlot<http://www.Telecom.ULg.ac.BE/ 

334 triangulation/doc/total_8c_source.html>}, U{V. Pierlot, M. Van Droogenbroeck, 

335 "A New Three Object Triangulation Algorithm for Mobile Robot Positioning" 

336 <https://ORBi.ULiege.BE/bitstream/2268/157469/1/Pierlot2014ANewThree.pdf>}, 

337 U{Vincent Pierlot, Marc Van Droogenbroeck, "18 Triangulation Algorithms for 2D 

338 Positioning (also known as the Resection Problem)"<http://www.Telecom.ULg.ac.BE/ 

339 triangulation>} and functions L{pygeodesy.pierlotx}, L{pygeodesy.cassini}, 

340 L{pygeodesy.collins5} and L{pygeodesy.tienstra7}. 

341 ''' 

342 

343 def _cot(s, c): # -eps < I{approximate} cotangent < eps 

344 if eps > 0: 

345 return c / (min(s, -eps) if s < 0 else max(s, eps)) 

346 raise ValueError(_SPACE_(_eps_, _not_, _positive_)) 

347 

348 B1, B2, B3 = _B3(useZ, point1, point2, point3) 

349 try: 

350 x, y, z = _pierlot3(B1, B2, B3, alpha12, alpha23, useZ, _cot) 

351 clas = Clas or point1.classof 

352 return clas(x, y, z, **_xkwds(Clas_kwds, name=pierlot.__name__)) 

353 

354 except (TypeError, ValueError) as x: 

355 raise ResectionError(point1=point1, point2=point2, point3=point3, 

356 alpha12=alpha12, alpha23=alpha23, eps=eps, cause=x) 

357 

358 

359def _pierlot3(B1, B2, B3, a12, a23, useZ, cot): 

360 '''(INTERNAL) Shared L{pierlot} and L{pierlotx}. 

361 ''' 

362 x1_, y1_, _ = B1.minus(B2).xyz 

363 x3_, y3_, _ = B3.minus(B2).xyz 

364 

365 s12, c12, s23, c23 = sincos2d_(a12, a23) 

366 # cot31 = (1 - cot12 * cot23) / (cot12 + cot32) 

367 # = (1 - c12 / s12 * c23 / s23) / (c12 / s12 + c23 / s23) 

368 # = (1 - (c12 * c23) / (s12 * s23)) / (c12 * s23 + s12 * c23) / (s12 * s23) 

369 # = (s12 * s23 - c12 * c23) / (c12 * s23 + s12 * c23) 

370 # = c31 / s31 

371 cot31 = cot(fsum1f_(c12 * s23, s12 * c23), # s31 

372 fsum1f_(s12 * s23, -c12 * c23)) # c31 

373 

374 K = Fsum(x3_ * x1_, cot31 * (y3_ * x1_), 

375 y3_ * y1_, -cot31 * (x3_ * y1_)) 

376 if K: 

377 cot12 = cot(s12, c12) 

378 cot23 = cot(s23, c23) 

379 

380 # x12 = x1_ + cot12 * y1_ 

381 # y12 = y1_ - cot12 * x1_ 

382 

383 # x23 = x3_ - cot23 * y3_ 

384 # y23 = y3_ + cot23 * x3_ 

385 

386 # x31 = x3_ + x1_ + cot31 * (y3_ - y1_) 

387 # y31 = y3_ + y1_ - cot31 * (x3_ - x1_) 

388 

389 # x12 - x23 = x1_ + cot12 * y1_ - x3_ + cot23 * y3_ 

390 X12_23 = Fsum(x1_, cot12 * y1_, -x3_, cot23 * y3_) 

391 # y12 - y23 = y1_ - cot12 * x1_ - y3_ - cot23 * x3_ 

392 Y12_23 = Fsum(y1_, -cot12 * x1_, -y3_, -cot23 * x3_) 

393 

394 # x31 - x23 = x3_ + x1_ + cot31 * (y3_ - y1_) - x3_ + cot23 * y3_ 

395 # = x1_ + cot31 * y3_ - cot31 * y1_ + cot23 * y3_ 

396 X31_23 = Fsum(x1_, -cot31 * y1_, cot31 * y3_, cot23 * y3_) 

397 # y31 - y23 = y3_ + y1_ - cot31 * (x3_ - x1_) - y3_ - cot23 * x3_ 

398 # = y1_ - cot31 * x3_ + cot31 * x1_ - cot23 * x3_ 

399 Y31_23 = Fsum(y1_, cot31 * x1_, -cot31 * x3_, -cot23 * x3_) 

400 

401 # d = (x12 - x23) * (y23 - y31) + (x31 - x23) * (y12 - y23) 

402 # = (x31 - x23) * (y12 - y23) - (x12 - x23) * (y12 - y23) 

403 d = float(X31_23 * Y12_23 - X12_23 * Y31_23) 

404 if isnear0(d): 

405 raise ValueError(_or(_coincident_, _colinear_, _concyclic_)) 

406 

407 x = (B2.x * d + K * Y12_23).fover(d) 

408 y = (B2.y * d - K * X12_23).fover(d) 

409 else: 

410 x, y, _ = B2.xyz 

411 return x, y, _zidw(x, y, useZ, B1, B2, B3) 

412 

413 

414def _pierlotx3(a_z_Bs, useZ, cot, C): 

415 '''(INTERNAL) Core of L{pierlotx}. 

416 ''' 

417 (a12, z12, B1), \ 

418 (a23, z23, B2), \ 

419 (a31, z31, B3) = a_z_Bs 

420 if z12 and not z23: 

421 C(1) 

422 elif z23 and not z31: 

423 C(2) 

424 a23, B1, B2, B3 = a31, B2, B3, B1 

425 elif z31 and not z12: 

426 C(3) 

427 a23, B2, B3 = a12, B3, B2 

428 else: 

429 C(4) 

430 return _pierlot3(B1, B2, B3, a12, a23, useZ, cot) 

431 

432 x1_, y1_, _ = B1.minus(B3).xyz 

433 x2_, y2_, _ = B2.minus(B3).xyz 

434 

435 K = Fsum(_1_0, y1_ * x2_, -x1_ * y2_, _N_1_0) # 1-primed 

436 if K: 

437 cot23 = cot(*sincos2d(a23)) 

438 

439 # x23 = x2_ + cot23 * y2_ 

440 # y23 = y2_ - cot23 * x2_ 

441 

442 # x31 = x1_ + cot23 * y1_ 

443 # y31 = y1_ - cot23 * x1_ 

444 

445 # x31 - x23 = x1_ + cot23 * y1_ - x2_ - cot23 * y2_ 

446 X31_23 = Fsum(x1_, cot23 * y1_, -x2_, -cot23 * y2_) 

447 # y31 - y23 = y1_ - cot23 * x1_ - y2_ + cot23 * x2_ 

448 Y31_23 = Fsum(y1_, -cot23 * x1_, -y2_, cot23 * x2_) 

449 

450 # d = (x31 - x23) * (x2_ - x1_) - (y31 - y23) * (y1_ - y2_) 

451 d = float(X31_23 * x2_ - X31_23 * x1_ + 

452 Y31_23 * y2_ - Y31_23 * y1_) 

453 if isnear0(d): 

454 raise ValueError(_or(_coincident_, _colinear_, _concyclic_)) 

455 

456 x = (B3.x * d - K * Y31_23).fover(d) 

457 y = (B3.y * d + K * X31_23).fover(d) 

458 else: 

459 x, y, _ = B3.xyz 

460 return x, y, _zidw(x, y, useZ, B1, B2, B3) 

461 

462 

463def pierlotx(point1, point2, point3, alpha1, alpha2, alpha3, useZ=False, 

464 Clas=None, **Clas_kwds): 

465 '''3-Point resection using U{Pierlot<http://www.Telecom.ULg.ac.BE/publi/ 

466 publications/pierlot/Pierlot2014ANewThree>}'s method C{ToTal} with 

467 I{exact} limits for the (pseudo-)singularities. 

468 

469 @arg point1: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

470 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

471 @arg point2: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

472 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

473 @arg point3: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, 

474 C{Vector4Tuple} or C{Vector2Tuple} if C{B{useZ}=False}). 

475 @arg alpha1: Angle at B{C{point1}} (C{degrees}, counter-clockwise). 

476 @arg alpha2: Angle at B{C{point2}} (C{degrees}, counter-clockwise). 

477 @arg alpha3: Angle at B{C{point3}} (C{degrees}, counter-clockwise). 

478 @kwarg useZ: If C{True}, interpolate the survey point's Z component, 

479 otherwise use C{z=INT0} (C{bool}). 

480 @kwarg Clas: Optional class to return the survey point, if C{None} use 

481 B{C{point1}}'s (sub-)class. 

482 @kwarg Clas_kwds: Optional, additional keyword arguments for the survey 

483 point instance. 

484 

485 @return: The survey (or robot) point, an instance of B{C{Clas}} or if 

486 C{B{Clas} is None} an instance of B{C{point1}}'s (sub-)class. 

487 

488 @raise ResectionError: Near-coincident, -colinear or -concyclic points or 

489 invalid B{C{alpha1}}, B{C{alpha2}} or B{C{alpha3}}. 

490 

491 @raise TypeError: Invalid B{C{point1}}, B{C{point2}} or B{C{point3}}. 

492 

493 @see: I{Pierlot's} C function U{triangulationPierlot2<http://www.Telecom.ULg.ac.BE/ 

494 triangulation/doc/total_8c_source.html>} and function L{pygeodesy.pierlot}. 

495 ''' 

496 

497 def _a_z_Bs(Bs, *alphas): 

498 a3 = map(_umod_360, alphas) # 0 <= alphas < 360 

499 (a1, a2, a3), (B1, B2, B3) = zip(*sorted(zip(a3, Bs))) 

500 for a, d, B in ((a1, a2, B1), (a2, a3, B2), (a3, a1, B3)): 

501 d -= a # a12 = a2 - a1, ... 

502 z = isnear0(fabs(d) % _180_0) 

503 yield d, z, B 

504 

505 def _cot(s, c): # I{exact} cotangent 

506 try: 

507 return (c / s) if c else (NEG0 if s < 0 else _0_0) 

508 except ZeroDivisionError: 

509 raise ValueError(_or(_coincident_, _colinear_)) 

510 

511 Bs = _B3(useZ, point1, point2, point3) 

512 try: 

513 C = [0] # pseudo-global, passing the exception Case 

514 x, y, z = _pierlotx3(_a_z_Bs(Bs, alpha1, alpha2, alpha3), 

515 useZ, _cot, C.append) 

516 clas = Clas or point1.classof 

517 return clas(x, y, z, **_xkwds(Clas_kwds, name=pierlotx.__name__)) 

518 

519 except (TypeError, ValueError) as x: 

520 raise ResectionError(point1=point1, point2=point2, point3=point3, C=C.pop(), 

521 alpha1=alpha1, alpha2=alpha2, alpha3=alpha3, cause=x) 

522 

523 

524def snellius3(a, b, degC, alpha, beta): 

525 '''Snellius' surveying using U{Snellius Pothenot<https://WikiPedia.org/wiki/Snellius–Pothenot_problem>}. 

526 

527 @arg a: Length of the triangle side between corners C{B} and C{C} and opposite of 

528 triangle corner C{A} (C{scalar}, non-negative C{meter}, conventionally). 

529 @arg b: Length of the triangle side between corners C{C} and C{A} and opposite of 

530 triangle corner C{B} (C{scalar}, non-negative C{meter}, conventionally). 

531 @arg degC: Angle at triangle corner C{C}, opposite triangle side C{c} (non-negative C{degrees}). 

532 @arg alpha: Angle subtended by triangle side B{C{b}} (non-negative C{degrees}). 

533 @arg beta: Angle subtended by triangle side B{C{a}} (non-negative C{degrees}). 

534 

535 @return: L{Survey3Tuple}C{(PA, PB, PC)} with distance from survey point C{P} to 

536 each of the triangle corners C{A}, C{B} and C{C}, same units as triangle 

537 sides B{C{a}}, B{C{b}} and B{C{c}}. 

538 

539 @raise TriangleError: Invalid B{C{a}}, B{C{b}} or B{C{degC}} or negative B{C{alpha}} 

540 or B{C{beta}}. 

541 

542 @see: Function L{pygeodesy.wildberger3}. 

543 ''' 

544 try: 

545 a, b, degC, alpha, beta = t = map1(float, a, b, degC, alpha, beta) 

546 if min(t) < 0: 

547 raise ValueError(_negative_) 

548 ra, rb, rC = map1(radians, alpha, beta, degC) 

549 

550 r = fsumf_(ra, rb, rC) * _0_5 

551 k = PI - r 

552 if min(k, r) < 0: 

553 raise ValueError(_or(_coincident_, _colinear_)) 

554 

555 sa, sb = sin(ra), sin(rb) 

556 p = atan2(a * sa, b * sb) 

557 sp, cp, sr, cr = sincos2_(PI_4 - p, r) 

558 w = atan2(sp * sr, cp * cr) 

559 x = k + w 

560 y = k - w 

561 

562 s = fabs(sa) 

563 if fabs(sb) > s: 

564 pc = fabs(a * sin(y) / sb) 

565 elif s: 

566 pc = fabs(b * sin(x) / sa) 

567 else: 

568 raise ValueError(_or(_colinear_, _coincident_)) 

569 

570 pa = _triSide(b, pc, fsumf_(PI, -ra, -x)) 

571 pb = _triSide(a, pc, fsumf_(PI, -rb, -y)) 

572 return Survey3Tuple(pa, pb, pc, name=snellius3.__name__) 

573 

574 except (TypeError, ValueError) as x: 

575 raise TriangleError(a=a, b=b, degC=degC, alpha=alpha, beta=beta, cause=x) 

576 

577 

578def tienstra7(pointA, pointB, pointC, alpha, beta=None, gamma=None, 

579 useZ=False, Clas=None, **Clas_kwds): 

580 '''3-Point resection using U{Tienstra<https://WikiPedia.org/wiki/Tienstra_formula>}'s formula. 

581 

582 @arg pointA: First point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, C{Vector4Tuple} or 

583 C{Vector2Tuple} if C{B{useZ}=False}). 

584 @arg pointB: Second point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, C{Vector4Tuple} or 

585 C{Vector2Tuple} if C{B{useZ}=False}). 

586 @arg pointC: Third point (C{Cartesian}, L{Vector3d}, C{Vector3Tuple}, C{Vector4Tuple} or 

587 C{Vector2Tuple} if C{B{useZ}=False}). 

588 @arg alpha: Angle subtended by triangle side C{a} from B{C{pointB}} to B{C{pointC}} 

589 (C{degrees}, non-negative). 

590 @kwarg beta: Angle subtended by triangle side C{b} from B{C{pointA}} to B{C{pointC}} 

591 (C{degrees}, non-negative) or C{None} if C{B{gamma} is not None}. 

592 @kwarg gamma: Angle subtended by triangle side C{c} from B{C{pointA}} to B{C{pointB}} 

593 (C{degrees}, non-negative) or C{None} if C{B{beta} is not None}. 

594 @kwarg useZ: If C{True}, use and interpolate the Z component, otherwise force C{z=INT0} 

595 (C{bool}). 

596 @kwarg Clas: Optional class to return the survey point or C{None} for B{C{pointA}}'s 

597 (sub-)class. 

598 @kwarg Clas_kwds: Optional, additional keyword arguments for the survey point instance. 

599 

600 @note: Points B{C{pointA}}, B{C{pointB}} and B{C{pointC}} are ordered clockwise. 

601 

602 @return: L{Tienstra7Tuple}C{(pointP, A, B, C, a, b, c)} with survey C{pointP}, an 

603 instance of B{C{Clas}} or if C{B{Clas} is None} an instance of B{C{pointA}}'s 

604 (sub-)class, with triangle angles C{A} at B{C{pointA}}, C{B} at B{C{pointB}} 

605 and C{C} at B{C{pointC}} in C{degrees} and with triangle sides C{a}, C{b} and 

606 C{c} in C{meter}, conventionally. 

607 

608 @raise ResectionError: Near-coincident, -colinear or -concyclic points or sum of 

609 B{C{alpha}}, B{C{beta}} and B{C{gamma}} not C{360} or negative 

610 B{C{alpha}}, B{C{beta}} or B{C{gamma}}. 

611 

612 @raise TypeError: Invalid B{C{pointA}}, B{C{pointB}} or B{C{pointC}}. 

613 

614 @see: U{3-Point Resection Solver<http://MesaMike.org/geocache/GC1B0Q9/tienstra/>}, 

615 U{V. Pierlot, M. Van Droogenbroeck, "A New Three Object Triangulation..." 

616 <http://www.Telecom.ULg.ac.BE/publi/publications/pierlot/Pierlot2014ANewThree/>}, 

617 U{18 Triangulation Algorithms...<http://www.Telecom.ULg.ac.BE/triangulation/>} and 

618 functions L{pygeodesy.cassini}, L{pygeodesy.collins5} and L{pygeodesy.pierlot}. 

619 ''' 

620 

621 def _deg_ks(r, s, ks, N): 

622 if isnear0(fsumf_(PI, r, -s)): # r + (PI2 - s) == PI 

623 raise ValueError(Fmt.PARENSPACED(concyclic=N)) 

624 # k = 1 / (cot(r) - cot(s)) 

625 # = 1 / (cos(r) / sin(r) - cos(s) / sin(s)) 

626 # = 1 / (cos(r) * sin(s) - cos(s) * sin(r)) / (sin(r) * sin(s)) 

627 # = sin(r) * sin(s) / (cos(r) * sin(s) - cos(s) * sin(r)) 

628 sr, cr, ss, cs = sincos2_(r, s) 

629 c = fsum1f_(cr * ss, -cs * sr) 

630 if isnear0(c): 

631 raise ValueError(Fmt.PARENSPACED(cotan=N)) 

632 ks.append(sr * ss / c) 

633 return Degrees(degrees(r), name=N) # C degrees 

634 

635 A, B, C = _ABC3(useZ, pointA, pointB, pointC) 

636 try: 

637 sa, sb, sc = map1(radians, alpha, (beta or 0), (gamma or 0)) 

638 if beta is None: 

639 if gamma is None: 

640 raise ValueError(_and(Fmt.EQUAL(beta=beta), Fmt.EQUAL(gamma=gamma))) 

641 sb = fsumf_(PI2, -sa, -sc) 

642 elif gamma is None: 

643 sc = fsumf_(PI2, -sa, -sb) 

644 else: # subtended angles must add to 360 degrees 

645 r = fsum1f_(sa, sb, sc) 

646 if fabs(r - PI2) > EPS: 

647 raise ValueError(Fmt.EQUAL(sum=degrees(r))) 

648 if min(sa, sb, sc) < 0: 

649 raise ValueError(_negative_) 

650 

651 # triangle sides 

652 a = B.minus(C).length 

653 b = A.minus(C).length 

654 c = A.minus(B).length 

655 

656 ks = [] # 3 Ks and triangle angles 

657 dA = _deg_ks(_triAngle(b, c, a), sa, ks, _A_) 

658 dB = _deg_ks(_triAngle(a, c, b), sb, ks, _B_) 

659 dC = _deg_ks(_triAngle(a, b, c), sc, ks, _C_) 

660 

661 k = fsum1(ks, floats=True) 

662 if isnear0(k): 

663 raise ValueError(Fmt.EQUAL(K=k)) 

664 x = Fdot(ks, A.x, B.x, C.x).fover(k) 

665 y = Fdot(ks, A.y, B.y, C.y).fover(k) 

666 z = _zidw(x, y, useZ, A, B, C) 

667 

668 n = tienstra7.__name__ 

669 clas = Clas or pointA.classof 

670 P = clas(x, y, z, **_xkwds(Clas_kwds, name=n)) 

671 return Tienstra7Tuple(P, dA, dB, dC, a, b, c, name=n) 

672 

673 except (TypeError, ValueError) as x: 

674 raise ResectionError(pointA=pointA, pointB=pointB, pointC=pointC, 

675 alpha=alpha, beta=beta, gamma=gamma, cause=x) 

676 

677 

678def triAngle(a, b, c): 

679 '''Compute one angle of a triangle. 

680 

681 @arg a: Adjacent triangle side length (C{scalar}, non-negative 

682 C{meter}, conventionally). 

683 @arg b: Adjacent triangle side length (C{scalar}, non-negative 

684 C{meter}, conventionally). 

685 @arg c: Opposite triangle side length (C{scalar}, non-negative 

686 C{meter}, conventionally). 

687 

688 @return: Angle in C{radians} at triangle corner C{C}, opposite 

689 triangle side B{C{c}}. 

690 

691 @raise TriangleError: Invalid or negative B{C{a}}, B{C{b}} or B{C{c}}. 

692 

693 @see: Functions L{pygeodesy.triAngle4} and L{pygeodesy.triSide}. 

694 ''' 

695 try: 

696 return _triAngle(a, b, c) 

697 except (TypeError, ValueError) as x: 

698 raise TriangleError(a=a, b=b, c=c, cause=x) 

699 

700 

701def _triAngle(a, b, c): 

702 # (INTERNAL) To allow callers to embellish errors 

703 a, b, c = map1(float, a, b, c) 

704 if a < b: 

705 a, b = b, a 

706 if b < 0 or c < 0: 

707 raise ValueError(_negative_) 

708 if a < EPS0: 

709 raise ValueError(_coincident_) 

710 b_a = b / a 

711 if b_a < EPS0: 

712 raise ValueError(_coincident_) 

713 t = fsumf_(_1_0, b_a**2, -(c / a)**2) / (b_a * _2_0) 

714 return acos1(t) 

715 

716 

717def triAngle5(a, b, c): 

718 '''Compute the angles of a triangle. 

719 

720 @arg a: Length of the triangle side opposite of triangle corner C{A} 

721 (C{scalar}, non-negative C{meter}, conventionally). 

722 @arg b: Length of the triangle side opposite of triangle corner C{B} 

723 (C{scalar}, non-negative C{meter}, conventionally). 

724 @arg c: Length of the triangle side opposite of triangle corner C{C} 

725 (C{scalar}, non-negative C{meter}, conventionally). 

726 

727 @return: L{TriAngle5Tuple}C{(radA, radB, radC, rIn, area)} with angles 

728 C{radA}, C{radB} and C{radC} at triangle corners C{A}, C{B} 

729 and C{C}, all in C{radians}, the C{InCircle} radius C{rIn} 

730 aka C{inradius}, same units as triangle sides B{C{a}}, 

731 B{C{b}} and B{C{c}} and the triangle C{area} in those same 

732 units I{squared}. 

733 

734 @raise TriangleError: Invalid or negative B{C{a}}, B{C{b}} or B{C{c}}. 

735 

736 @see: Functions L{pygeodesy.triAngle} and L{pygeodesy.triArea}. 

737 ''' 

738 try: 

739 x, y, z = map1(float, a, b, c) 

740 ab = x < y 

741 if ab: 

742 x, y = y, x 

743 bc = y < z 

744 if bc: 

745 y, z = z, y 

746 

747 if z > EPS0: # z = min(a, b, c) 

748 s = fsumf_(z, y, x) * _0_5 

749 sa, sb, r = (s - x), (s - y), (s - z) 

750 r *= _over(sa * sb, s) 

751 if r < EPS02: 

752 raise ValueError(_coincident_) 

753 r = sqrt(r) 

754 rA = atan2(r, sa) * _2_0 

755 rB = atan2(r, sb) * _2_0 

756 rC = fsumf_(PI, -rA, -rB) 

757 if min(rA, rB, rC) < 0: 

758 raise ValueError(_colinear_) 

759 s *= r # Heron's area 

760 elif z < 0: 

761 raise ValueError(_negative_) 

762 else: # 0 <= c <= EPS0 

763 rA = rB = PI_2 

764 rC = r = s = _0_0 

765 

766 if bc: 

767 rB, rC = rC, rB 

768 if ab: 

769 rA, rB = rB, rA 

770 return TriAngle5Tuple(rA, rB, rC, r, s, name=triAngle5.__name__) 

771 

772 except (TypeError, ValueError) as x: 

773 raise TriangleError(a=a, b=b, c=c, cause=x) 

774 

775 

776def triArea(a, b, c): 

777 '''Compute the area of a triangle using U{Heron's<https:// 

778 WikiPedia.org/wiki/Heron%27s_formula>} C{stable} formula. 

779 

780 @arg a: Length of the triangle side opposite of triangle corner C{A} 

781 (C{scalar}, non-negative C{meter}, conventionally). 

782 @arg b: Length of the triangle side opposite of triangle corner C{B} 

783 (C{scalar}, non-negative C{meter}, conventionally). 

784 @arg c: Length of the triangle side opposite of triangle corner C{C} 

785 (C{scalar}, non-negative C{meter}, conventionally). 

786 

787 @return: The triangle area (C{float}, conventionally C{meter} or 

788 same units as B{C{a}}, B{C{b}} and B{C{c}} I{squared}). 

789 

790 @raise TriangleError: Invalid or negative B{C{a}}, B{C{b}} or B{C{c}}. 

791 ''' 

792 try: 

793 r, y, x = sorted(map1(float, a, b, c)) 

794 if r > 0: # r = min(a, b, c) 

795 ab = x - y 

796 bc = y - r 

797 y += r 

798 r = (x + y) * (r - ab) * (r + ab) * (x + bc) 

799 if r: 

800 r = sqrt(r / _16_0) 

801 elif r < 0: 

802 raise ValueError(_negative_) 

803 return r 

804 

805 except (TypeError, ValueError) as x: 

806 raise TriangleError(a=a, b=b, c=c, cause=x) 

807 

808 

809def triSide(a, b, radC): 

810 '''Compute one side of a triangle. 

811 

812 @arg a: Adjacent triangle side length (C{scalar}, 

813 non-negative C{meter}, conventionally). 

814 @arg b: Adjacent triangle side length (C{scalar}, 

815 non-negative C{meter}, conventionally). 

816 @arg radC: Angle included by sides B{C{a}} and B{C{b}}, 

817 opposite triangle side C{c} (C{radians}). 

818 

819 @return: Length of triangle side C{c}, opposite triangle 

820 corner C{C} and angle B{C{radC}}, same units as 

821 B{C{a}} and B{C{b}}. 

822 

823 @raise TriangleError: Invalid B{C{a}}, B{C{b}} or B{C{radC}}. 

824 

825 @see: Functions L{pygeodesy.sqrt_a}, L{pygeodesy.triAngle}, 

826 L{pygeodesy.triSide2} and L{pygeodesy.triSide4}. 

827 ''' 

828 try: 

829 return _triSide(a, b, radC) 

830 except (TypeError, ValueError) as x: 

831 raise TriangleError(a=a, b=b, radC=radC, cause=x) 

832 

833 

834def _triSide(a, b, radC): 

835 # (INTERNAL) To allow callers to embellish errors 

836 a, b, r = t = map1(float, a, b, radC) 

837 if min(t) < 0: 

838 raise ValueError(_negative_) 

839 

840 if a < b: 

841 a, b = b, a 

842 if a > EPS0: 

843 ba = b / a 

844 c2 = fsumf_(_1_0, ba**2, _N_2_0 * ba * cos(r)) 

845 if c2 > EPS02: 

846 return a * sqrt(c2) 

847 elif c2 < 0: 

848 raise ValueError(_invalid_) 

849 return hypot(a, b) 

850 

851 

852def triSide2(b, c, radB): 

853 '''Compute a side and its opposite angle of a triangle. 

854 

855 @arg b: Adjacent triangle side length (C{scalar}, 

856 non-negative C{meter}, conventionally). 

857 @arg c: Adjacent triangle side length (C{scalar}, 

858 non-negative C{meter}, conventionally). 

859 @arg radB: Angle included by sides B{C{a}} and B{C{c}}, 

860 opposite triangle side C{b} (C{radians}). 

861 

862 @return: L{TriSide2Tuple}C{(a, radA)} with triangle angle 

863 C{radA} in C{radians} and length of the opposite 

864 triangle side C{a}, same units as B{C{b}} and B{C{c}}. 

865 

866 @raise TriangleError: Invalid B{C{b}} or B{C{c}} or either 

867 B{C{b}} or B{C{radB}} near zero. 

868 

869 @see: Functions L{pygeodesy.sqrt_a}, L{pygeodesy.triSide} 

870 and L{pygeodesy.triSide4}. 

871 ''' 

872 try: 

873 return _triSide2(b, c, radB) 

874 except (TypeError, ValueError) as x: 

875 raise TriangleError(b=b, c=c, radB=radB, cause=x) 

876 

877 

878def _triSide2(b, c, radB): 

879 # (INTERNAL) To allow callers to embellish errors 

880 b, c, rB = map1(float, b, c, radB) 

881 if min(b, c, rB) < 0: 

882 raise ValueError(_negative_) 

883 sB, cB = sincos2(rB) 

884 if isnear0(sB): 

885 if not isnear0(b): 

886 raise ValueError(_invalid_) 

887 if cB < 0: 

888 a, rA = (b + c), PI 

889 else: 

890 a, rA = fabs(b - c), _0_0 

891 elif isnear0(b): 

892 raise ValueError(_invalid_) 

893 else: 

894 rA = fsumf_(PI, -rB, -asin1(c * sB / b)) 

895 a = sin(rA) * b / sB 

896 return TriSide2Tuple(a, rA, name=triSide2.__name__) 

897 

898 

899def triSide4(radA, radB, c): 

900 '''Compute two sides and the height of a triangle. 

901 

902 @arg radA: Angle at triangle corner C{A}, opposite triangle side C{a} 

903 (non-negative C{radians}). 

904 @arg radB: Angle at triangle corner C{B}, opposite triangle side C{b} 

905 (non-negative C{radians}). 

906 @arg c: Length of triangle side between triangle corners C{A} and C{B}, 

907 (C{scalar}, non-negative C{meter}, conventionally). 

908 

909 @return: L{TriSide4Tuple}C{(a, b, radC, d)} with triangle sides C{a} and 

910 C{b} and triangle height C{d} perpendicular to triangle side 

911 B{C{c}}, all in the same units as B{C{c}} and interior angle 

912 C{radC} in C{radians} at triangle corner C{C}, opposite 

913 triangle side B{C{c}}. 

914 

915 @raise TriangleError: Invalid or negative B{C{radA}}, B{C{radB}} or B{C{c}}. 

916 

917 @see: U{Triangulation, Surveying<https://WikiPedia.org/wiki/Triangulation_(surveying)>} 

918 and functions L{pygeodesy.sqrt_a}, L{pygeodesy.triSide} and L{pygeodesy.triSide2}. 

919 ''' 

920 try: 

921 rA, rB, c = map1(float, radA, radB, c) 

922 rC = fsumf_(PI, -rA, -rB) 

923 if min(rC, rA, rB, c) < 0: 

924 raise ValueError(_negative_) 

925 sa, ca, sb, cb = sincos2_(rA, rB) 

926 sc = fsum1f_(sa * cb, sb * ca) 

927 if sc < EPS0 or min(sa, sb) < 0: 

928 raise ValueError(_invalid_) 

929 sc = c / sc 

930 return TriSide4Tuple((sa * sc), (sb * sc), rC, (sa * sb * sc), 

931 name=triSide4.__name__) 

932 

933 except (TypeError, ValueError) as x: 

934 raise TriangleError(radA=radA, radB=radB, c=c, cause=x) 

935 

936 

937def wildberger3(a, b, c, alpha, beta, R3=min): 

938 '''Snellius' surveying using U{Rational Trigonometry 

939 <https://WikiPedia.org/wiki/Snellius–Pothenot_problem>}. 

940 

941 @arg a: Length of the triangle side between corners C{B} and C{C} and opposite of 

942 triangle corner C{A} (C{scalar}, non-negative C{meter}, conventionally). 

943 @arg b: Length of the triangle side between corners C{C} and C{A} and opposite of 

944 triangle corner C{B} (C{scalar}, non-negative C{meter}, conventionally). 

945 @arg c: Length of the triangle side between corners C{A} and C{B} and opposite of 

946 triangle corner C{C} (C{scalar}, non-negative C{meter}, conventionally). 

947 @arg alpha: Angle subtended by triangle side B{C{b}} (C{degrees}, non-negative). 

948 @arg beta: Angle subtended by triangle side B{C{a}} (C{degrees}, non-negative). 

949 @kwarg R3: Callable to determine C{R3} from C{(R3 - C)**2 = D}, typically standard 

950 Python function C{min} or C{max}, invoked with 2 arguments. 

951 

952 @return: L{Survey3Tuple}C{(PA, PB, PC)} with distance from survey point C{P} to 

953 each of the triangle corners C{A}, C{B} and C{C}, same units as B{C{a}}, 

954 B{C{b}} and B{C{c}}. 

955 

956 @raise TriangleError: Invalid B{C{a}}, B{C{b}} or B{C{c}} or negative B{C{alpha}} or 

957 B{C{beta}} or B{C{R3}} not C{callable}. 

958 

959 @see: U{Wildberger, Norman J.<https://Math.Sc.Chula.ac.TH/cjm/content/ 

960 survey-article-greek-geometry-rational-trigonometry-and-snellius-–-pothenot-surveying>}, 

961 U{Devine Proportions, page 252<http://www.MS.LT/derlius/WildbergerDivineProportions.pdf>} 

962 and function L{pygeodesy.snellius3}. 

963 ''' 

964 def _s(x): 

965 return sin(x)**2 

966 

967 def _vpa(r1, r3, q2, q3, s3): 

968 r = r1 * r3 * _4_0 

969 n = (r - Fsum(r1, r3, -q2).fpow(2)).fover(s3) 

970 if n < 0 or isnear0(r): 

971 raise ValueError(_coincident_) 

972 return sqrt((n / r) * q3) if n else _0_0 

973 

974 try: 

975 a, b, c, da, db = t = map1(float, a, b, c, alpha, beta) 

976 if min(t) < 0: 

977 raise ValueError(_negative_) 

978 

979 ra, rb = radians(da), radians(db) 

980 s1, s2, s3 = s = map1(_s, rb, ra, ra + rb) # rb, ra! 

981 if min(s) < EPS02: 

982 raise ValueError(_or(_coincident_, _colinear_)) 

983 

984 q1, q2, q3 = q = a**2, b**2, c**2 

985 if min(q) < EPS02: 

986 raise ValueError(_coincident_) 

987 

988 r1 = s2 * q3 / s3 # s2! 

989 r2 = s1 * q3 / s3 # s1! 

990 Qs = Fsum(*q) # == hypot2_(a, b, c) 

991 Ss = Fsum(*s) # == fsum1(s, floats=True) 

992 s += Qs * _0_5, # tuple! 

993 C0 = Fdot(s, q1, q2, q3, -Ss) 

994 r3 = C0.fover(-s3) 

995 d0 = Qs.fpow(2).fsub_(hypot2_(*q) * _2_0).fmul(s1 * s2).fover(s3) 

996 if d0 > EPS02: # > c0 

997 d0 = sqrt(d0) 

998 if not callable(R3): 

999 raise ValueError(_R3__ + _not_(callable.__name__)) 

1000 r3 = R3(float(C0 + d0), float(C0 - d0)) # XXX min or max 

1001 elif d0 < 0: 

1002 raise ValueError(_negative_) 

1003 

1004 pa = _vpa(r1, r3, q2, q3, s3) 

1005 pb = _vpa(r2, r3, q1, q3, s3) 

1006 pc = favg(_triSide2(b, pa, ra).a, 

1007 _triSide2(a, pb, rb).a) 

1008 return Survey3Tuple(pa, pb, pc, name=wildberger3.__name__) 

1009 

1010 except (TypeError, ValueError) as x: 

1011 raise TriangleError(a=a, b=b, c=c, alpha=alpha, beta=beta, R3=R3, cause=x) 

1012 

1013 

1014def _zidw(x, y, useZ, *ABC): 

1015 if useZ: # interpolate z or coplanar with A, B and C? 

1016 t = tuple(_.z for _ in ABC) 

1017 v = Vector3d(x, y, fmean(t)) 

1018 z = fidw(t, (v.minus(T).length for T in ABC)) 

1019 else: 

1020 z = INT0 

1021 return z 

1022 

1023# **) MIT License 

1024# 

1025# Copyright (C) 2016-2023 -- mrJean1 at Gmail -- All Rights Reserved. 

1026# 

1027# Permission is hereby granted, free of charge, to any person obtaining a 

1028# copy of this software and associated documentation files (the "Software"), 

1029# to deal in the Software without restriction, including without limitation 

1030# the rights to use, copy, modify, merge, publish, distribute, sublicense, 

1031# and/or sell copies of the Software, and to permit persons to whom the 

1032# Software is furnished to do so, subject to the following conditions: 

1033# 

1034# The above copyright notice and this permission notice shall be included 

1035# in all copies or substantial portions of the Software. 

1036# 

1037# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 

1038# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 

1039# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 

1040# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR 

1041# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 

1042# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 

1043# OTHER DEALINGS IN THE SOFTWARE.