Fawkes API
Fawkes Development Version
globfromrel.cpp
1
2
/***************************************************************************
3
* globfromrel.cpp - Implementation of the global ball position model
4
*
5
* Created: Fri Jun 03 22:56:22 2005
6
* Copyright 2005 Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de>
7
* 2005-2006 Tim Niemueller [www.niemueller.de]
8
* 2005 Martin Heracles <Martin.Heracles@rwth-aachen.de>
9
*
10
****************************************************************************/
11
12
/* This program is free software; you can redistribute it and/or modify
13
* it under the terms of the GNU General Public License as published by
14
* the Free Software Foundation; either version 2 of the License, or
15
* (at your option) any later version. A runtime exception applies to
16
* this software (see LICENSE.GPL_WRE file mentioned below for details).
17
*
18
* This program is distributed in the hope that it will be useful,
19
* but WITHOUT ANY WARRANTY; without even the implied warranty of
20
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21
* GNU Library General Public License for more details.
22
*
23
* Read the full text in the LICENSE.GPL_WRE file in the doc directory.
24
*/
25
26
#include <fvmodels/global_position/globfromrel.h>
27
#include <fvmodels/relative_position/relativepositionmodel.h>
28
29
#include <cmath>
30
31
namespace
firevision {
32
33
/** @class GlobalFromRelativePos <fvmodels/global_position/globfromrel.h>
34
* Calculate global ball position based on a relative position model.
35
* The relative position model must of course be tied to the ball.
36
*/
37
38
/** Constructor.
39
* @param model relative position model for the ball.
40
*/
41
GlobalFromRelativePos::GlobalFromRelativePos
(RelativePositionModel *model)
42
{
43
m_pRelaModel = model;
44
m_fPosX = 0.0f;
45
m_fPosY = 0.0f;
46
m_fPhi = 0.0f;
47
}
48
49
void
50
GlobalFromRelativePos::set_robot_position
(
float
x,
float
y,
float
ori)
51
{
52
m_fPosX = x;
53
m_fPosY = y;
54
m_fPhi = ori;
55
}
56
57
void
58
GlobalFromRelativePos::set_position_in_image
(
unsigned
int
x,
unsigned
int
y)
59
{
60
}
61
62
void
63
GlobalFromRelativePos::calc
()
64
{
65
}
66
67
bool
68
GlobalFromRelativePos::is_pos_valid
()
const
69
{
70
return
m_pRelaModel->
is_pos_valid
();
71
}
72
73
float
74
GlobalFromRelativePos::get_x
()
const
75
{
76
/*
77
cout << " DETAILS of \"getX()\"" << endl
78
<< " Formula: " << endl
79
<< " ( relX * cos(phi) -" << endl
80
<< " relY * sin(phi) ) + robX" << endl
81
<< " ( " << m_pRelaModel->getX() << " * " << "cos(" << m_fPhi << ") -" << endl
82
<< " " << m_pRelaModel->getY() << " * " << "sin(" << m_fPhi << ") ) + " << m_fPosX << endl
83
<< " ( " << m_pRelaModel->getX() << " * " << cos(m_fPhi) << ") -" << endl
84
<< " " << m_pRelaModel->getY() << " * " << sin(m_fPhi) << " ) + " << m_fPosX << endl
85
<< " ( " << m_pRelaModel->getX() * cos(m_fPhi) << ") -" << endl
86
<< " " << m_pRelaModel->getY() * sin(m_fPhi) << ") ) + " << m_fPosX << endl
87
<< " ---> " << (m_pRelaModel->getX() * cos(m_fPhi) - m_pRelaModel->getY() * sin(m_fPhi)) + m_fPosX << flush;
88
*/
89
return
(m_pRelaModel->
get_x
() * cos(m_fPhi) - m_pRelaModel->
get_y
() * sin(m_fPhi)) + m_fPosX;
90
}
91
92
float
93
GlobalFromRelativePos::get_y
()
const
94
{
95
/*
96
cout << " DETAILS of \"getY()\"" << endl
97
<< " Formula: " << endl
98
<< " ( relX * sin(phi) -" << endl
99
<< " relY * cos(phi) ) + robY" << endl
100
<< " ( " << m_pRelaModel->getX() << " * " << "sin(" << m_fPhi << ") +" << endl
101
<< " " << m_pRelaModel->getY() << " * " << "cos(" << m_fPhi << ") ) + " << m_fPosY << endl
102
<< " ( " << m_pRelaModel->getX() << " * " << sin(m_fPhi) << ") +" << endl
103
<< " " << m_pRelaModel->getY() << " * " << cos(m_fPhi) << " ) + " << m_fPosY << endl
104
<< " ( " << m_pRelaModel->getX() * sin(m_fPhi) << ") +" << endl
105
<< " " << m_pRelaModel->getY() * cos(m_fPhi) << ") ) + " << m_fPosY << endl
106
<< " ---> " << (m_pRelaModel->getX() * sin(m_fPhi) + m_pRelaModel->getY() * cos(m_fPhi)) + m_fPosY << flush;
107
*/
108
return
(m_pRelaModel->
get_x
() * sin(m_fPhi) + m_pRelaModel->
get_y
() * cos(m_fPhi)) + m_fPosY;
109
}
110
111
}
// end namespace firevision
firevision::GlobalFromRelativePos::GlobalFromRelativePos
GlobalFromRelativePos(RelativePositionModel *model)
Constructor.
Definition:
globfromrel.cpp:49
firevision::GlobalFromRelativePos::set_robot_position
virtual void set_robot_position(float x, float y, float ori)
Definition:
globfromrel.cpp:58
firevision::GlobalFromRelativePos::get_x
virtual float get_x(void) const
Definition:
globfromrel.cpp:82
firevision::RelativePositionModel::get_x
virtual float get_x() const =0
firevision::GlobalFromRelativePos::get_y
virtual float get_y(void) const
Definition:
globfromrel.cpp:101
firevision::GlobalFromRelativePos::calc
virtual void calc()
Definition:
globfromrel.cpp:71
firevision::RelativePositionModel::get_y
virtual float get_y() const =0
firevision::GlobalFromRelativePos::set_position_in_image
virtual void set_position_in_image(unsigned int x, unsigned int y)
Definition:
globfromrel.cpp:66
firevision::GlobalFromRelativePos::is_pos_valid
virtual bool is_pos_valid() const
Definition:
globfromrel.cpp:76
firevision::RelativePositionModel::is_pos_valid
virtual bool is_pos_valid() const =0
src
libs
fvmodels
global_position
globfromrel.cpp
Generated by
1.8.17