|
Wiselib
|
wiselib.testing/algorithms/localization/distance_based/modules/distance/localization_euclidean_messages.h
Go to the documentation of this file.
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_LOCALIZATION_EUCLIDEAN_MESSAGES_H 00020 #define __ALGORITHMS_LOCALIZATION_DISTANCE_BASED_LOCALIZATION_EUCLIDEAN_MESSAGES_H 00021 00022 #include "util/serialization/simple_types.h" 00023 #include "util/serialization/math_vec.h" 00024 #include "algorithms/localization/distance_based/util/localization_defutils.h" 00025 00026 namespace wiselib 00027 { 00028 00029 template<typename OsModel_P, 00030 typename Radio_P> 00031 class LocalizationEuclideanInitMessage 00032 { 00033 public: 00034 typedef OsModel_P OsModel; 00035 typedef Radio_P Radio; 00036 typedef typename Radio::node_id_t node_id_t; 00037 typedef typename Radio::block_data_t block_data_t; 00038 typedef typename Radio::size_t size_t; 00039 typedef typename Radio::message_id_t message_id_t; 00040 // -------------------------------------------------------------------- 00041 inline LocalizationEuclideanInitMessage(); 00042 // -------------------------------------------------------------------- 00043 inline message_id_t msg_id() 00044 { return read<OsModel, block_data_t, message_id_t>( buffer ); }; 00045 // -------------------------------------------------------------------- 00046 inline void set_msg_id( message_id_t id ) 00047 { write<OsModel, block_data_t, message_id_t>( buffer, id ); } 00048 // -------------------------------------------------------------------- 00049 inline bool anchor() 00050 { return read<OsModel, block_data_t, bool>(buffer + ANCHOR_POS); } 00051 // -------------------------------------------------------------------- 00052 inline void set_anchor( bool anchor ) 00053 { write<OsModel, block_data_t, bool>(buffer + ANCHOR_POS, anchor); } 00054 // -------------------------------------------------------------------- 00055 inline Vec source_position() 00056 { return read<OsModel, block_data_t, Vec>(buffer + SOURCE_POSITION_POS); } 00057 // -------------------------------------------------------------------- 00058 inline void set_source_position( Vec pos ) 00059 { write<OsModel, block_data_t, Vec>(buffer + SOURCE_POSITION_POS, pos); } 00060 // -------------------------------------------------------------------- 00061 inline size_t buffer_size() 00062 { return MSGEND_POS; } 00063 00064 private: 00065 enum data_positions 00066 { 00067 ANCHOR_POS = sizeof(message_id_t), 00068 SOURCE_POSITION_POS = ANCHOR_POS + sizeof(bool), 00069 MSGEND_POS = SOURCE_POSITION_POS + 3 * sizeof(double) 00070 }; 00071 00072 block_data_t buffer[MSGEND_POS]; 00073 }; 00074 // ----------------------------------------------------------------------- 00075 template<typename OsModel_P, 00076 typename Radio_P> 00077 LocalizationEuclideanInitMessage<OsModel_P, Radio_P>:: 00078 LocalizationEuclideanInitMessage() 00079 { 00080 set_msg_id( 0 ); 00081 set_anchor( 0 ); 00082 set_source_position( UNKNOWN_POSITION ); 00083 } 00084 // ----------------------------------------------------------------------- 00085 // ----------------------------------------------------------------------- 00086 // ----------------------------------------------------------------------- 00087 template<typename OsModel_P, 00088 typename Radio_P> 00089 class LocalizationEuclideanAnchorMessage 00090 { 00091 public: 00092 typedef OsModel_P OsModel; 00093 typedef Radio_P Radio; 00094 typedef typename Radio::node_id_t node_id_t; 00095 typedef typename Radio::block_data_t block_data_t; 00096 typedef typename Radio::size_t size_t; 00097 typedef typename Radio::message_id_t message_id_t; 00098 // -------------------------------------------------------------------- 00099 inline LocalizationEuclideanAnchorMessage(); 00100 // -------------------------------------------------------------------- 00101 inline message_id_t msg_id() 00102 { return read<OsModel, block_data_t, message_id_t>( buffer ); }; 00103 // -------------------------------------------------------------------- 00104 inline void set_msg_id( message_id_t id ) 00105 { write<OsModel, block_data_t, message_id_t>( buffer, id ); } 00106 // -------------------------------------------------------------------- 00107 inline node_id_t anchor() 00108 { return read<OsModel, block_data_t, node_id_t>(buffer + ANCHOR_POS); } 00109 // -------------------------------------------------------------------- 00110 inline void set_anchor( node_id_t anchor ) 00111 { write<OsModel, block_data_t, node_id_t>(buffer + ANCHOR_POS, anchor); } 00112 // -------------------------------------------------------------------- 00113 inline double distance() 00114 { return read<OsModel, block_data_t, double>(buffer + DISTANCE_POS); } 00115 // -------------------------------------------------------------------- 00116 inline void set_distance( double distance ) 00117 { write<OsModel, block_data_t, double>(buffer + DISTANCE_POS, distance); } 00118 // -------------------------------------------------------------------- 00119 inline Vec anchor_position() 00120 { return read<OsModel, block_data_t, Vec>(buffer + ANCHOR_POSITION_POS); } 00121 // -------------------------------------------------------------------- 00122 inline void set_anchor_position( Vec pos ) 00123 { write<OsModel, block_data_t, Vec>(buffer + ANCHOR_POSITION_POS, pos); } 00124 // -------------------------------------------------------------------- 00125 inline size_t buffer_size() 00126 { return MSGEND_POS; } 00127 00128 private: 00129 enum data_positions 00130 { 00131 ANCHOR_POS = sizeof(message_id_t), 00132 DISTANCE_POS = ANCHOR_POS + sizeof(node_id_t), 00133 ANCHOR_POSITION_POS = DISTANCE_POS + sizeof(double), 00134 MSGEND_POS = ANCHOR_POSITION_POS + 3 * sizeof(double) 00135 }; 00136 00137 block_data_t buffer[MSGEND_POS]; 00138 }; 00139 // ----------------------------------------------------------------------- 00140 template<typename OsModel_P, 00141 typename Radio_P> 00142 LocalizationEuclideanAnchorMessage<OsModel_P, Radio_P>:: 00143 LocalizationEuclideanAnchorMessage() 00144 { 00145 set_msg_id( 0 ); 00146 set_anchor( 0 ); 00147 set_distance( 0.0 ); 00148 set_anchor_position( UNKNOWN_POSITION ); 00149 } 00150 // ----------------------------------------------------------------------- 00151 // ----------------------------------------------------------------------- 00152 // ----------------------------------------------------------------------- 00153 template<typename OsModel_P, 00154 typename Radio_P, 00155 typename DistanceMap_P, 00156 int ENTRY_CNT = 20> 00157 class LocalizationEuclideanNeighborMessage 00158 { 00159 public: 00160 typedef OsModel_P OsModel; 00161 typedef Radio_P Radio; 00162 typedef DistanceMap_P DistanceMap; 00163 00164 typedef typename DistanceMap::value_type distmap_value_t; 00165 typedef typename DistanceMap::key_type distmap_key_t; 00166 typedef typename DistanceMap::mapped_type distmap_mapped_t; 00167 typedef typename DistanceMap::iterator distmap_iterator_t; 00168 00169 typedef typename Radio::node_id_t node_id_t; 00170 typedef typename Radio::block_data_t block_data_t; 00171 typedef typename Radio::size_t size_t; 00172 typedef typename Radio::message_id_t message_id_t; 00173 // -------------------------------------------------------------------- 00174 inline LocalizationEuclideanNeighborMessage(); 00175 // -------------------------------------------------------------------- 00176 message_id_t msg_id() 00177 { return read<OsModel, block_data_t, message_id_t>( buffer ); }; 00178 // -------------------------------------------------------------------- 00179 void set_msg_id( message_id_t id ) 00180 { write<OsModel, block_data_t, message_id_t>( buffer, id ); } 00181 // -------------------------------------------------------------------- 00182 DistanceMap neighbors() 00183 { 00184 DistanceMap dm; 00185 for ( int i = 0; i < entry_cnt(); i++ ) 00186 { 00187 distmap_key_t key; 00188 distmap_mapped_t mapped; 00189 if ( entry( i, key, mapped ) ) 00190 dm[key] = mapped; 00191 } 00192 return dm; 00193 } 00194 // -------------------------------------------------------------------- 00195 void set_neighbors( DistanceMap& neighbors ) 00196 { 00197 set_entry_cnt( neighbors.size() ); 00198 int idx = 0; 00199 for ( distmap_iterator_t 00200 it = neighbors.begin(); 00201 it != neighbors.end(); 00202 ++it, idx++ ) 00203 { 00204 set_entry( idx, (*it) ); 00205 } 00206 } 00207 // -------------------------------------------------------------------- 00208 size_t buffer_size() 00209 { return DATA_POS + entry_cnt() * sizeof(distmap_value_t); } 00210 00211 private: 00212 // -------------------------------------------------------------------- 00213 uint8_t entry_cnt() 00214 { return read<OsModel, block_data_t, uint8_t>(buffer + ENTRY_CNT_POS); } 00215 // -------------------------------------------------------------------- 00216 void set_entry_cnt( uint8_t cnt ) 00217 { write<OsModel, block_data_t, uint8_t>(buffer + ENTRY_CNT_POS, cnt); } 00218 // -------------------------------------------------------------------- 00219 void set_entry( uint8_t idx, const distmap_value_t& value ) 00220 { 00221 int offset = DATA_POS + idx * sizeof(distmap_value_t); 00222 write<OsModel, block_data_t>( buffer + offset, value.first ); 00223 write<OsModel, block_data_t>( buffer + offset + sizeof(distmap_key_t), value.second ); 00224 }; 00225 // -------------------------------------------------------------------- 00226 bool entry( uint8_t idx, distmap_key_t& key, distmap_mapped_t& mapped ) 00227 { 00228 if (idx >= ENTRY_CNT) 00229 return false; 00230 00231 int offset = DATA_POS + idx * sizeof(distmap_value_t); 00232 read<OsModel, block_data_t>( buffer + offset, key ); 00233 read<OsModel, block_data_t>( buffer + offset + sizeof(distmap_key_t), mapped ); 00234 return true; 00235 }; 00236 // -------------------------------------------------------------------- 00237 enum data_positions 00238 { 00239 MSG_ID_POS = 0, 00240 ENTRY_CNT_POS = MSG_ID_POS + sizeof(message_id_t), 00241 DATA_POS = ENTRY_CNT_POS + 1 00242 }; 00243 00244 block_data_t buffer[DATA_POS + ENTRY_CNT * sizeof(distmap_value_t)]; 00245 }; 00246 // ----------------------------------------------------------------------- 00247 template<typename OsModel_P, 00248 typename Radio_P, 00249 typename DistanceMap_P, 00250 int ENTRY_CNT> 00251 LocalizationEuclideanNeighborMessage<OsModel_P, Radio_P, DistanceMap_P, ENTRY_CNT>:: 00252 LocalizationEuclideanNeighborMessage() 00253 { 00254 set_msg_id( 0 ); 00255 } 00256 00257 }// namespace wiselib 00258 #endif
