Continues migration with DDS, Common

Lts/Common/LtsUtils/LtsUtilsCore/qplane3d.cpp

@@ -0,0 +1,88 @@
+#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);
+}
+
+}