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 namespace firevision {
00031 #if 0 /* just to make Emacs auto-indent happy */
00032 }
00033 #endif
00034 
00035 /** @class GlobalFromRelativePos <models/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