Fawkes API  Fawkes Development Version
circle_sector.cpp
1 
2 /***************************************************************************
3  * circle_sector.cpp - Filter laser data for circle sector
4  *
5  * Created: Sat Feb 19 00:28:41 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "circle_sector.h"
24 
25 #include <core/exception.h>
26 #include <utils/math/angle.h>
27 #include <algorithm>
28 #include <cstring>
29 
30 using namespace fawkes;
31 
32 /** @class LaserCircleSectorDataFilter "circle.h"
33  * Erase beams outside specified circle sector.
34  * Only data inside the specified circle sector is copied, all other data is
35  * set to zero.
36  * @author Tim Niemueller
37  */
38 
39 /** Constructor.
40  * @param from start angle (index in data)
41  * @param to end angle (index in data)
42  * @param in_data_size number of entries in value arrays
43  * @param in vector of input arrays
44  */
46  unsigned int to,
47  unsigned int in_data_size,
48  std::vector<LaserDataFilter::Buffer *> &in)
49  : LaserDataFilter(in_data_size, in, in.size())
50 {
51  __from = from;
52  __to = to;
53 }
54 
55 
56 void
58 {
59  const unsigned int vecsize = std::min(in.size(), out.size());
60  const unsigned int arrsize = std::min(in_data_size, out_data_size);
61  for (unsigned int a = 0; a < vecsize; ++a) {
62 
63  reset_outbuf(out[a]);
64  out[a]->frame = in[a]->frame;
65 
66  float *inbuf = in[a]->values;
67  float *outbuf = out[a]->values;
68 
69  if (__from > __to) {
70  for (unsigned int i = __from; i < arrsize; ++i) {
71  outbuf[i] = inbuf[i];
72  }
73  for (unsigned int i = 0; i <= std::min(__to, arrsize-1); ++i) {
74  outbuf[i] = inbuf[i];
75  }
76  } else {
77  for (unsigned int i = __from; i <= std::min(__to, arrsize-1); ++i) {
78  outbuf[i] = inbuf[i];
79  }
80  }
81  }
82 }
std::vector< Buffer * > out
Vector of output arrays.
Definition: filter.h:67
Fawkes library namespace.
void filter()
Filter the incoming data.
LaserCircleSectorDataFilter(unsigned int from, unsigned int to, unsigned int data_size, std::vector< LaserDataFilter::Buffer * > &in)
Constructor.
void reset_outbuf(Buffer *b)
Resets all readings in outbuf to 0.0.
Definition: filter.cpp:180
unsigned int out_data_size
Number of entries in output arrays.
Definition: filter.h:64
std::vector< Buffer * > in
Vector of input arrays.
Definition: filter.h:66
unsigned int in_data_size
Number of entries in input arrays.
Definition: filter.h:65
Laser data filter.
Definition: filter.h:28