/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2007-2017 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.apache.org/licenses/LICENSE-2.0              //
//                                                         //
/////////////////////////////////////////////////////////////

#include "Triangle3D.h"

#include <iostream>
#include <utility>

using std::pair;
using std::make_pair;



/*
  modified 3x3 equation system solver - only 2 components needed
*/
pair<double,double> rsolve(Vector3& v1,Vector3& v2,Vector3& v3,Vector3& rhs)
{
  double det1,detx,dety;

  det1=v1.X()*(v2.Y()*v3.Z()-v3.Y()*v2.Z())+v2.X()*(v3.Y()*v1.Z()-v1.Y()*v3.Z())+v3.X()*(v1.Y()*v2.Z()-v2.Y()*v1.Z());

  detx=rhs.X()*(v2.Y()*v3.Z()-v3.Y()*v2.Z())+v2.X()*(v3.Y()*rhs.Z()-rhs.Y()*v3.Z())+v3.X()*(rhs.Y()*v2.Z()-v2.Y()*rhs.Z());
  dety=v1.X()*(rhs.Y()*v3.Z()-v3.Y()*rhs.Z())+rhs.X()*(v3.Y()*v1.Z()-v1.Y()*v3.Z())+v3.X()*(v1.Y()*rhs.Z()-rhs.Y()*v1.Z());

  
  double x=detx/det1;
  double y=dety/det1;

  return make_pair(x,y);
}

Triangle3D::Triangle3D(const Vector3& p1,const Vector3& p2,const Vector3& p3, int tag)
  :m_p1(p1),m_p2(p2),m_p3(p3),m_tag(tag)
{}
  
/*!
  check if line between 2 points intersects the triangle

  \param p1 point 1
  \param p2 point 2
*/
bool Triangle3D::crosses(const Vector3& p1, const Vector3& p2) const
{
  bool res=false;

  //-- check if points on same side of plane
  Vector3 plane_normal=cross((m_p2-m_p1),(m_p3-m_p1));
  //std::cout << "normal: " << plane_normal << std::endl;

  double d1=plane_normal*(p1-m_p1);
  double d2=plane_normal*(p2-m_p1);
  //std::cout << "d1,d2 " << d1 << " / " << d2 << std::endl;
  
  if(d1*d2<0.0){ // sign(d1)!=sign(d2) -> different sides
    
    Vector3 v1=m_p2-m_p1;
    Vector3 v2=m_p3-m_p1;
    Vector3 v3=p2-p1;
    Vector3 rhs=p1-m_p1;

    pair<double,double> p=rsolve(v1,v2,v3,rhs);
    
    res=((p.first>=0.0) && (p.first<=1.0) && 
	 (p.second>=0.0) && (p.second<=1.0) && 
	 (p.first+p.second<=1.0));
  }

  return res;
}

/*!
  get minimum corner of the bounding box
*/
Vector3 Triangle3D::getMinPoint() const
{
  double xmin=(m_p1.x() < m_p2.x()) ? m_p1.x() : m_p2.x();
  xmin = (xmin < m_p3.x()) ? xmin : m_p3.x();
  double ymin=(m_p1.y() < m_p2.y()) ? m_p1.y() : m_p2.y();
  ymin = (ymin < m_p3.y()) ? ymin : m_p3.y();
  double zmin=(m_p1.z() < m_p2.z()) ? m_p1.z() : m_p2.z();
  zmin = (zmin < m_p3.z()) ? zmin : m_p3.z();

  return Vector3(xmin,ymin,zmin);
}

/*!
  get maximum corner of the bounding box
*/
Vector3 Triangle3D::getMaxPoint() const
{
  double xmax=(m_p1.x() > m_p2.x()) ? m_p1.x() : m_p2.x();
  xmax = (xmax > m_p3.x()) ? xmax : m_p3.x();
  double ymax=(m_p1.y() > m_p2.y()) ? m_p1.y() : m_p2.y();
  ymax = (ymax > m_p3.y()) ? ymax : m_p3.y();
  double zmax=(m_p1.z() > m_p2.z()) ? m_p1.z() : m_p2.z();
  zmax = (zmax > m_p3.z()) ? zmax : m_p3.z();

  return Vector3(xmax,ymax,zmax);
}


/*
  local helper function to calculate dist between line segment and point
*/
double EdgeSep(const Vector3& p0,const Vector3& p1,const Vector3& p) 
{
  double sep;

  Vector3 v=p1-p0;
  Vector3 vu=v.unit();
  double d=((p-p0)*vu);
  if((d>0.0)&(d<v.norm())){
    sep=((p-p0)-d*vu).norm();
  }else{
    sep=-1;
  }
  return sep;
}

/*!
  Get the distance of a point from the Triangle

  \param p the point
*/
double Triangle3D::getDist(const Vector3& p) const
{
  double dist=-1.0;


  // check if closest point is in triangle
  // calc. the closest point in triangle local coords.
  Vector3 v1=m_p2-m_p1; // 1-2 edge of the triangle
  Vector3 v2=m_p3-m_p1; // 1-3 edge of the triangle
  Vector3 normal=cross(v1,v2).unit(); // plane normal
  Vector3 rhs=p-m_p1; // vector from point 1 of the triangle to input point

  pair<double,double> cp=rsolve(v2,v1,normal,rhs);
  if((cp.first>=0.0) && (cp.first<=1.0) && 
     (cp.second>=0.0) && (cp.second<=1.0) && 
     (cp.first+cp.second<=1.0)){ // point inside 
    dist=fabs((p-m_p1)*normal);
  } else { // need to check distance to edges/corners 
    double d1=EdgeSep(m_p1,m_p2,p);
    double d2=EdgeSep(m_p1,m_p3,p);
    double d3=EdgeSep(m_p2,m_p3,p);
    // find the minimum separation != -1 (messy)
    if(d1>0.0){
      if(d2>0.0){
	dist=(d1<d2) ? d1 : d2;
	if(d3>0.0){
	  dist=(d3<dist) ? d3 : dist; 
	}
      } else if(d3>0){
	dist=(d1<d3) ? d1 : d3;
      } else {
	dist=d1;
      }
    } else if (d2>0){
      if (d3>0){
	dist=(d2<d3) ? d2 : d3;
      } else {
	dist=d2;
      }
    } else {
      dist=d3;
    }
    if(dist==-1.0){ // no edge-> get corner dist
      d1=(p-m_p1).norm();
      d2=(p-m_p2).norm();
      d3=(p-m_p3).norm();
      dist=(d1<d2) ? d1 : d2;
      dist=(dist<d3) ? dist : d3;
    }
  }

  //std::cout << "Triangle dist: [" << m_p1 << " " << m_p2 << " " << m_p3 << " ] " << p << " - " << dist << std::endl;
  return dist;
}
