Wiselib: wiselib.testing/algorithms/tracking/PLTT

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 __PLTT_TRACE_H__
00020 #define __PLTT_TRACE_H__
00021 namespace wiselib
00022 {
00023    template<   typename Os_P,
00024             typename Radio_P,
00025             typename TimesNumber_P,
00026             typename SecondsNumber_P,
00027             typename IntensityNumber_P,
00028             typename Node_P,
00029             typename NodeID_P,
00030             typename Debug_P>
00031    class PLTT_TraceType
00032    {
00033    public:
00034       typedef Os_P Os;
00035       typedef Radio_P Radio;
00036       typedef TimesNumber_P TimesNumber;
00037       typedef SecondsNumber_P SecondsNumber;
00038       typedef IntensityNumber_P IntensityNumber;
00039       typedef Node_P Node;
00040       typedef NodeID_P NodeID;
00041       typedef Debug_P Debug;
00042       typedef typename Radio::block_data_t block_data_t;
00043       typedef typename Radio::size_t size_t;
00044       typedef PLTT_TraceType<Os, Radio, TimesNumber, SecondsNumber, IntensityNumber, Node, NodeID, Debug> self_type;
00045       typedef struct PLTT_TraceData_Type
00046       {
00047          Node current;
00048          Node parent;
00049          Node grandparent;
00050          NodeID target_id;
00051          TimesNumber start_time;
00052          SecondsNumber diminish_seconds;
00053          IntensityNumber diminish_amount;
00054          IntensityNumber spread_penalty;
00055          IntensityNumber intensity;
00056          PLTT_TraceData_Type():
00057          target_id      (0),
00058          start_time     (0),
00059          diminish_seconds  (0),
00060          diminish_amount   (0),
00061          spread_penalty    (0),
00062          intensity         (0)
00063          {}
00064       } PLTT_TraceData;
00065 
00066       inline PLTT_TraceType():
00067       inhibited  ( 0 )
00068       {}
00069       inline PLTT_TraceType(const SecondsNumber& _ds, const IntensityNumber& _da, const IntensityNumber& _sp,  const IntensityNumber& _si, const TimesNumber& _st )
00070       {
00071          trace_data.diminish_seconds = _ds;
00072          trace_data.diminish_amount = _da;
00073          trace_data.spread_penalty = _sp;
00074          trace_data.intensity = _si;
00075          trace_data.start_time = _st;
00076          inhibited = 0;
00077       }
00078       inline PLTT_TraceType( const PLTT_TraceData& _td )
00079       {
00080          trace_data = _td;
00081          inhibited = 0;
00082       }
00083       inline PLTT_TraceType( const self_type& _t)
00084       {
00085          *this = _t;
00086       }
00087       inline PLTT_TraceType( block_data_t* buff, size_t offset = 0 )
00088       {
00089          get_from_buffer( buff, offset );
00090          inhibited = 0;
00091       }
00092       inline block_data_t* set_buffer_from( block_data_t* buff, size_t offset = 0 )
00093       {
00094          uint8_t CURRENT_NODE_POS = 0;
00095          uint8_t PARENT_NODE_POS = trace_data.current.get_buffer_size() + CURRENT_NODE_POS;
00096          uint8_t GRANDPARENT_NODE_POS = trace_data.parent.get_buffer_size() + PARENT_NODE_POS;
00097          uint8_t TARGET_ID_POS = trace_data.grandparent.get_buffer_size() + GRANDPARENT_NODE_POS;
00098          uint8_t START_TIME_POS = sizeof( NodeID ) + TARGET_ID_POS;
00099          uint8_t DIMINISH_SECS_POS = sizeof( TimesNumber ) + START_TIME_POS;
00100          uint8_t DIMINISH_AMOUNT_POS = sizeof( SecondsNumber ) + DIMINISH_SECS_POS;
00101          uint8_t SPREAD_PENALTY_POS = sizeof( IntensityNumber ) + DIMINISH_AMOUNT_POS;
00102          uint8_t INTENSITY_POS = sizeof( IntensityNumber ) + SPREAD_PENALTY_POS;
00103          trace_data.current.set_buffer_from( buff, CURRENT_NODE_POS + offset );
00104          trace_data.parent.set_buffer_from( buff, PARENT_NODE_POS + offset);
00105          trace_data.grandparent.set_buffer_from( buff, GRANDPARENT_NODE_POS + offset);
00106          write<Os, block_data_t, NodeID>( buff + TARGET_ID_POS + offset, trace_data.target_id );
00107          write<Os, block_data_t, TimesNumber>( buff + START_TIME_POS + offset, trace_data.start_time );
00108          write<Os, block_data_t, SecondsNumber>( buff + DIMINISH_SECS_POS + offset, trace_data.diminish_seconds );
00109          write<Os, block_data_t, IntensityNumber>( buff + DIMINISH_AMOUNT_POS + offset, trace_data.diminish_amount );
00110          write<Os, block_data_t, IntensityNumber>( buff + SPREAD_PENALTY_POS + offset, trace_data.spread_penalty );
00111          write<Os, block_data_t, IntensityNumber>( buff + INTENSITY_POS + offset, trace_data.intensity );
00112          return buff;
00113       }
00114       // --------------------------------------------------------------------
00115       inline void get_from_buffer(block_data_t* buff, size_t offset = 0)
00116       {
00117          uint8_t CURRENT_NODE_POS = 0;
00118          uint8_t PARENT_NODE_POS = trace_data.current.get_buffer_size() + CURRENT_NODE_POS;
00119          uint8_t GRANDPARENT_NODE_POS = trace_data.parent.get_buffer_size() + PARENT_NODE_POS;
00120          uint8_t TARGET_ID_POS = trace_data.grandparent.get_buffer_size() + GRANDPARENT_NODE_POS;
00121          uint8_t START_TIME_POS = sizeof( NodeID ) + TARGET_ID_POS;
00122          uint8_t DIMINISH_SECS_POS = sizeof( TimesNumber ) + START_TIME_POS;
00123          uint8_t DIMINISH_AMOUNT_POS = sizeof( SecondsNumber ) + DIMINISH_SECS_POS;
00124          uint8_t SPREAD_PENALTY_POS = sizeof( IntensityNumber ) + DIMINISH_AMOUNT_POS;
00125          uint8_t INTENSITY_POS = sizeof( IntensityNumber ) + SPREAD_PENALTY_POS;
00126          trace_data.current.get_from_buffer( buff, CURRENT_NODE_POS + offset);
00127          trace_data.parent.get_from_buffer( buff, PARENT_NODE_POS + offset );
00128          trace_data.grandparent.get_from_buffer( buff, GRANDPARENT_NODE_POS + offset );
00129          trace_data.target_id = read<Os, block_data_t, NodeID>(buff + TARGET_ID_POS + offset );
00130          trace_data.start_time = read<Os, block_data_t, TimesNumber>(buff + START_TIME_POS + offset );
00131          trace_data.diminish_seconds = read<Os, block_data_t, SecondsNumber>(buff + DIMINISH_SECS_POS + offset );
00132          trace_data.diminish_amount = read<Os, block_data_t, IntensityNumber>(buff + DIMINISH_AMOUNT_POS + offset );
00133          trace_data.spread_penalty = read<Os, block_data_t, IntensityNumber>(buff + SPREAD_PENALTY_POS + offset );
00134          trace_data.intensity = read<Os, block_data_t, IntensityNumber>(buff + INTENSITY_POS + offset );
00135       }
00136       inline size_t get_buffer_size()
00137       {
00138          uint8_t CURRENT_NODE_POS = 0;
00139          uint8_t PARENT_NODE_POS = trace_data.current.get_buffer_size() + CURRENT_NODE_POS;
00140          uint8_t GRANDPARENT_NODE_POS = trace_data.parent.get_buffer_size() + PARENT_NODE_POS;
00141          uint8_t TARGET_ID_POS = trace_data.grandparent.get_buffer_size() + GRANDPARENT_NODE_POS;
00142          uint8_t START_TIME_POS = sizeof( NodeID ) + TARGET_ID_POS;
00143          uint8_t DIMINISH_SECS_POS = sizeof( TimesNumber ) + START_TIME_POS;
00144          uint8_t DIMINISH_AMOUNT_POS = sizeof( SecondsNumber ) + DIMINISH_SECS_POS;
00145          uint8_t SPREAD_PENALTY_POS = sizeof( IntensityNumber ) + DIMINISH_AMOUNT_POS;
00146          uint8_t INTENSITY_POS = sizeof( IntensityNumber ) + SPREAD_PENALTY_POS;
00147          return INTENSITY_POS + sizeof( IntensityNumber );
00148       }
00149       inline self_type& operator=( const self_type& _t)
00150       {
00151          trace_data = _t.trace_data;
00152          inhibited = _t.inhibited;
00153          return *this;
00154       }
00155       inline void update_intensity_diminish()
00156       {
00157          trace_data.intensity = trace_data.intensity - trace_data.diminish_amount;
00158          if ( trace_data.intensity < 0 )
00159          { trace_data.intensity = 0; }
00160       }
00161       inline void update_intensity_penalize()
00162       {
00163          trace_data.intensity = trace_data.intensity - trace_data.spread_penalty;
00164          if ( trace_data.intensity < 0 )
00165          { trace_data.intensity = 0; }
00166       }
00167       inline void update_path( const Node& _n )
00168       {
00169          trace_data.grandparent = trace_data.parent;
00170          trace_data.parent = trace_data.current;
00171          trace_data.current = _n;
00172       }
00173       inline void update_start_time()  { trace_data.start_time = trace_data.start_time + 1; }
00174       inline IntensityNumber get_diminish_amount() { return trace_data.diminish_amount; }
00175       inline SecondsNumber get_diminish_seconds(){ return trace_data.diminish_seconds; }
00176       inline IntensityNumber get_intensity(){ return trace_data.intensity; }
00177       inline NodeID get_target_id(){ return trace_data.target_id; }
00178       inline Node get_current(){ return trace_data.current; }
00179       inline Node get_parent(){ return trace_data.parent; }
00180       inline Node get_grandparent(){ return trace_data.grandparent; }
00181       inline NodeID get_random_id(){ return random_id; }
00182       inline NodeID get_furthest_id(){ return furthest_id; }
00183       inline PLTT_TraceData get_trace_data(){ return trace_data; }
00184       inline SecondsNumber get_inhibited() { return inhibited; }
00185       inline TimesNumber get_start_time() { return trace_data.start_time; }
00186       inline IntensityNumber get_spread_penalty() { return trace_data.spread_penalty; }
00187       inline Node get_repulsion_point()
00188       {
00189          if ( trace_data.grandparent.get_id() != 0 ) { return trace_data.grandparent; }
00190          if ( trace_data.parent.get_id() != 0 ) { return trace_data.parent; }
00191          return trace_data.current;
00192       }
00193       inline void set_target_id( const NodeID& tarid ){ trace_data.target_id = tarid; }
00194       inline void set_inhibited(){ inhibited = inhibited + 1;  }
00195       inline void set_inhibited( const SecondsNumber& n ){ inhibited = n; }
00196       inline void set_start_time( const TimesNumber& t ) { trace_data.start_time = t; }
00197       inline void set_random_id( const NodeID& r_id ){ random_id = r_id; }
00198       inline void set_current( const Node& _c ){ trace_data.current = _c; }
00199       inline void set_parent( const Node& _p ){ trace_data.parent = _p; }
00200       inline void set_grandparent( const Node& _g ){ trace_data.grandparent = _g; }
00201       inline void set_furthest_id( const NodeID& f_id ){ furthest_id = f_id; }
00202       inline void print( Debug& debug )
00203       {
00204          debug.debug( "Trace (size %i) :", get_buffer_size() );
00205          debug.debug( "diminish_seconds (size %i) : %i", sizeof( trace_data.diminish_seconds ), trace_data.diminish_seconds );
00206          debug.debug( "diminish_amount (size %i) : %i", sizeof( trace_data.diminish_amount ), trace_data.diminish_amount );
00207          debug.debug( "spread_penalty (size %i) : %i", sizeof( trace_data.spread_penalty ), trace_data.spread_penalty );
00208          debug.debug( "intensity (size %i) : %i", sizeof( trace_data.intensity ), trace_data.intensity );
00209          debug.debug( "start_time (size %i) : %i", sizeof( trace_data.start_time ), trace_data.start_time  );
00210          debug.debug( "target id (size %i) : %x", sizeof( trace_data.target_id ), trace_data.target_id );
00211          trace_data.current.print( debug );
00212          trace_data.parent.print( debug );
00213          trace_data.grandparent.print( debug );
00214          debug.debug( "inhibited (size %i) : %i", sizeof( inhibited ), inhibited );
00215       }
00216       // --------------------------------------------------------------------
00217    private:
00218       PLTT_TraceData trace_data;
00219       uint8_t inhibited;
00220       NodeID random_id;
00221       NodeID furthest_id;
00222    };
00223 }
00224 #endif