1

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

3

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

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

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

7

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

13 from pygeodesy.basics import map1, _ALL_LAZY

14 from pygeodesy.constants import EPS, EPS0, EPS02, INT0, PI, PI2, PI_2, PI_4, isnear0, \

15 _0_0, _0_5, _1_0, _N_1_0, _2_0, _N_2_0, _4_0, _360_0

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

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

18 from pygeodesy.fsums import Fsum, fsum_, fsum1, fsum1_

19 from pygeodesy.interns import _a_, _A_, _b_, _B_, _c_, _C_, _coincident_, _colinear_, \

20 _d_, _invalid_, _negative_, _not_, _rIn_, _SPACE_

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

22 from pygeodesy.named import Fmt, _NamedTuple, _Pass

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

24 from pygeodesy.units import Degrees, Distance, Radians

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

26 from pygeodesy.vector3d import _otherV3d, Vector3d

27

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

29

30__all__ = _ALL_LAZY.resections

31__version__ = '23.03.19'

32

33_concyclic_ = 'concyclic'

34_PA_ = 'PA'

35_PB_ = 'PB'

36_PC_ = 'PC'

37_pointH_ = 'pointH'

38_pointP_ = 'pointP'

39_R3__ = 'R3 '

40_radA_ = 'radA'

41_radB_ = 'radB'

42_radC_ = 'radC'

43

44

45 class Collins5Tuple(_NamedTuple):

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

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

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

49 '''

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

51 _Units_ = (_Pass, _Pass, Distance, Distance, Distance)

52

53

54 class ResectionError(_ValueError):

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

56 '''

57 pass

58

59

60 class Survey3Tuple(_NamedTuple):

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

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

63 '''

64 _Names_ = (_PA_, _PB_, _PC_)

65 _Units_ = ( Distance, Distance, Distance)

66

67

68 class Tienstra7Tuple(_NamedTuple):

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

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

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

72 '''

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

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

75

76

77 class TriAngle4Tuple(_NamedTuple):

78 '''4-Tuple C{(radA, radB, radC, rIn)} with the interior angles at triangle

79 corners C{A}, C{B} and C{C} in C{radians} and the C{InCircle} radius

80 C{rIn} aka C{inradius} in C{meter}, conventionally.

81 '''

82 _Names_ = (_radA_, _radB_, _radC_, _rIn_)

83 _Units_ = ( Radians, Radians, Radians, Distance)

84

85

86 class TriSide2Tuple(_NamedTuple):

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

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

89 '''

90 _Names_ = (_a_, _radA_)

91 _Units_ = ( Distance, Radians)

92

93

94 class TriSide4Tuple(_NamedTuple):

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

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

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

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

99 '''

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

101 _Units_ = ( Distance, Distance, Radians, Distance)

102

103

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

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

106

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

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

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

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

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

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

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

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

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

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

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

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

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

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

121 @kwarg Clas_kwds: Optional additional keyword argument for the survey

122 point instance.

123

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

125

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

127 None} of B{C{pointA}}'s (sub-)class.

128

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

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

131

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

133

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

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

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

137 L{pygeodesy.tienstra7}.

138 '''

139

140 def _H(A, C, sa):

141 s, c = sincos2d(sa)

142 if isnear0(s):

143 raise ValueError(_or(_coincident_, _colinear_))

144 t = s, c, c

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

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

147 return Vector3d(x, y, 0)

148

149 A = _otherV3d(useZ=useZ, pointA=pointA)

150 B = _otherV3d(useZ=useZ, pointB=pointB)

151 C = _otherV3d(useZ=useZ, pointC=pointC)

152

153 try:

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

155 if min(sa, sb) < 0:

156 raise ValueError(_negative_)

157 if fsum_(_360_0, -sa, -sb) < EPS0:

158 raise ValueError()

159

160 H1 = _H(A, C, sa)

161 H2 = _H(B, C, -sb)

162

163 x = H1.x - H2.x

164 y = H1.y - H2.y

165 # x, y, _ = H1.minus(H2).xyz

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

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

168

169 m = y / x

170 n = x / y

171 N = n + m

172 if isnear0(N):

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

174

175 t = n, m, _1_0, _N_1_0

176 x = Fdot(t, C.x, H1.x, C.y, H1.y).fover(N)

177 y = Fdot(t, H1.y, C.y, C.x, H1.x).fover(N)

178 z = _zidw(A, B, C, x, y) if useZ else INT0

179

180 clas = Clas or pointA.classof

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

182

183 except (TypeError, ValueError) as x:

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

185 alpha=alpha, beta=beta, cause=x)

186

187

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

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

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

191

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

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

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

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

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

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

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

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

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

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

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

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

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

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

206 @kwarg Clas_kwds: Optional additional keyword argument for the survey

207 and auxiliary point instance.

208

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

210

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

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

213 is None} of B{C{pointA}}'s (sub-)class and triangle sides C{a},

214 C{b} and C{c} in C{meter}, conventionally.

215

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

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

218

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

220

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

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

223 L{pygeodesy.tienstra7}.

224 '''

