Fawkes API
Fawkes Development Version
|
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 }