vpRectOriented.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 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 https://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:
 * Defines a (possibly oriented) rectangle in the plane.
 */
#include <visp3/core/vpRectOriented.h>
 
#include <cmath>
 
BEGIN_VISP_NAMESPACE
vpRectOriented::vpRectOriented()
  : m_center(), m_width(), m_height(), m_theta(), m_topLeft(), m_topRight(), m_bottomLeft(), m_bottomRight()
{ }

vpRectOriented::vpRectOriented(const vpImagePoint &center, double width, double height, double theta)
{
  m_center = center;
  m_width = width;
  m_height = height;
  m_theta = theta;
  m_topLeft.set_i(m_center.get_i() - ((m_height * cos(m_theta)) / 2.0) - ((m_width * sin(m_theta)) / 2.0));
  m_topLeft.set_j((m_center.get_j() + ((m_height * sin(m_theta)) / 2.0)) - ((m_width * cos(m_theta)) / 2.0));
  m_bottomLeft.set_i((m_center.get_i() + ((m_height * cos(m_theta)) / 2.0)) - ((m_width * sin(m_theta)) / 2.0));
  m_bottomLeft.set_j(m_center.get_j() - ((m_height * sin(m_theta)) / 2.0) - ((m_width * cos(m_theta)) / 2.0));
  m_bottomRight.set_i(m_center.get_i() + ((m_height * cos(m_theta)) / 2.0) + ((m_width * sin(m_theta)) / 2.0));
  m_bottomRight.set_j((m_center.get_j() - ((m_height * sin(m_theta)) / 2.0)) + ((m_width * cos(m_theta)) / 2.0));
  m_topRight.set_i((m_center.get_i() - ((m_height * cos(m_theta)) / 2.0)) + ((m_width * sin(m_theta)) / 2.0));
  m_topRight.set_j(m_center.get_j() + ((m_height * sin(m_theta)) / 2.0) + ((m_width * cos(m_theta)) / 2.0));
}

vpRectOriented::vpRectOriented(const vpRect &rect)
{
  m_center = rect.getCenter();
  m_width = rect.getWidth();
  m_height = rect.getHeight();
  m_theta = .0;
  m_topLeft.set_i(m_center.get_i() - (m_height / 2.0));
  m_topLeft.set_j(m_center.get_j() - (m_width / 2.0));
  m_bottomLeft.set_i(m_center.get_i() + (m_height / 2.0));
  m_bottomLeft.set_j(m_center.get_j() - (m_width / 2.0));
  m_bottomRight.set_i(m_center.get_i() + (m_height / 2.0));
  m_bottomRight.set_j(m_center.get_j() + (m_width / 2.0));
  m_topRight.set_i(m_center.get_i() - (m_height / 2.0));
  m_topRight.set_j(m_center.get_j() + (m_width / 2.0));
}
 
#if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98)
vpRectOriented::vpRectOriented(const vpRectOriented &rectOriented) { *this = rectOriented; }

vpRectOriented &vpRectOriented::operator=(const vpRectOriented &rectOriented)
{
  m_center = rectOriented.getCenter();
  m_width = rectOriented.getWidth();
  m_height = rectOriented.getHeight();
  m_theta = rectOriented.getOrientation();
  m_topLeft = rectOriented.getTopLeft();
  m_bottomLeft = rectOriented.getBottomLeft();
  m_bottomRight = rectOriented.getBottomRight();
  m_topRight = rectOriented.getTopRight();
  return *this;
}
#endif

vpRectOriented &vpRectOriented::operator=(const vpRect &rect)
{
  m_center = rect.getCenter();
  m_width = rect.getWidth();
  m_height = rect.getHeight();
  m_theta = .0;
  m_topLeft.set_i(m_center.get_i() - (m_height / 2.0));
  m_topLeft.set_j(m_center.get_j() - (m_width / 2.0));
  m_bottomLeft.set_i(m_center.get_i() + (m_height / 2.0));
  m_bottomLeft.set_j(m_center.get_j() - (m_width / 2.0));
  m_bottomRight.set_i(m_center.get_i() + (m_height / 2.0));
  m_bottomRight.set_j(m_center.get_j() + (m_width / 2.0));
  m_topRight.set_i(m_center.get_i() - (m_height / 2.0));
  m_topRight.set_j(m_center.get_j() + (m_width / 2.0));
  return *this;
}
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
vpRectOriented::operator vpRect()
{
  if (std::fabs(m_theta) > std::numeric_limits<double>::epsilon()) {
    throw(vpException(vpException::badValue, "Cannot convert a vpRectOriented with non-zero orientation to a vpRect"));
  }
 
  return vpRect(m_topLeft, m_bottomRight);
}
#endif

