Visual Servoing Platform version 3.5.0
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
73class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not
74 // be added since this class is complete
75 // inline
76{
77public:
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
198private:
199 double rDist;
200 double hAngle;
201 double vAngle;
202};
203
241inline 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
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Class that defines a single laser scanner point.
Definition: vpScanPoint.h:76
double getX() const
Definition: vpScanPoint.h:132
virtual ~vpScanPoint()
Definition: vpScanPoint.h:100
friend bool operator!=(const vpScanPoint &sp1, const vpScanPoint &sp2)
Definition: vpScanPoint.h:182
friend std::ostream & operator<<(std::ostream &s, const vpScanPoint &p)
Definition: vpScanPoint.h:241
vpScanPoint(double r_dist, double h_angle, double v_angle)
Definition: vpScanPoint.h:93
double getVAngle() const
Definition: vpScanPoint.h:120
double getZ() const
Definition: vpScanPoint.h:147
void setPolar(double r_dist, double h_angle, double v_angle)
Definition: vpScanPoint.h:107
double getY() const
Definition: vpScanPoint.h:140
vpScanPoint & operator=(const vpScanPoint &)=default
vpScanPoint(const vpScanPoint &scanpoint)
Definition: vpScanPoint.h:81
double getRadialDist() const
Definition: vpScanPoint.h:116
double getHAngle() const
Definition: vpScanPoint.h:124
friend bool operator==(const vpScanPoint &sp1, const vpScanPoint &sp2)
Definition: vpScanPoint.h:160