vpTriangle.cpp

/****************************************************************************
 *
 * $Id: vpTriangle.cpp 4632 2014-02-03 17:06:40Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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 <visp/vpTriangle.h>
#include <visp/vpDebug.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;
  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);
}