Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpScanPoint.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Single laser scanner point.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 #ifndef vpScanPoint_h
39 #define vpScanPoint_h
40 
41 #include <visp3/core/vpMath.h>
42 
43 #include <cmath> // std::fabs
44 #include <limits> // numeric_limits
45 #include <math.h>
46 #include <ostream>
47 #include <sstream>
48 
73 class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not
74  // be added since this class is complete
75  // inline
76 {
77 public:
79  inline vpScanPoint() : rDist(0), hAngle(0), vAngle(0) {}
81  inline vpScanPoint(const vpScanPoint &scanpoint) : rDist(0), hAngle(0), vAngle(0)
82  {
83  this->rDist = scanpoint.rDist;
84  this->hAngle = scanpoint.hAngle;
85  this->vAngle = scanpoint.vAngle;
86  }
93  inline vpScanPoint(double r_dist, double h_angle, double v_angle) : rDist(r_dist), hAngle(h_angle), vAngle(v_angle)
94  {
95  this->rDist = r_dist;
96  this->hAngle = h_angle;
97  this->vAngle = v_angle;
98  }
100  inline virtual ~vpScanPoint(){};
107  inline void setPolar(double r_dist, double h_angle, double v_angle)
108  {
109  this->rDist = r_dist;
110  this->hAngle = h_angle;
111  this->vAngle = v_angle;
112  }
116  inline double getRadialDist() const { return (this->rDist); }
120  inline double getVAngle() const { return (this->vAngle); }
124  inline double getHAngle() const { return (this->hAngle); }
132  inline double getX() const { return (rDist * cos(this->hAngle) * cos(this->vAngle)); }
140  inline double getY() const { return (rDist * sin(this->hAngle)); }
147  inline double getZ() const { return (rDist * cos(this->hAngle) * sin(this->vAngle)); }
148 
149 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
150  vpScanPoint &operator=(const vpScanPoint &) = default;
151 #endif
152 
153  friend inline std::ostream &operator<<(std::ostream &s, const vpScanPoint &p);
154 
160  friend inline bool operator==(const vpScanPoint &sp1, const vpScanPoint &sp2)
161  {
162  // return ( ( sp1.getRadialDist() == sp2.getRadialDist() )
163  // && ( sp1.getHAngle() == sp2.getHAngle() )
164  // && ( sp1.getVAngle() == sp2.getVAngle() ) );
165  double rd1 = sp1.getRadialDist();
166  double ha1 = sp1.getHAngle();
167  double va1 = sp1.getVAngle();
168  double rd2 = sp2.getRadialDist();
169  double ha2 = sp2.getHAngle();
170  double va2 = sp2.getVAngle();
171 
172  return ((std::fabs(rd1 - rd2) <= std::fabs(vpMath::maximum(rd1, rd2)) * std::numeric_limits<double>::epsilon()) &&
173  (std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1, ha2)) * std::numeric_limits<double>::epsilon()) &&
174  (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * std::numeric_limits<double>::epsilon()));
175  }
176 
182  friend inline bool operator!=(const vpScanPoint &sp1, const vpScanPoint &sp2)
183  {
184  // return ( ( sp1.getRadialDist() != sp2.getRadialDist() )
185  // || ( sp1.getHAngle() != sp2.getHAngle() )
186  // || ( sp1.getVAngle() != sp2.getVAngle() ) );
187  double rd1 = sp1.getRadialDist();
188  double ha1 = sp1.getHAngle();
189  double va1 = sp1.getVAngle();
190  double rd2 = sp2.getRadialDist();
191  double ha2 = sp2.getHAngle();
192  double va2 = sp2.getVAngle();
193  return ((std::fabs(rd1 - rd2) > std::fabs(vpMath::maximum(rd1, rd2)) * std::numeric_limits<double>::epsilon()) ||
194  (std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1, ha2)) * std::numeric_limits<double>::epsilon()) ||
195  (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * std::numeric_limits<double>::epsilon()));
196  }
197 
198 private:
199  double rDist;
200  double hAngle;
201  double vAngle;
202 };
203 
241 inline std::ostream &operator<<(std::ostream &s, const vpScanPoint &p)
242 {
243  std::ios_base::fmtflags original_flags = s.flags();
244 
245  s.precision(10);
246  s << p.getRadialDist() << " " << p.getHAngle() << " " << p.getVAngle() << " " << p.getX() << " " << p.getY() << " "
247  << p.getZ();
248 
249  s.setf(original_flags); // restore s to standard state
250 
251  return s;
252 }
253 
254 #endif
double getRadialDist() const
Definition: vpScanPoint.h:116
double getHAngle() const
Definition: vpScanPoint.h:124
virtual ~vpScanPoint()
Definition: vpScanPoint.h:100
vpScanPoint(const vpScanPoint &scanpoint)
Definition: vpScanPoint.h:81
vpScanPoint(double r_dist, double h_angle, double v_angle)
Definition: vpScanPoint.h:93
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
double getZ() const
Definition: vpScanPoint.h:147
Class that defines a single laser scanner point.
Definition: vpScanPoint.h:73
friend std::ostream & operator<<(std::ostream &s, const vpScanPoint &p)
Definition: vpScanPoint.h:241
friend bool operator==(const vpScanPoint &sp1, const vpScanPoint &sp2)
Definition: vpScanPoint.h:160
double getVAngle() const
Definition: vpScanPoint.h:120
double getX() const
Definition: vpScanPoint.h:132
vpScanPoint & operator=(const vpScanPoint &)=default
void setPolar(double r_dist, double h_angle, double v_angle)
Definition: vpScanPoint.h:107
double getY() const
Definition: vpScanPoint.h:140
friend bool operator!=(const vpScanPoint &sp1, const vpScanPoint &sp2)
Definition: vpScanPoint.h:182