225

226 def _azi_len2(A, B, pi2):

227 v = B.minus(A)

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

229 if pi2 and r < 0:

230 r += pi2

231 return r, v.length

232

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

234 s, c = sincos2(r)

235 x = A.x + d * s

236 y = A.y + d * c

237 z = _zidw(A, B, C, x, y) if useZ else INT0

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

239

240 A = _otherV3d(useZ=useZ, pointA=pointA)

241 B = _otherV3d(useZ=useZ, pointB=pointB)

242 C = _otherV3d(useZ=useZ, pointC=pointC)

243

244 try:

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

246 if min(ra, rb) < 0:

247 raise ValueError(_negative_)

248

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

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

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

252

253 clas = Clas or pointA.classof

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

255

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

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

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

259

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

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

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

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

264

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

266

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

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

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

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

271

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

273 a = B.minus(C).length

274

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

276

277 except (TypeError, ValueError) as x:

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

279 alpha=alpha, beta=beta, cause=x)

280

281

282 def pierlot(point1, point2, point3, alpha12, alpha23, useZ=False, Clas=None, **Clas_kwds):

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

284 pierlot/Pierlot2014ANewThree>}'s method C{ToTal}.

285

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

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

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

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

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

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

292 @arg alpha12: Angle subtended from B{C{point1}} to B{C{point2}} (C{degrees}).

293 @arg alpha23: Angle subtended from B{C{point2}} to B{C{point3}} (C{degrees}).

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

295 (C{bool}).

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

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

298 @kwarg Clas_kwds: Optional additional keyword arguments for the survey

299 point instance.

300

301 @note: Points B{C{point1}}, B{C{point2}} and B{C{point3}} are ordered

302 counter-clockwise, typically.

303

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

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

306

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

308 or invalid B{C{alpha12}} or B{C{alpha23}}.

309

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

311

312 @see: U{V. Pierlot, M. Van Droogenbroeck, "A New Three Object Triangulation

313 Algorithm for Mobile Robot Positioning"<https://ORBi.ULiege.Be/

314 bitstream/2268/157469/1/Pierlot2014ANewThree.pdf>}, U{Vincent Pierlot,

315 Marc Van Droogenbroeck, "18 Triangulation Algorithms for 2D Positioning

316 (also known as the Resection Problem)"<http://www.Telecom.ULg.ac.Be/

317 triangulation>} and functions L{pygeodesy.cassini}, L{pygeodesy.collins5}

318 and L{pygeodesy.tienstra7}.

319 '''

320 B1 = _otherV3d(useZ=useZ, point1=point1)

321 B2 = _otherV3d(useZ=useZ, point2=point2)

322 B3 = _otherV3d(useZ=useZ, point3=point3)

323

324 try: # (INTERNAL) Raises error for (pseudo-)singularities

325 s12, c12, s23, c23 = sincos2d_(alpha12, alpha23)

326 if isnear0(s12) or isnear0(s23):

327 raise ValueError(_or(_coincident_, _colinear_))

328 cot12 = c12 / s12

329 cot23 = c23 / s23

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

331 d = fsum1_(c12 * s23, s12 * c23)

332 if isnear0(d):

333 raise ValueError(_or(_colinear_, _coincident_))

334 cot31 = Fsum(_1_0, s12 * s23, -c12 * c23, _N_1_0).fover(d)

335

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

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

338

339# x23 = x3_ - cot23 * y3_

340# y23 = y3_ + cot23 * x3_

341

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

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

344

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

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

347

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

349 if isnear0(d):

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

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

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

353

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

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

356 z = _zidw(B1, B2, B3, x, y) if useZ else INT0

357

358 clas = Clas or point1.classof

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

360

361 except (TypeError, ValueError) as x:

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

363 alpha12=alpha12, alpha23=alpha23, cause=x)

364

365

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

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

368

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

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

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

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

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

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

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

376

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

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

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

380

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

382 or B{C{beta}}.

383

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

385 '''

386 try:

387 a, b, degC, alpha, beta = map1(float, a, b, degC, alpha, beta)

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

389 if min(ra, rb, rC, a, b) < 0:

390 raise ValueError(_negative_)

391

392 r = fsum_(ra, rb, rC) * _0_5

393 k = PI - r

394 if min(k, r) < 0:

395 raise ValueError(_or(_coincident_, _colinear_))

396

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

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

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

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

401 x = k + w

402 y = k - w

403

