Wiselib: wiselib.testing/algorithms/localization/distance_based/math/localization_lcs

Wiselib
00001 /***************************************************************************
00002  ** This file is part of the generic algorithm library Wiselib.           **
00003  ** Copyright (C) 2008,2009 by the Wisebed (www.wisebed.eu) project.      **
00004  **                                                                       **
00005  ** The Wiselib is free software: you can redistribute it and/or modify   **
00006  ** it under the terms of the GNU Lesser General Public License as        **
00007  ** published by the Free Software Foundation, either version 3 of the    **
00008  ** License, or (at your option) any later version.                       **
00009  **                                                                       **
00010  ** The Wiselib is distributed in the hope that it will be useful,        **
00011  ** but WITHOUT ANY WARRANTY; without even the implied warranty of        **
00012  ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         **
00013  ** GNU Lesser General Public License for more details.                   **
00014  **                                                                       **
00015  ** You should have received a copy of the GNU Lesser General Public      **
00016  ** License along with the Wiselib.                                       **
00017  ** If not, see <http://www.gnu.org/licenses/>.                           **
00018  ***************************************************************************/
00019 #ifndef __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_LCS_HELPERS_H
00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_LCS_HELPERS_H
00021 
00022 #include "algorithms/localization/distance_based/math/vec.h"
00023 #include "algorithms/localization/distance_based/util/localization_defutils.h"
00024 #include <float.h>
00025 
00026 namespace wiselib
00027 {
00028 
00043 template <typename Arithmatic_P >
00044    static bool compute_basis_coords(
00045          Arithmatic_P, Arithmatic_P, Arithmatic_P,
00046          Vec<Arithmatic_P>&, Vec<Arithmatic_P>& );
00062 template <typename Arithmatic_P >
00063    static bool compute_rel_coord_triangulisation(
00064          Arithmatic_P, Arithmatic_P, Arithmatic_P, Arithmatic_P, Arithmatic_P, Arithmatic_P,
00065          Vec<Arithmatic_P>&, Arithmatic_P );
00066    // -----------------------------------------------------------------------
00067    // -----------------------------------------------------------------------
00068    // -----------------------------------------------------------------------
00069 template <typename Arithmatic_P >
00070    bool
00071    compute_basis_coords(
00072          Arithmatic_P src_p, Arithmatic_P src_q, Arithmatic_P p_q,
00073          Vec<Arithmatic_P>& pos_p, Vec<Arithmatic_P>& pos_q )
00074    {
00075       if ( p_q == UNKNOWN_DISTANCE || p_q == 0 ||
00076             src_p == UNKNOWN_DISTANCE || src_p == 0 ||
00077             src_q == UNKNOWN_DISTANCE || src_q == 0 )
00078          return false;
00079 
00080       Arithmatic_P gamma = acos( ( src_q*src_q + src_p*src_p - p_q*p_q ) / ( 2 * src_p * src_q ) );
00081 
00082       Arithmatic_P pos_x_q = src_q*cos(gamma);
00083       Arithmatic_P pos_y_q = src_q*sin(gamma);
00084 
00085       if ( isnan( pos_x_q ) || isnan( pos_y_q ) )
00086          return false;
00087 
00088       pos_p = Vec<Arithmatic_P>( src_p, 0.0, 0.0 );
00089       pos_q = Vec<Arithmatic_P>( pos_x_q, pos_y_q, 0.0 );
00090 
00091       return true;
00092    }
00093    // ----------------------------------------------------------------------
00094 template <typename Arithmatic_P >
00095    bool
00096    compute_rel_coord_triangulisation(
00097          Arithmatic_P src_p, Arithmatic_P src_q, Arithmatic_P src_j, Arithmatic_P p_j, Arithmatic_P q_j, Arithmatic_P p_q,
00098          Vec<Arithmatic_P>& pos_j, Arithmatic_P epsilon )
00099    {
00100       if ( src_j == UNKNOWN_DISTANCE || src_j == 0 ||
00101             p_j == UNKNOWN_DISTANCE || p_j == 0 ||
00102             q_j == UNKNOWN_DISTANCE || q_j == 0 )
00103          return false;
00104 
00105       Arithmatic_P alpha_j =
00106          acos( ( src_j*src_j + src_p*src_p - p_j*p_j ) / ( 2 * src_j * src_p )  );
00107       Arithmatic_P beta_j =
00108          acos( ( src_j*src_j + src_q*src_q - q_j*q_j ) / ( 2 * src_j * src_q )  );
00109 
00110       Arithmatic_P gamma = acos( ( src_q*src_q + src_p*src_p - p_q*p_q ) / ( 2 * src_p * src_q ) );
00111 
00112       Arithmatic_P pos_x_j = src_j * cos( alpha_j );
00113       Arithmatic_P pos_y_j = 0.0;
00114       if ( fabs( fabs( alpha_j - gamma ) - beta_j ) <= epsilon )
00115          pos_y_j = src_j * sin( alpha_j );
00116       else
00117          pos_y_j = -src_j * sin( alpha_j );
00118 
00119       if ( isnan( pos_x_j ) || isnan( pos_y_j ) )
00120          return false;
00121 
00122       pos_j = Vec<Arithmatic_P>( pos_x_j, pos_y_j, 0.0 );
00123 
00124       return true;
00125    }
00126 
00127 }// namespace wiselib
00128 #endif