#include <QVector3D>
#include "math.h"

/**
* \brief Jira GUEZE-2540: Methode zur Sichtbarkeitsbestimmung Stufe 1
*        Funktion isInCone
*
* \param p1     Scheitelpunkt Kegel
* \param p2     Punkt auf Achse im Kegel
* \param angle  Halber Oeffnungswinkel
* \param ptt    Testpunkt
*
* \return b     1, falls Testpunkt im Kegel
*               0, sonst
*/
bool isInCone(const QVector3D top, const QVector3D bottom, const float angle, const QVector3D pointToTest)
{
  QVector3D v1;
  QVector3D v2;
  float alpha;
  float cosAlpha;

  v1.setX(bottom.x() - top.x());
  v1.setY(bottom.y() - top.y());
  v1.setZ(bottom.z() - top.z());
  v2.setX(pointToTest.x() - top.x());
  v2.setY(pointToTest.y() - top.y());
  v2.setZ(pointToTest.z() - top.z());
  if ((v1.length() < 0.00001) || (v2.length() < 0.00001))
  {
    return false;
  }

  cosAlpha = (v1.x()*v2.x() + v1.y()*v2.y() + v1.z()*v2.z()) / (v1.length() * v2.length());
  if (fabs(cosAlpha) > 0.99999)
  {
    alpha = 1.0;
  }
  else
  {
    alpha = acos(cosAlpha);
  }

  if (alpha < 3.14159265359 * fabs(angle) / 180.0)
  {
    return true;
  }
  else
  {
    return false;
  }

}