summaryrefslogtreecommitdiff
path: root/digital/beacon/others/triangle_beacon/doc/triangle.py
blob: 828fe277b3d929679fa74d1a48728575df33f3b8 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
# Triangle - Triangulation beacon system study. {{{
#
# Copyright (C) 2010 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

def solve (Ba, Bb, alphaa, alphab):
    """Solve using angles."""
    l = norm (Ba - Bb)
    d = l * sin (alphaa) * sin (alphab) / sin (alphaa + alphab)
    b = d / tan (alphaa)
    v_n = Bb - Ba
    v_n = v_n / norm (v_n)
    o = Ba + v_n * b + rotate (v_n) * d
    return o

def trace (Ba, Bb, o, f, output, factor):
    """Trace f()[output] * factor for alphan corresponding to the point o."""
    # Compute angles.
    alphaa = angle (Ba, Bb, o)
    alphab = angle (Bb, o, Ba)
    # Return.
    return f (Ba, Bb, alphaa, alphab)[output] * factor

def compute_prec (Ba, Bb, o, f, prec):
    """Return an aproximation of distance error with the given angle error."""
    # Compute angles.
    alphaa = angle (Ba, Bb, o)
    alphab = angle (Bb, o, Ba)
    # Evaluate error.
    e = 0
    for i in ((-1, -1), (-1, 1), (1, -1), (1, 1)):
        o2 = f (Ba, Bb, alphaa + i[0] * prec, alphab + i[1] * prec)
        e = max (norm (o - o2), e)
    return e

if __name__ == '__main__':
    # Parameters.
    Ba = B1
    Bb = B2
    method = solve
    prec = 2 * pi / 720
    plot = 'prec'
    style = 'map'
    hardcopy = None
    zrange = None
    # Setup gnuplot.
    persist = False
    g = Gnuplot.Gnuplot (persist = persist)
    g ('set term x11')
    g ('set style data 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 == '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 (Ba, Bb, array ([x, y]), method, prec), binary=0))
    else:
        g.splot (Gnuplot.funcutils.compute_GridData (x, y,
            lambda x, y: trace (Ba, Bb, array ([x, y]), method, plot, 1), binary=0))
    # Hardcopy:
    if hardcopy:
        g.hardcopy (filename = hardcopy, terminal = 'png')
    if not persist:
        raw_input ("Pause...")