54 lines
1.1 KiB
C++
Executable File
54 lines
1.1 KiB
C++
Executable File
#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;
|
|
}
|
|
|
|
}
|