summaryrefslogtreecommitdiff
path: root/digital/beacon/simu/position.c
blob: 5bc4a8f83c402f026a8dc00518c6699cf8aadb16 (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
/* position.c */
/* Beacon triangulation algorithms. {{{
 *
 * Copyright (C) 2011 Florent Duchon
 *
 * 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.
 *
 * }}} */

#include <stdio.h>
#include "position.h"

extern beacon_value_s beacon;
extern opponent_s opponent;

void init_struct(void)
{
	beacon.angle[0]=0;
	beacon.angle[1]=0;
	beacon.angle[2]=0;
	beacon.angle[3]=0;
	beacon.last_updated_beacon = 0;
	beacon.before_last_updated_beacon = 0;
	opponent.x = 0;
	opponent.y = 0;
}

float degree_to_radian(int value)
{
	float temp = 0;
	temp = (value * 2 * M_PI) / 360;
	return temp;
}

void update_position(int beacon_number,double angle)
{
	int which_formula = 0;	
	if(beacon_number == beacon.last_updated_beacon)
	{
		beacon.last_updated_beacon = beacon_number;
	}
	else
	{
		beacon.before_last_updated_beacon = beacon.last_updated_beacon;
		beacon.last_updated_beacon = beacon_number;
	}
	which_formula = beacon.before_last_updated_beacon + beacon.last_updated_beacon;
// 	printf("[position.c] => Update_position beacon_number = %d   angle = %f\n",(int)beacon_number,(double)angle);
	beacon.angle[beacon_number] = angle;

	switch(which_formula)
	{
		case 3:
//  			printf("[position.c] => Formula 3\r\n");
// 			printf("[position.c] => angle[1] = %f angle[2] = %f\n",beacon.angle[1],beacon.angle[2]);
  			opponent.x = LARGEUR_TABLE * tan(beacon.angle[2]) * tan(beacon.angle[1]) / (tan(beacon.angle[2])+tan(beacon.angle[1]));
  			opponent.y = LARGEUR_TABLE * tan(beacon.angle[1]) / (tan(beacon.angle[2])+tan(beacon.angle[1]));
			break;
		case 4:
//  			printf("[position.c] => Formula 4\r\n");
			if(beacon.angle[3] > M_PI/2)
			{
 				opponent.y = (LARGEUR_DEMI_TABLE*tan(M_PI - beacon.angle[3]) - LARGEUR_TABLE*tan(beacon.angle[1]) + LONGUEUR_TABLE) / (tan(M_PI - beacon.angle[3]) - tan(beacon.angle[1]));
			}
			else
			{
				opponent.y = (LARGEUR_DEMI_TABLE*tan(beacon.angle[3]) + LARGEUR_TABLE*tan(beacon.angle[1])-LONGUEUR_TABLE) / (tan(beacon.angle[1]) + tan(beacon.angle[3]));
			}
			opponent.x = (LARGEUR_TABLE - opponent.y)*tan(beacon.angle[1]);
			break;
		case 5:
// 			printf("[position.c] => formula 5\r\n");
			if(beacon.angle[3] > M_PI/2)
			{
				opponent.y = (LONGUEUR_TABLE + LARGEUR_DEMI_TABLE * tan(M_PI - beacon.angle[3])) / (tan(beacon.angle[2]) + tan(M_PI - beacon.angle[3]));
			}
			else
			{
				opponent.y = (LARGEUR_DEMI_TABLE*tan(beacon.angle[3]) - LONGUEUR_TABLE) / (tan(beacon.angle[3]) - tan(beacon.angle[2]));
			}
			opponent.x = tan(beacon.angle[2]) * opponent.y;
			break;
		default:
// 			printf("[position.c] => Unknown Formula\r\n");
			break;
	}
}