404 s = fabs(sa)

405 if fabs(sb) > s:

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

407 elif s:

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

409 else:

410 raise ValueError(_or(_colinear_, _coincident_))

411

412 pa = _triSide(b, pc, fsum_(PI, -ra, -x))

413 pb = _triSide(a, pc, fsum_(PI, -rb, -y))

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

415

416 except (TypeError, ValueError) as x:

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

418

419

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

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

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

423

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

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

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

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

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

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

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

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

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

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

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

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

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

437 (C{bool}).

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

439 (sub-)class.

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

441

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

443

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

445 instance of B{C{Clas}} or if C{B{Clas} is None} of B{C{pointA}}'s (sub-)class,

446 with triangle angles C{A} at B{C{pointA}}, C{B} at B{C{pointB}} and C{C} at

447 B{C{pointC}} in C{degrees} and with triangle sides C{a}, C{b} and C{c} in

448 C{meter}, conventionally.

449

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

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

452 negative B{C{alpha}}, B{C{beta}} or B{C{gamma}}.

453

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

455

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

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

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

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

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

461 '''

462

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

464 if isnear0(fsum_(PI, r, -s)): # r + (PI2 - s) == PI

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

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

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

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

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

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

471 c = cr * ss - cs * sr

472 if isnear0(c):

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

474 ks.append(sr * ss / c)

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

476

477 A = _otherV3d(useZ=useZ, pointA=pointA)

478 B = _otherV3d(useZ=useZ, pointB=pointB)

479 C = _otherV3d(useZ=useZ, pointC=pointC)

480

481 try:

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

483 if beta is None:

484 if gamma is None:

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

486 sb = fsum_(PI2, -sa, -sc)

487 elif gamma is None:

488 sc = fsum_(PI2, -sa, -sb)

489 else: # subtended angles must add to 360 degrees

490 r = fsum1_(sa, sb, sc)

491 if fabs(r - PI2) > EPS:

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

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

494 raise ValueError(_negative_)

495

496 # triangle sides

497 a = B.minus(C).length

498 b = A.minus(C).length

499 c = A.minus(B).length

500

501 ks = [] # 3 Ks and triangle angles

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

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

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

505

506 k = fsum1(ks, floats=True)

507 if isnear0(k):

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

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

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

511 z = _zidw(A, B, C, x, y) if useZ else INT0

512

513 n = tienstra7.__name__

514 clas = Clas or pointA.classof

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

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

517

518 except (TypeError, ValueError) as x:

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

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

521

522

523 def triAngle(a, b, c):

524 '''Compute one angle of a triangle.

525

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

527 C{meter}, conventionally).

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

529 C{meter}, conventionally).

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

531 C{meter}, conventionally).

532

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

534 triangle side B{C{c}}.

535

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

537

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

539 '''

540 try:

541 return _triAngle(a, b, c)

542 except (TypeError, ValueError) as x:

543 raise TriangleError(a=a, b=b, c=c, tx=str(x), cause=x)

544

545

546 def _triAngle(a, b, c):

547 # (INTERNAL) To allow callers to embellish errors

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

549 if a < b:

550 a, b = b, a

551 if b < 0 or c < 0:

552 raise ValueError(_negative_)

553 if a < EPS0:

554 raise ValueError(_coincident_)

555 b_a = b / a

556 if b_a < EPS0:

557 raise ValueError(_coincident_)

558 return acos1(fsum_(_1_0, b_a**2, -(c / a)**2) / (b_a * _2_0))

559

560

561 def triAngle4(a, b, c):

562 '''Compute the angles of a triangle.

563

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

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

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

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

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

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

570

571 @return: L{TriAngle4Tuple}C{(radA, radB, radC, rIn)} with angles C{radA},

572 C{radB} and C{radC} at triangle corners C{A}, C{B} and C{C}, all

573 in C{radians} and the C{InCircle} radius C{rIn} aka C{inradius},

574 same units as triangle sides B{C{a}}, B{C{b}} and B{C{c}}.

575

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

577

578 @see: Function L{pygeodesy.triAngle}.

579 '''

580 try:

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

582 ab = a < b

583 if ab:

584 a, b = b, a

585 bc = b < c

586 if bc:

587 b, c = c, b

588

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

590 s = float(Fsum(a, b, c) * _0_5)

591 if s < EPS0:

592 raise ValueError(_negative_)

593 sa, sb, sc = (s - a), (s - b), (s - c)

594 r = sa * sb * sc / s

595 if r < EPS02:

596 raise ValueError(_coincident_)

597 r = sqrt(r)

598 rA = atan2(r, sa) * _2_0

599 rB = atan2(r, sb) * _2_0

600 rC = fsum_(PI, -rA, -rB)

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

602 raise ValueError(_colinear_)

603 elif c < 0:

604 raise ValueError(_negative_)

605 else: # 0 <= c <= EPS0

606 rA = rB = PI_2

607 rC = r = _0_0

608

609 if bc:

610 rB, rC = rC, rB

611 if ab:

612 rA, rB = rB, rA

613 return TriAngle4Tuple(rA, rB, rC, r, name=triAngle4.__name__)

614

615 except (TypeError, ValueError) as x:

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

617

618

619 def triSide(a, b, radC):

620 '''Compute one side of a triangle.

