vpScanPoint.h

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Single laser scanner point.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/
#ifndef vpScanPoint_h
#define vpScanPoint_h

#include <visp3/core/vpMath.h>

#include <cmath>  // std::fabs
#include <limits> // numeric_limits
#include <math.h>
#include <ostream>
#include <sstream>

class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not
                                    // be added since this class is complete
                                    // inline
{
public:
  inline vpScanPoint() : rDist(0), hAngle(0), vAngle(0) {}
  inline vpScanPoint(const vpScanPoint &scanpoint) : rDist(0), hAngle(0), vAngle(0)
  {
    this->rDist = scanpoint.rDist;
    this->hAngle = scanpoint.hAngle;
    this->vAngle = scanpoint.vAngle;
  }
  inline vpScanPoint(double r_dist, double h_angle, double v_angle) : rDist(r_dist), hAngle(h_angle), vAngle(v_angle)
  {
    this->rDist = r_dist;
    this->hAngle = h_angle;
    this->vAngle = v_angle;
  }
  inline virtual ~vpScanPoint(){};
  inline void setPolar(double r_dist, double h_angle, double v_angle)
  {
    this->rDist = r_dist;
    this->hAngle = h_angle;
    this->vAngle = v_angle;
  }
  inline double getRadialDist() const { return (this->rDist); }
  inline double getVAngle() const { return (this->vAngle); }
  inline double getHAngle() const { return (this->hAngle); }
  inline double getX() const { return (rDist * cos(this->hAngle) * cos(this->vAngle)); }
  inline double getY() const { return (rDist * sin(this->hAngle)); }
  inline double getZ() const { return (rDist * cos(this->hAngle) * sin(this->vAngle)); }

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
  vpScanPoint &operator=(const vpScanPoint &) = default;
#endif

  friend inline std::ostream &operator<<(std::ostream &s, const vpScanPoint &p);

  friend inline bool operator==(const vpScanPoint &sp1, const vpScanPoint &sp2)
  {
    // return ( ( sp1.getRadialDist() == sp2.getRadialDist() )
    //        && ( sp1.getHAngle() == sp2.getHAngle() )
    //        && ( sp1.getVAngle() == sp2.getVAngle() ) );
    double rd1 = sp1.getRadialDist();
    double ha1 = sp1.getHAngle();
    double va1 = sp1.getVAngle();
    double rd2 = sp2.getRadialDist();
    double ha2 = sp2.getHAngle();
    double va2 = sp2.getVAngle();

    return ((std::fabs(rd1 - rd2) <= std::fabs(vpMath::maximum(rd1, rd2)) * std::numeric_limits<double>::epsilon()) &&
            (std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1, ha2)) * std::numeric_limits<double>::epsilon()) &&
            (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * std::numeric_limits<double>::epsilon()));
  }

  friend inline bool operator!=(const vpScanPoint &sp1, const vpScanPoint &sp2)
  {
    // return ( ( sp1.getRadialDist() != sp2.getRadialDist() )
    //     || ( sp1.getHAngle() != sp2.getHAngle() )
    //     || ( sp1.getVAngle() != sp2.getVAngle() ) );
    double rd1 = sp1.getRadialDist();
    double ha1 = sp1.getHAngle();
    double va1 = sp1.getVAngle();
    double rd2 = sp2.getRadialDist();
    double ha2 = sp2.getHAngle();
    double va2 = sp2.getVAngle();
    return ((std::fabs(rd1 - rd2) > std::fabs(vpMath::maximum(rd1, rd2)) * std::numeric_limits<double>::epsilon()) ||
            (std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1, ha2)) * std::numeric_limits<double>::epsilon()) ||
            (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * std::numeric_limits<double>::epsilon()));
  }

private:
  double rDist;
  double hAngle;
  double vAngle;
};

inline std::ostream &operator<<(std::ostream &s, const vpScanPoint &p)
{
  std::ios_base::fmtflags original_flags = s.flags();

  s.precision(10);
  s << p.getRadialDist() << " " << p.getHAngle() << " " << p.getVAngle() << " " << p.getX() << " " << p.getY() << " "
    << p.getZ();

  s.setf(original_flags); // restore s to standard state

  return s;
}

#endif