89 lines
1.9 KiB
C++
Executable File
89 lines
1.9 KiB
C++
Executable File
#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);
|
|
}
|
|
|
|
}
|