#include "qplane3d.h"

#include <cmath>


namespace LtsUtils
{





QPlane3D::QPlane3D(const QPoint3D &pos, const QPoint3D &vec1, const QPoint3D &vec2)
  : m_pos(pos)
  , m_vec1(vec1)
  , m_vec2(vec2)
{
  m_normal = QPoint3D::crossProduct(m_vec1, m_vec2);
  m_planeC = QPoint3D::dotProduct(m_pos, m_normal);
}


QPoint3D QPlane3D::intersection(const QPoint3D &point, const QPoint3D &direction) const
{
  const double dotProduct = QPoint3D::dotProduct(direction, m_normal);
  if (qFuzzyIsNull(dotProduct))
    return inPlane(point) ? point : QPoint3D();
  const double distance = distanceTo(point) / std::abs(dotProduct);
  return point + distance * direction;
}

bool QPlane3D::intersects(const QPoint3D &direction) const
{
  return !isParallel(direction);
}

bool QPlane3D::isParallel(const QPoint3D &direction) const
{
  return qFuzzyIsNull(QPoint3D::dotProduct(m_normal, direction));
}

bool QPlane3D::inPlane(const QPoint3D &point) const
{
  if (point == m_pos)
    return true;
  return qFuzzyIsNull(QPoint3D::dotProduct(m_normal, point - m_pos));
}

double QPlane3D::distanceTo(const QPoint3D &point) const
{
  return QPoint3D::dotProduct(point - m_pos, m_normal);
}






QPolygon3D::QPolygon3D(const QVector<QPoint3D> &vertices)
  : QPlane3D(vertices[0], vertices[1], vertices[2])
  , m_vertices(vertices)
{
}


bool QPolygon3D::containsPoint(const QPoint3D &point) const
{
  if (!inPlane(point))
    return false;

  if (contains(point))
    return true;

  double angleSum = 0.;
  double cosTheta = 0.;
  const int numberOfVertices = m_vertices.size();
  for (int i = 0; i < m_vertices.size(); ++i)
  {
    const QPoint3D &first = m_vertices[i % numberOfVertices] - point;
    const QPoint3D &second = m_vertices[(i + 1) % numberOfVertices] - point;

    cosTheta = QPoint3D::dotProduct(first, second) / (first.length() * second.length());
    angleSum += acos(cosTheta);
  } 
  return qFuzzyIsNull(M_2_PI - angleSum);
}

}