From 8ac7f52728a8ec2a09a6eeebc3cf5d4afff9b485 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Tue, 13 Mar 2012 22:50:23 +0100 Subject: digital/beacon, digital/lol: beacon folders reorganisation --- .../beacon/others/triangle_robot/doc/triangle.plot | 80 ++ .../beacon/others/triangle_robot/doc/triangle.py | 186 +++++ .../others/triangle_robot/doc/triangle_drawing.svg | 877 +++++++++++++++++++++ 3 files changed, 1143 insertions(+) create mode 100644 digital/beacon/others/triangle_robot/doc/triangle.plot create mode 100644 digital/beacon/others/triangle_robot/doc/triangle.py create mode 100644 digital/beacon/others/triangle_robot/doc/triangle_drawing.svg (limited to 'digital/beacon/others/triangle_robot') diff --git a/digital/beacon/others/triangle_robot/doc/triangle.plot b/digital/beacon/others/triangle_robot/doc/triangle.plot new file mode 100644 index 00000000..8a104b9b --- /dev/null +++ b/digital/beacon/others/triangle_robot/doc/triangle.plot @@ -0,0 +1,80 @@ +set angles degrees +set xrange [0:3000] +set yrange [0:2100] +dist (x0, y0, x1, y1) = sqrt ((x0 - x1) ** 2 + (y0 - y1) ** 2) +dot (x0, y0, x1, y1) = x0 * x1 + y0 * y1 +anglea (x, y, x0, y0, x1, y1) = acos (dot (x0 - x, y0 - y, x1 - x, y1 - y) / (dist (x, y, x0, y0) * dist (x, y, x1, y1))) +angle (x, y, x0, y0, x1, y1) = ((x0 - x) * (y1 - y) - (x1 - x) * (y0 - y) > 0 ? anglea (x, y, x0, y0, x1, y1) : 360 - anglea (x, y, x0, y0, x1, y1)) +angle_b12 (x, y) = angle (x, y, 3000, 0, 3000, 2100) +angle_b23 (x, y) = angle (x, y, 3000, 0, 0, 1050) +angle_b31 (x, y) = angle (x, y, 3000, 2100, 0, 1050) + +t1 = angle (3000, 0, 3000, 2100, 0, 1050) +t2 = angle (3000, 2100, 0, 1050, 3000, 0) +t3 = angle (0, 1050, 3000, 0, 3000, 2100) +b1 = dist (3000, 0, 3000, 2100) +b2 = dist (3000, 2100, 0, 1050) +b3 = dist (0, 1050, 3000, 0) + +solve_angle (a1, a3) = atan ((-b1 + b3 * sin (a3 + t2) / sin (a3)) / (b1 * cos (a1) / sin (a1) + b3 * cos (a3 + t1) / sin (a3))) + +set isosamples 50 +set contour +set cntrparam levels incremental 0, 10, 360 + +set term png +set output 'triangle_surf12.png' +splot angle_b12 (x, y) +set output 'triangle_surf23.png' +splot angle_b23 (x, y) +set output 'triangle_surf31.png' +splot angle_b31 (x, y) + +set contour +set cntrparam levels incremental 0, 1, 360 +unset surface +set view map +unset clabel + +set output 'triangle_contour12.png' +splot angle_b12 (x, y) +set output 'triangle_contour23.png' +splot angle_b23 (x, y) +set output 'triangle_contour31.png' +splot angle_b31 (x, y) +set output 'triangle_contour123.png' +splot angle_b12 (x, y), angle_b23 (x, y), angle_b31 (x, y) +set cntrparam levels incremental 0, 3, 360 +set output 'triangle_contour123-3deg.png' +splot angle_b12 (x, y), angle_b23 (x, y), angle_b31 (x, y) +set cntrparam levels incremental 0, 1, 360 + +set size square +set xrange [0:100] +set yrange [0:100] +set output 'triangle_zoom+0+0.png' +splot angle_b12 (x, y), angle_b23 (x, y), angle_b31 (x, y) +set xrange [0:100] +set yrange [1000:1100] +set output 'triangle_zoom+0+1050.png' +splot angle_b12 (x, y), angle_b23 (x, y), angle_b31 (x, y) +set xrange [2900:3000] +set yrange [2000:2100] +set output 'triangle_zoom+3000+2100.png' +splot angle_b12 (x, y), angle_b23 (x, y), angle_b31 (x, y) +set xrange [1450:1550] +set yrange [1000:1100] +set output 'triangle_zoom+1500+1050.png' +splot angle_b12 (x, y), angle_b23 (x, y), angle_b31 (x, y) + +reset +set xrange [0:3000] +set yrange [0:2100] +set isosamples 50 +set angles degrees +set contour +set cntrparam levels incremental 0, 5, 360 + +set term png +set output 'triangle_solve_angle.png' +splot solve_angle (angle_b12 (x, y), angle_b31 (x, y)) diff --git a/digital/beacon/others/triangle_robot/doc/triangle.py b/digital/beacon/others/triangle_robot/doc/triangle.py new file mode 100644 index 00000000..0471a417 --- /dev/null +++ b/digital/beacon/others/triangle_robot/doc/triangle.py @@ -0,0 +1,186 @@ +# 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 rotate (v): + """Rotate a vector by pi/2.""" + return array ([-v[1], v[0]]) + +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 solve_cvra (alpha1, alpha2, alpha3): + """Solve using CVRA method, slightly modified for simpler vector based + computations.""" + # Find circle on which B1, B2 and o are located. + v = (B2 - B1) / 2 + cc2 = B1 + v + rotate (v) / tan (alpha1) + # Find circle on which B1, B3 and o are located. + v = (B3 - B1) / 2 + cc1 = B1 + v - rotate (v) / tan (alpha3) + # Find the circles intersection... knowing that B1 is a solution. + v = cc2 - B1 + n = (cc1 - cc2) / norm (cc1 - cc2) + o = B1 + (v + n * dot (-v, n)) * 2 + return (o, cc2[0], cc2[1], cc1[0], cc1[1]) + +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 = solve_cvra + prec = 2 * pi / 2200 + plot = 'prec' + style = '3d' + hardcopy = None + zrange = (0, 150) + # Setup gnuplot. + persist = False + g = Gnuplot.Gnuplot (persist = persist) + 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)) + if zrange: + g.set_range ('zrange', zrange) + 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)) + else: + g.splot (Gnuplot.funcutils.compute_GridData (x, y, + lambda x, y: trace (array ([x, y]), method, plot, 1), binary=0)) + # Hardcopy: + if hardcopy: + g.hardcopy (filename = hardcopy, terminal = 'png') + if not persist: + raw_input ("Pause...") diff --git a/digital/beacon/others/triangle_robot/doc/triangle_drawing.svg b/digital/beacon/others/triangle_robot/doc/triangle_drawing.svg new file mode 100644 index 00000000..32ea9e5b --- /dev/null +++ b/digital/beacon/others/triangle_robot/doc/triangle_drawing.svg @@ -0,0 +1,877 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + α + 1 + + + α + 2 + + + α + 3 + + B1 + B2 + B3 + + x + y + + + + + b + 2 + + + b + 3 + + + b + 1 + + + + + + γ + 1 + + + β + 2 + + + θ + 2 + + + + + + γ + 3 + + + β + 1 + + + θ + 1 + + + + + + γ + 2 + + + β + 3 + + + θ + 3 + + + a + 3 + + + a + 2 + + + a + 1 + + + + -- cgit v1.2.3