/////////////////////////////////////////////////////////////
//                                                         //
// 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 "ConvexPolyhedron.h"

ConvexPolyhedron::ConvexPolyhedron()
{}

/*!
  Construct polyhedron - without planes its just a box 

  \param pmin minimum point of bounding box
  \param pmax maximum point of bounding box
*/
ConvexPolyhedron::ConvexPolyhedron(const Vector3& pmin,const Vector3& pmax) 
  : BoxWithPlanes3D(pmin,pmax)
{}

ConvexPolyhedron::~ConvexPolyhedron()
{}

/*!
  get point inside the box. The Argument is ignored
*/
Vector3 ConvexPolyhedron::getAPoint(int)const
{
  double px,py,pz;
  Vector3 res;
  
  //  std::cout << m_pmin << " - " << m_pmax << std::endl;
  do{
    px=m_random(m_pmin.x(),m_pmax.x());
    py=m_random(m_pmin.y(),m_pmax.y());
    pz=m_random(m_pmin.z(),m_pmax.z());
    res=Vector3(px,py,pz);
  } while (!isIn(res));
  return res;
}

/*!
  check if point is inside

  \param p the point
*/
bool ConvexPolyhedron::isIn(const Vector3& p) const
{
  bool res;
  // std::cout << p << std::endl;
  // check inside bounding box
  res= ((p.x()>m_pmin.x()) && (p.x()<m_pmax.x()) && 
	(p.y()>m_pmin.y()) && (p.y()<m_pmax.y()) &&
	(p.z()>m_pmin.z()) && (p.z()<m_pmax.z()));

  // std::cout << "inside bbx : " << res << std::endl;
  // if inside, check against planes
  if(res){
    vector<Plane>::const_iterator iter=m_planes.begin();
  
    while((iter!=m_planes.end()) && (res)){
      Vector3 normal=iter->getNormal();
      Vector3 pdist=p-(iter->getOrig());
      res=(normal*pdist>0);
      iter++;
    }
  }
  return res;
}

/*!
  check if a Sphere is inside and doesn't intersect any planes

  \param S the Sphere
*/
bool ConvexPolyhedron::isIn(const Sphere& S)
{
  bool res;

  double r=S.Radius();
  Vector3 p=S.Center();

  //  std::cout << "rad: " << r << "  pos: " << p << std::endl;
  // inside bbx
  res=(p.X()>m_pmin.X()+r) && (p.X()<m_pmax.X()-r) && 
    (p.Y()>m_pmin.Y()+r) && (p.Y()<m_pmax.Y()-r) &&
    (p.Z()>m_pmin.Z()+r) && (p.Z()<m_pmax.Z()-r);
  
  // if inside, check against planes
  if(res){
    vector<Plane>::const_iterator iter=m_planes.begin();
  
    while((iter!=m_planes.end()) && (res)){
      Vector3 normal=iter->getNormal();
      Vector3 pdist=p-(iter->getOrig());
      res=(normal*pdist>r);
      iter++;
    }
  }

  return res;
}
