From cb2c2c5aaa4b98fe3953f263e726621848ae545a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jul 2009 22:02:17 +0200 Subject: * digital/beacon/triangle/doc (re #73): - added python script for solving methods comparison. --- digital/beacon/triangle/doc/triangle.py | 158 ++++++++++++++++++++++++++++++++ 1 file changed, 158 insertions(+) create mode 100644 digital/beacon/triangle/doc/triangle.py (limited to 'digital/beacon/triangle/doc/triangle.py') diff --git a/digital/beacon/triangle/doc/triangle.py b/digital/beacon/triangle/doc/triangle.py new file mode 100644 index 00000000..9390caa0 --- /dev/null +++ b/digital/beacon/triangle/doc/triangle.py @@ -0,0 +1,158 @@ +# Triangle - Triangulation beacon system study. {{{ +# +# Copyright (C) 2009 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT org +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# }}} +from math import * +from numpy import * +import Gnuplot, Gnuplot.funcutils + +B1 = array ([3000, 0]) +B2 = array ([3000, 2100]) +B3 = array ([0, 2100 / 2]) + +def norm (v): + """Compute vector norm.""" + return sqrt (sum (v ** 2)) + +def angle (o, a, b): + """Compute angle (v_oa, v_ob).""" + v_oa = a - o + v_ob = b - o + # Use dot product to compute angle. + angle = acos (dot (v_oa, v_ob) / (norm (v_oa) * norm (v_ob))) + # Use cross product to determine angle sign. + if v_oa[0] * v_ob[1] - v_ob[0] * v_oa[1] >= 0: + return angle + else: + return 2 * pi - angle + +theta1 = angle (B1, B2, B3) +theta2 = angle (B2, B3, B1) +theta3 = angle (B3, B1, B2) +b1 = norm (B1 - B2) +b2 = norm (B2 - B3) +b3 = norm (B3 - B1) + +def solve1 (alpha1, alpha2, alpha3): + """Solve using beta1.""" + beta1 = atan ( + (-b1 + b3 * sin (alpha3 + theta2) / sin (alpha3)) + / (b1 * cos (alpha1) / sin (alpha1) + + b3 * cos (alpha3 + theta1) / sin (alpha3)) + ) + a1 = sin (pi - beta1 - alpha1) * b1 / sin (alpha1) + o = array ([B1[0] - sin (beta1) * a1, B1[1] + cos (beta1) * a1]) + return (o, beta1, a1) + +def solve2 (alpha1, alpha2, alpha3): + """Solve using beta2 (rotation of solve1).""" + beta2 = atan ( + (-b2 + b1 * sin (alpha1 + theta3) / sin (alpha1)) + / (b2 * cos (alpha2) / sin (alpha2) + + b1 * cos (alpha1 + theta2) / sin (alpha1)) + ) + a2 = sin (pi - beta2 - alpha2) * b2 / sin (alpha2) + o = array ([B2[0] - sin (theta2 - beta2) * a2, B2[1] - cos (theta2 - beta2) * a2]) + return (o, beta2, a2) + +def solve2m (alpha1, alpha2, alpha3): + """Solve using gamma1 (mirror of solve1).""" + gamma1 = atan ( + (-b1 + b2 * sin (alpha2 + theta1) / sin (alpha2)) + / (b1 * cos (alpha1) / sin (alpha1) + + b2 * cos (alpha2 + theta2) / sin (alpha2)) + ) + a2 = sin (pi - gamma1 - alpha1) * b1 / sin (alpha1) + o = array ([B2[0] - sin (gamma1) * a2, B2[1] - cos (gamma1) * a2]) + return (o, gamma1, a2) + +def solve3 (alpha1, alpha2, alpha3): + """Solve using beta3 (rotation of solve1).""" + beta3 = atan ( + (-b3 + b2 * sin (alpha2 + theta1) / sin (alpha2)) + / (b3 * cos (alpha3) / sin (alpha3) + + b2 * cos (alpha2 + theta3) / sin (alpha2)) + ) + a3 = sin (pi - beta3 - alpha3) * b3 / sin (alpha3) + o = array ([B3[0] - cos (beta3 - theta3 / 2) * a3, B3[1] + sin (beta3 - theta3 / 2) * a3]) + return (o, beta3, a3) + +def trace (o, f, output, factor): + """Trace f()[output] * factor for alphan corresponding to the point o.""" + # Compute angles. + alpha1 = angle (o, B1, B2) + alpha2 = angle (o, B2, B3) + alpha3 = angle (o, B3, B1) + # Return. + return f (alpha1, alpha2, alpha3)[output] * factor + +def compute_prec (o, f, prec): + """Return an aproximation of distance error with the given angle error.""" + # Compute angles. + alpha1 = angle (o, B1, B2) + alpha2 = angle (o, B2, B3) + alpha3 = angle (o, B3, B1) + # Evaluate error. + e = 0 + for i in ((-1, -1, -1), (-1, -1, 1), (-1, 1, -1), (-1, 1, 1), + (1, -1, -1), (1, -1, 1), (1, 1, -1), (1, 1, 1)): + o2 = f (alpha1 + i[0] * prec, alpha2 + i[1] * prec, alpha3 + i[2] * prec)[0] + e = max (norm (o - o2), e) + return e + +if __name__ == '__main__': + # Parameters. + method = solve1 + prec = 0.5 * pi / 180 + plot = 'prec' + style = 'iso' + hardcopy = None + # Setup gnuplot. + g = Gnuplot.Gnuplot (persist = True) + g ('set term x11') + g ('set data style lines') + if style == '3d': + pass + else: + g ('set view map') + g ('set nosurface') + if style == 'map': + g ('set pm3d') + elif style == 'iso': + g ('set contour') + g.set_range ('xrange', (0,3000)) + g.set_range ('yrange', (0,2100)) + x = arange (25, 3000, 50) + y = arange (25, 2100, 50) + # Plot: + if plot == 'angle': + g ('set cntrparam levels incremental 0, 5, 360') + g.splot (Gnuplot.funcutils.compute_GridData (x, y, + lambda x, y: trace (array ([x, y]), method, 1, 180 / pi), binary=0)) + elif plot == 'prec': + g ('set cntrparam levels discrete 5, 10, 20, 30, 40, 50, 100, 150, 200, 500') + g ('set cbrange [0:100]') + g.splot (Gnuplot.funcutils.compute_GridData (x, y, + lambda x, y: compute_prec (array ([x, y]), method, prec), binary=0)) + # Hardcopy: + if hardcopy: + g.hardcopy (filename = hardcopy, terminal = 'png') -- cgit v1.2.3