Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
reverse_angle.cpp
1 
2 /***************************************************************************
3  * reverse_angle.cpp - Reverse the angle in which laser data is taken
4  *
5  * Created: Wed Jan 06 17:15:38 2010
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 "reverse_angle.h"
24 
25 #include <core/exception.h>
26 #include <utils/math/angle.h>
27 #include <cstdlib>
28 
29 /** @class LaserReverseAngleDataFilter "reverse_angle.h"
30  * Reverse the angle of beams.
31  * This filter will reverse the direction in which the beams are stored.
32  * If the original interface stores the data in clockwise direction, the
33  * outcome will be in counter-clockwise direction and vice versa. This is
34  * required for example to convert between the (clockwise) RCSoftX angles,
35  * and the (counter-clockwise) angles in the Fawkes coordinate system.
36  * @author Tim Niemueller
37  */
38 
39 /** Constructor.
40  * @param in_data_size number of entries input value arrays
41  * @param in vector of input arrays
42  */
44  std::vector<LaserDataFilter::Buffer *> &in)
45  : LaserDataFilter(in_data_size, in, in.size())
46 {
47 }
48 
49 void
51 {
52  const unsigned int vecsize = std::min(in.size(), out.size());
53  const unsigned int arrsize = std::min(in_data_size, out_data_size);
54  for (unsigned int a = 0; a < vecsize; ++a) {
55  out[a]->frame = in[a]->frame;
56  float *inbuf = in[a]->values;
57  float *outbuf = out[a]->values;
58  for (unsigned int i = 0; i < arrsize; ++i) {
59  outbuf[i] = inbuf[arrsize - i];
60  }
61  }
62 }