void vpRectOriented::setPoints(const vpImagePoint &topLeft, const vpImagePoint &topRight,
                               const vpImagePoint &bottomLeft, const vpImagePoint &bottomRight)
{
  m_topLeft = topLeft;
  m_bottomLeft = bottomLeft;
  m_bottomRight = bottomRight;
  m_topRight = topRight;
  m_center.set_i((m_topLeft.get_i() + m_bottomLeft.get_i() + m_bottomRight.get_i() + m_topRight.get_i()) / 4.0);
  m_center.set_j((m_topLeft.get_j() + m_bottomLeft.get_j() + m_bottomRight.get_j() + m_topRight.get_j()) / 4.0);
  m_width = sqrt(((m_topRight.get_i() - m_topLeft.get_i()) * (m_topRight.get_i() - m_topLeft.get_i())) +
                 ((m_topRight.get_j() - m_topLeft.get_j()) * (m_topRight.get_j() - m_topLeft.get_j())));
  m_height = sqrt(((m_bottomLeft.get_i() - m_topLeft.get_i()) * (m_bottomLeft.get_i() - m_topLeft.get_i())) +
                  ((m_bottomLeft.get_j() - m_topLeft.get_j()) * (m_bottomLeft.get_j() - m_topLeft.get_j())));
  m_theta = atan2(m_topRight.get_i() - m_topLeft.get_i(), m_topRight.get_j() - m_topLeft.get_j());
}

void vpRectOriented::setCenter(const vpImagePoint &center)
{
  m_topLeft += center - m_center;
  m_bottomLeft += center - m_center;
  m_bottomRight += center - m_center;
  m_topRight += center - m_center;
  m_center = center;
}

vpImagePoint vpRectOriented::getCenter() const { return m_center; }

vpImagePoint vpRectOriented::getTopLeft() const { return m_topLeft; }

vpImagePoint vpRectOriented::getTopRight() const { return m_topRight; }

vpImagePoint vpRectOriented::getBottomLeft() const { return m_bottomLeft; }

vpImagePoint vpRectOriented::getBottomRight() const { return m_bottomRight; }

void vpRectOriented::setSize(double width, double height)
{
  const int val_2 = 2;
  m_width = width;
  m_height = height;
  m_topLeft.set_i(m_center.get_i() - ((m_height * cos(m_theta)) / 2.0) - ((m_width * sin(m_theta)) / val_2));
  m_topLeft.set_j((m_center.get_j() + ((m_height * sin(m_theta)) / 2.0)) - ((m_width * cos(m_theta)) / val_2));
  m_bottomLeft.set_i((m_center.get_i() + ((m_height * cos(m_theta)) / 2.0)) - ((m_width * sin(m_theta)) / val_2));
  m_bottomLeft.set_j(m_center.get_j() - ((m_height * sin(m_theta)) / 2.0) - ((m_width * cos(m_theta)) / val_2));
  m_bottomRight.set_i(m_center.get_i() + ((m_height * cos(m_theta)) / 2.0) + ((m_width * sin(m_theta)) / val_2));
  m_bottomRight.set_j((m_center.get_j() - ((m_height * sin(m_theta)) / 2.0)) + ((m_width * cos(m_theta)) / val_2));
  m_topRight.set_i((m_center.get_i() - ((m_height * cos(m_theta)) / 2.0)) + ((m_width * sin(m_theta)) / val_2));
  m_topRight.set_j(m_center.get_j() + ((m_height * sin(m_theta)) / 2.0) + ((m_width * cos(m_theta)) / val_2));
}

double vpRectOriented::getWidth() const { return m_width; }

double vpRectOriented::getHeight() const { return m_height; }

void vpRectOriented::setOrientation(double theta)
{
  const int val_2 = 2;
  m_theta = theta;
  m_topLeft.set_i(m_center.get_i() - ((m_height * cos(m_theta)) / 2.0) - ((m_width * sin(m_theta)) / val_2));
  m_topLeft.set_j((m_center.get_j() + ((m_height * sin(m_theta)) / 2.0)) - ((m_width * cos(m_theta)) / val_2));
  m_bottomLeft.set_i((m_center.get_i() + ((m_height * cos(m_theta)) / 2.0)) - ((m_width * sin(m_theta)) / val_2));
  m_bottomLeft.set_j(m_center.get_j() - ((m_height * sin(m_theta)) / 2.0) - ((m_width * cos(m_theta)) / val_2));
  m_bottomRight.set_i(m_center.get_i() + ((m_height * cos(m_theta)) / 2.0) + ((m_width * sin(m_theta)) / val_2));
  m_bottomRight.set_j((m_center.get_j() - ((m_height * sin(m_theta)) / 2.0)) + ((m_width * cos(m_theta)) / val_2));
  m_topRight.set_i((m_center.get_i() - ((m_height * cos(m_theta)) / 2.0)) + ((m_width * sin(m_theta)) / val_2));
  m_topRight.set_j(m_center.get_j() + ((m_height * sin(m_theta)) / 2.0) + ((m_width * cos(m_theta)) / val_2));
}

double vpRectOriented::getOrientation() const { return m_theta; }

bool vpRectOriented::isInside(const vpImagePoint &point) const
{
  if (!isLeft(point, m_topLeft, m_bottomLeft)) {
    return false;
  }
  if (!isLeft(point, m_bottomLeft, m_bottomRight)) {
    return false;
  }
  if (!isLeft(point, m_bottomRight, m_topRight)) {
    return false;
  }
  if (!isLeft(point, m_topRight, m_topLeft)) {
    return false;
  }
  return true;
}
 
bool vpRectOriented::isLeft(const vpImagePoint &pointToTest, const vpImagePoint &point1,
                            const vpImagePoint &point2) const
{
  double a = point1.get_j() - point2.get_j();
  double b = point2.get_i() - point1.get_i();
  double c = -((a * point1.get_i()) + (b * point1.get_j()));
  double d = (a * pointToTest.get_i()) + (b * pointToTest.get_j()) + c;
  return (d > 0);
}
END_VISP_NAMESPACE