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 <cmath>
27 #include <fvmodels/global_position/globfromrel.h>
28 #include <fvmodels/relative_position/relativepositionmodel.h>
29 
30 namespace firevision {
31 #if 0 /* just to make Emacs auto-indent happy */
32 }
33 #endif
34 
35 /** @class GlobalFromRelativePos <fvmodels/global_position/globfromrel.h>
36  * Calculate global ball position based on a relative position model.
37  * The relative position model must of course be tied to the ball.
38  */
39 
40 /** Constructor.
41  * @param model relative position model for the ball.
42  */
44 {
45  m_pRelaModel = model;
46  m_fPosX = 0.0f;
47  m_fPosY = 0.0f;
48  m_fPhi = 0.0f;
49 }
50 
51 
52 void
53 GlobalFromRelativePos::set_robot_position(float x, float y, float ori)
54 {
55  m_fPosX = x;
56  m_fPosY = y;
57  m_fPhi = ori;
58 }
59 
60 
61 void
62 GlobalFromRelativePos::set_position_in_image(unsigned int x, unsigned int y)
63 {
64 }
65 
66 
67 void
69 {
70 }
71 
72 
73 bool
75 {
76  return m_pRelaModel->is_pos_valid();
77 }
78 
79 
80 float
82 {
83  /*
84  cout << " DETAILS of \"getX()\"" << endl
85  << " Formula: " << endl
86  << " ( relX * cos(phi) -" << endl
87  << " relY * sin(phi) ) + robX" << endl
88  << " ( " << m_pRelaModel->getX() << " * " << "cos(" << m_fPhi << ") -" << endl
89  << " " << m_pRelaModel->getY() << " * " << "sin(" << m_fPhi << ") ) + " << m_fPosX << endl
90  << " ( " << m_pRelaModel->getX() << " * " << cos(m_fPhi) << ") -" << endl
91  << " " << m_pRelaModel->getY() << " * " << sin(m_fPhi) << " ) + " << m_fPosX << endl
92  << " ( " << m_pRelaModel->getX() * cos(m_fPhi) << ") -" << endl
93  << " " << m_pRelaModel->getY() * sin(m_fPhi) << ") ) + " << m_fPosX << endl
94  << " ---> " << (m_pRelaModel->getX() * cos(m_fPhi) - m_pRelaModel->getY() * sin(m_fPhi)) + m_fPosX << flush;
95  */
96  return ( m_pRelaModel->get_x() * cos(m_fPhi)
97  - m_pRelaModel->get_y() * sin(m_fPhi) )
98  + m_fPosX;
99 }
100 
101 
102 float
104 {
105  /*
106  cout << " DETAILS of \"getY()\"" << endl
107  << " Formula: " << endl
108  << " ( relX * sin(phi) -" << endl
109  << " relY * cos(phi) ) + robY" << endl
110  << " ( " << m_pRelaModel->getX() << " * " << "sin(" << m_fPhi << ") +" << endl
111  << " " << m_pRelaModel->getY() << " * " << "cos(" << m_fPhi << ") ) + " << m_fPosY << endl
112  << " ( " << m_pRelaModel->getX() << " * " << sin(m_fPhi) << ") +" << endl
113  << " " << m_pRelaModel->getY() << " * " << cos(m_fPhi) << " ) + " << m_fPosY << endl
114  << " ( " << m_pRelaModel->getX() * sin(m_fPhi) << ") +" << endl
115  << " " << m_pRelaModel->getY() * cos(m_fPhi) << ") ) + " << m_fPosY << endl
116  << " ---> " << (m_pRelaModel->getX() * sin(m_fPhi) + m_pRelaModel->getY() * cos(m_fPhi)) + m_fPosY << flush;
117  */
118  return ( m_pRelaModel->get_x() * sin(m_fPhi)
119  + m_pRelaModel->get_y() * cos(m_fPhi) )
120  + m_fPosY;
121 }
122 
123 } // end namespace firevision
virtual float get_x() const =0
Get relative X coordinate of object.
GlobalFromRelativePos(RelativePositionModel *model)
Constructor.
Definition: globfromrel.cpp:43
Relative Position Model Interface.
virtual float get_y() const =0
Get relative Y coordinate of object.
virtual float get_x(void) const
Get global x coordinate of object.
Definition: globfromrel.cpp:81
virtual bool is_pos_valid() const
Check if the position is valid.
Definition: globfromrel.cpp:74
virtual bool is_pos_valid() const =0
Check if position is valid.
virtual void set_position_in_image(unsigned int x, unsigned int y)
Set the position of the object as recognized in the image.
Definition: globfromrel.cpp:62
virtual void calc()
Calculate position.
Definition: globfromrel.cpp:68
virtual void set_robot_position(float x, float y, float ori)
Set the global position of the object.
Definition: globfromrel.cpp:53
virtual float get_y(void) const
Get global y coordinate of object.