621

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

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

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

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

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

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

628

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

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

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

632

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

634

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

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

637 '''

638 try:

639 return _triSide(a, b, radC)

640 except (TypeError, ValueError) as x:

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

642

643

644 def _triSide(a, b, radC):

645 # (INTERNAL) To allow callers to embellish errors

646 a, b, r = map1(float, a, b, radC)

647 if min(a, b, r) < 0:

648 raise ValueError(_negative_)

649

650 if a < b:

651 a, b = b, a

652 if a > EPS0:

653 ba = b / a

654 c2 = fsum_(_1_0, ba**2, _N_2_0 * ba * cos(r))

655 if c2 > EPS02:

656 return a * sqrt(c2)

657 elif c2 < 0:

658 raise ValueError(_invalid_)

659 return hypot(a, b)

660

661

662 def triSide2(b, c, radB):

663 '''Compute one side and the opposite angle of a triangle.

664

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

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

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

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

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

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

671

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

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

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

675

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

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

678

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

680 and L{pygeodesy.triSide4}.

681 '''

682 try:

683 return _triSide2(b, c, radB)

684 except (TypeError, ValueError) as x:

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

686

687

688 def _triSide2(b, c, radB):

689 # (INTERNAL) To allow callers to embellish errors

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

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

692 raise ValueError(_negative_)

693 sB, cB = sincos2(rB)

694 if isnear0(sB):

695 if not isnear0(b):

696 raise ValueError(_invalid_)

697 if cB < 0:

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

699 else:

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

701 elif isnear0(b):

702 raise ValueError(_invalid_)

703 else:

704 rA = fsum_(PI, -rB, -asin1(c * sB / b))

705 a = sin(rA) * b / sB

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

707

708

709 def triSide4(radA, radB, c):

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

711

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

713 (non-negative C{radians}).

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

715 (non-negative C{radians}).

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

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

718

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

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

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

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

723 triangle side B{C{c}}.

724

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

726

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

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

729 '''

730 try:

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

732 rC = fsum_(PI, -rA, -rB)

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

734 raise ValueError(_negative_)

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

736 sc = fsum1_(sa * cb, sb * ca)

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

738 raise ValueError(_invalid_)

739 sc = c / sc

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

741 name=triSide4.__name__)

742

743 except (TypeError, ValueError) as x:

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

745

746

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

748 '''Snellius' surveying using U{Rational Trigonometry<https://WikiPedia.org/wiki/Snellius–Pothenot_problem>}.

749

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

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

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

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

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

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

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

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

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

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

760

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

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

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

764

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

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

767

768 @see: U{Wildberger, Norman J.<https://math.sc.Chula.ac.TH/cjm/content/

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

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

771 and function L{pygeodesy.snellius3}.

772 '''

773 def _s(x):

774 return sin(x)**2

775

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

777 r = r1 * r3 * _4_0

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

779 if n < 0 or isnear0(r):

780 raise ValueError(_coincident_)

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

782

783 try:

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

785 if min(a, b, c, da, db) < 0:

786 raise ValueError(_negative_)

787

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

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

790 if min(s) < EPS02:

791 raise ValueError(_or(_coincident_, _colinear_))

792

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

794 if min(q) < EPS02:

795 raise ValueError(_coincident_)

796

797 r1 = s2 * q3 / s3 # s2!

798 r2 = s1 * q3 / s3 # s1!

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

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

801 s += Qs * _0_5, # tuple!

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

803 r3 = C0.fover(-s3)

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

805 if d0 > EPS02: # > c0

806 d0 = sqrt(d0)

807 if not callable(R3):

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

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

810 elif d0 < 0:

811 raise ValueError(_negative_)

812

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

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

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

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

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

818

819 except (TypeError, ValueError) as x:

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

821

822

823 def _zidw(A, B, C, x, y):

824 # interpolate z or coplanar with A, B and C?

825 t = A.z, B.z, C.z

826 m = Vector3d(x, y, fmean(t)).minus

827 return fidw(t, (m(T).length for T in (A, B, C)))

828

829# **) MIT License

830#

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

832#

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

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

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

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

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

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

839#

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

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

842#

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

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

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

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

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

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

849# OTHER DEALINGS IN THE SOFTWARE.