Fawkes API  Fawkes Development Version
circle_sector.cpp
00001 
00002 /***************************************************************************
00003  *  circle_sector.cpp - Filter laser data for circle sector
00004  *
00005  *  Created: Sat Feb 19 00:28:41 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "circle_sector.h"
00024 
00025 #include <core/exception.h>
00026 #include <utils/math/angle.h>
00027 #include <algorithm>
00028 #include <cstring>
00029 
00030 using namespace fawkes;
00031 
00032 /** @class LaserCircleSectorDataFilter "circle.h"
00033  * Erase beams outside specified circle sector.
00034  * Only data inside the specified circle sector is copied, all other data is
00035  * set to zero.
00036  * @author Tim Niemueller
00037  */
00038 
00039 /** Constructor.
00040  * @param from start angle (index in data)
00041  * @param to end angle (index in data)
00042  * @param in_data_size number of entries in value arrays
00043  * @param in vector of input arrays
00044  */
00045 LaserCircleSectorDataFilter::LaserCircleSectorDataFilter(unsigned int from,
00046                                                          unsigned int to,
00047                                                          unsigned int in_data_size,
00048                                                          std::vector<LaserDataFilter::Buffer *> &in)
00049   : LaserDataFilter(in_data_size, in, in.size())
00050 {
00051   __from = from;
00052   __to   = to;
00053 }
00054 
00055 
00056 void
00057 LaserCircleSectorDataFilter::filter()
00058 {
00059   const unsigned int vecsize = std::min(in.size(), out.size());
00060   const unsigned int arrsize = std::min(in_data_size, out_data_size);
00061   for (unsigned int a = 0; a < vecsize; ++a) {
00062 
00063     reset_outbuf(out[a]);
00064     out[a]->frame = in[a]->frame;
00065 
00066     float *inbuf  = in[a]->values;
00067     float *outbuf = out[a]->values;
00068 
00069     if (__from > __to) {
00070       for (unsigned int i = __from; i < arrsize; ++i) {
00071         outbuf[i] = inbuf[i];
00072       }
00073       for (unsigned int i = 0; i <= std::min(__to, arrsize-1); ++i) {
00074         outbuf[i] = inbuf[i];
00075       }
00076     } else {
00077       for (unsigned int i = __from; i <= std::min(__to, arrsize-1); ++i) {
00078         outbuf[i] = inbuf[i];
00079       }
00080     }
00081   }
00082 }