globfromrel.cpp
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 <models/global_position/globfromrel.h> 00028 #include <models/relative_position/relativepositionmodel.h> 00029 00030 /** @class GlobalFromRelativePos <models/global_position/globfromrel.h> 00031 * Calculate global ball position based on a relative position model. 00032 * The relative position model must of course be tied to the ball. 00033 */ 00034 00035 /** Constructor. 00036 * @param model relative position model for the ball. 00037 */ 00038 GlobalFromRelativePos::GlobalFromRelativePos(RelativePositionModel* model) 00039 { 00040 m_pRelaModel = model; 00041 m_fPosX = 0.0f; 00042 m_fPosY = 0.0f; 00043 m_fPhi = 0.0f; 00044 } 00045 00046 00047 void 00048 GlobalFromRelativePos::set_robot_position(float x, float y, float ori) 00049 { 00050 m_fPosX = x; 00051 m_fPosY = y; 00052 m_fPhi = ori; 00053 } 00054 00055 00056 void 00057 GlobalFromRelativePos::set_position_in_image(unsigned int x, unsigned int y) 00058 { 00059 } 00060 00061 00062 void 00063 GlobalFromRelativePos::calc() 00064 { 00065 } 00066 00067 00068 bool 00069 GlobalFromRelativePos::is_pos_valid() const 00070 { 00071 return m_pRelaModel->is_pos_valid(); 00072 } 00073 00074 00075 float 00076 GlobalFromRelativePos::get_x() const 00077 { 00078 /* 00079 cout << " DETAILS of \"getX()\"" << endl 00080 << " Formula: " << endl 00081 << " ( relX * cos(phi) -" << endl 00082 << " relY * sin(phi) ) + robX" << endl 00083 << " ( " << m_pRelaModel->getX() << " * " << "cos(" << m_fPhi << ") -" << endl 00084 << " " << m_pRelaModel->getY() << " * " << "sin(" << m_fPhi << ") ) + " << m_fPosX << endl 00085 << " ( " << m_pRelaModel->getX() << " * " << cos(m_fPhi) << ") -" << endl 00086 << " " << m_pRelaModel->getY() << " * " << sin(m_fPhi) << " ) + " << m_fPosX << endl 00087 << " ( " << m_pRelaModel->getX() * cos(m_fPhi) << ") -" << endl 00088 << " " << m_pRelaModel->getY() * sin(m_fPhi) << ") ) + " << m_fPosX << endl 00089 << " ---> " << (m_pRelaModel->getX() * cos(m_fPhi) - m_pRelaModel->getY() * sin(m_fPhi)) + m_fPosX << flush; 00090 */ 00091 return ( m_pRelaModel->get_x() * cos(m_fPhi) 00092 - m_pRelaModel->get_y() * sin(m_fPhi) ) 00093 + m_fPosX; 00094 } 00095 00096 00097 float 00098 GlobalFromRelativePos::get_y() const 00099 { 00100 /* 00101 cout << " DETAILS of \"getY()\"" << endl 00102 << " Formula: " << endl 00103 << " ( relX * sin(phi) -" << endl 00104 << " relY * cos(phi) ) + robY" << endl 00105 << " ( " << m_pRelaModel->getX() << " * " << "sin(" << m_fPhi << ") +" << endl 00106 << " " << m_pRelaModel->getY() << " * " << "cos(" << m_fPhi << ") ) + " << m_fPosY << endl 00107 << " ( " << m_pRelaModel->getX() << " * " << sin(m_fPhi) << ") +" << endl 00108 << " " << m_pRelaModel->getY() << " * " << cos(m_fPhi) << " ) + " << m_fPosY << endl 00109 << " ( " << m_pRelaModel->getX() * sin(m_fPhi) << ") +" << endl 00110 << " " << m_pRelaModel->getY() * cos(m_fPhi) << ") ) + " << m_fPosY << endl 00111 << " ---> " << (m_pRelaModel->getX() * sin(m_fPhi) + m_pRelaModel->getY() * cos(m_fPhi)) + m_fPosY << flush; 00112 */ 00113 return ( m_pRelaModel->get_x() * sin(m_fPhi) 00114 + m_pRelaModel->get_y() * cos(m_fPhi) ) 00115 + m_fPosY; 00116 }
