vpTriangle.cpp

/****************************************************************************
 *
 * 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:
 * Defines a 2D triangle.
 *
 * Author:
 * Amaury Dame
 * Nicolas Melchior
 *
 *****************************************************************************/

#include <visp3/core/vpDebug.h>
#include <visp3/core/vpTriangle.h>

vpTriangle::vpTriangle()
  : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0), ptempo0(0), ptempo1(0), area(0), apex1(),
    apex2(), apex3()
{
  init(vpImagePoint(0, 0), vpImagePoint(1, 0), vpImagePoint(0, 1));
}

vpTriangle::vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
  : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0), ptempo0(0), ptempo1(0), area(0), apex1(),
    apex2(), apex3()
{
  init(iP1, iP2, iP3);
}

vpTriangle::vpTriangle(const vpTriangle &tri)
  : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0), ptempo0(0), ptempo1(0), area(0), apex1(),
    apex2(), apex3()
{
  *this = tri;
}

vpTriangle::~vpTriangle() {}

vpTriangle &vpTriangle::operator=(const vpTriangle &tri)
{
  goodTriange = tri.goodTriange;
  S1 = tri.S1;
  uvinv00 = tri.uvinv00;
  uvinv01 = tri.uvinv01;
  uvinv10 = tri.uvinv10;
  uvinv11 = tri.uvinv11;
  ptempo0 = tri.ptempo0;
  ptempo1 = tri.ptempo1;
  area = tri.area;
  apex1 = tri.apex1;
  apex2 = tri.apex2;
  apex3 = tri.apex3;
  return *this;
};

void vpTriangle::buildFrom(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
{
  init(iP1, iP2, iP3);
}

void vpTriangle::init(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
{
  ptempo0 = ptempo1 = 0.;
  apex1 = iP1;
  apex2 = iP2;
  apex3 = iP3;

  vpMatrix uv(2, 2);
  vpMatrix uvinv(2, 2);

  uv[0][0] = iP2.get_i() - iP1.get_i();
  uv[1][0] = iP3.get_i() - iP1.get_i();
  uv[0][1] = iP2.get_j() - iP1.get_j();
  uv[1][1] = iP3.get_j() - iP1.get_j();
  try {
    uvinv = uv.inverseByLU();
    goodTriange = true;
  } catch (...) {
    goodTriange = false;
    std::cout << "Empty triangle" << std::endl;
  }

  uvinv00 = uvinv[0][0];
  uvinv01 = uvinv[0][1];
  uvinv10 = uvinv[1][0];
  uvinv11 = uvinv[1][1];
  S1 = iP1;
  area = 0.5 * fabs(uv.det());
}

bool vpTriangle::inTriangle(const vpImagePoint &iP, double threshold)
{
  if (!goodTriange)
    return false;

  if (threshold < 0)
    threshold = 0;

  ptempo0 = iP.get_i() - S1.get_i();
  ptempo1 = iP.get_j() - S1.get_j();

  double p_ds_uv0 = ptempo0 * uvinv00 + ptempo1 * uvinv10;
  double p_ds_uv1 = ptempo0 * uvinv01 + ptempo1 * uvinv11;

  return (p_ds_uv0 + p_ds_uv1 < 1. + threshold && p_ds_uv0 > -threshold && p_ds_uv1 > -threshold);
}