Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * globfromrel.cpp - Implementation of the global ball position model 00004 * 00005 * Created: Fri Jun 03 22:56:22 2005 00006 * Copyright 2005 Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de> 00007 * 2005-2006 Tim Niemueller [www.niemueller.de] 00008 * 2005 Martin Heracles <Martin.Heracles@rwth-aachen.de> 00009 * 00010 ****************************************************************************/ 00011 00012 /* This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU General Public License as published by 00014 * the Free Software Foundation; either version 2 of the License, or 00015 * (at your option) any later version. A runtime exception applies to 00016 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00017 * 00018 * This program is distributed in the hope that it will be useful, 00019 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00020 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00021 * GNU Library General Public License for more details. 00022 * 00023 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00024 */ 00025 00026 #include <cmath> 00027 #include <fvmodels/global_position/globfromrel.h> 00028 #include <fvmodels/relative_position/relativepositionmodel.h> 00029 00030 namespace firevision { 00031 #if 0 /* just to make Emacs auto-indent happy */ 00032 } 00033 #endif 00034 00035 /** @class GlobalFromRelativePos <fvmodels/global_position/globfromrel.h> 00036 * Calculate global ball position based on a relative position model. 00037 * The relative position model must of course be tied to the ball. 00038 */ 00039 00040 /** Constructor. 00041 * @param model relative position model for the ball. 00042 */ 00043 GlobalFromRelativePos::GlobalFromRelativePos(RelativePositionModel* model) 00044 { 00045 m_pRelaModel = model; 00046 m_fPosX = 0.0f; 00047 m_fPosY = 0.0f; 00048 m_fPhi = 0.0f; 00049 } 00050 00051 00052 void 00053 GlobalFromRelativePos::set_robot_position(float x, float y, float ori) 00054 { 00055 m_fPosX = x; 00056 m_fPosY = y; 00057 m_fPhi = ori; 00058 } 00059 00060 00061 void 00062 GlobalFromRelativePos::set_position_in_image(unsigned int x, unsigned int y) 00063 { 00064 } 00065 00066 00067 void 00068 GlobalFromRelativePos::calc() 00069 { 00070 } 00071 00072 00073 bool 00074 GlobalFromRelativePos::is_pos_valid() const 00075 { 00076 return m_pRelaModel->is_pos_valid(); 00077 } 00078 00079 00080 float 00081 GlobalFromRelativePos::get_x() const 00082 { 00083 /* 00084 cout << " DETAILS of \"getX()\"" << endl 00085 << " Formula: " << endl 00086 << " ( relX * cos(phi) -" << endl 00087 << " relY * sin(phi) ) + robX" << endl 00088 << " ( " << m_pRelaModel->getX() << " * " << "cos(" << m_fPhi << ") -" << endl 00089 << " " << m_pRelaModel->getY() << " * " << "sin(" << m_fPhi << ") ) + " << m_fPosX << endl 00090 << " ( " << m_pRelaModel->getX() << " * " << cos(m_fPhi) << ") -" << endl 00091 << " " << m_pRelaModel->getY() << " * " << sin(m_fPhi) << " ) + " << m_fPosX << endl 00092 << " ( " << m_pRelaModel->getX() * cos(m_fPhi) << ") -" << endl 00093 << " " << m_pRelaModel->getY() * sin(m_fPhi) << ") ) + " << m_fPosX << endl 00094 << " ---> " << (m_pRelaModel->getX() * cos(m_fPhi) - m_pRelaModel->getY() * sin(m_fPhi)) + m_fPosX << flush; 00095 */ 00096 return ( m_pRelaModel->get_x() * cos(m_fPhi) 00097 - m_pRelaModel->get_y() * sin(m_fPhi) ) 00098 + m_fPosX; 00099 } 00100 00101 00102 float 00103 GlobalFromRelativePos::get_y() const 00104 { 00105 /* 00106 cout << " DETAILS of \"getY()\"" << endl 00107 << " Formula: " << endl 00108 << " ( relX * sin(phi) -" << endl 00109 << " relY * cos(phi) ) + robY" << endl 00110 << " ( " << m_pRelaModel->getX() << " * " << "sin(" << m_fPhi << ") +" << endl 00111 << " " << m_pRelaModel->getY() << " * " << "cos(" << m_fPhi << ") ) + " << m_fPosY << endl 00112 << " ( " << m_pRelaModel->getX() << " * " << sin(m_fPhi) << ") +" << endl 00113 << " " << m_pRelaModel->getY() << " * " << cos(m_fPhi) << " ) + " << m_fPosY << endl 00114 << " ( " << m_pRelaModel->getX() * sin(m_fPhi) << ") +" << endl 00115 << " " << m_pRelaModel->getY() * cos(m_fPhi) << ") ) + " << m_fPosY << endl 00116 << " ---> " << (m_pRelaModel->getX() * sin(m_fPhi) + m_pRelaModel->getY() * cos(m_fPhi)) + m_fPosY << flush; 00117 */ 00118 return ( m_pRelaModel->get_x() * sin(m_fPhi) 00119 + m_pRelaModel->get_y() * cos(m_fPhi) ) 00120 + m_fPosY; 00121 } 00122 00123 } // end namespace firevision