Initial commit
This commit is contained in:
torsten committed
commit
b7d90b9afe
16245 files changed
+3396666
No files matched your search
+680
@@ -0,0 +1,680 @@
|
||||
/**
|
||||
* @RDE_HEADER@
|
||||
* Math claculations for VWM
|
||||
* $Id$
|
||||
* $HeadURL$
|
||||
* $Author$
|
||||
* Created by Christian Ehrlicher, SLSE, 2017-01-24
|
||||
*/
|
||||
|
||||
#include "vwm_math.h"
|
||||
|
||||
#include <logger.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
// [GUEZE-483] RPL Nr 756: Steilfeuer Mörser auf Gebäude
|
||||
// values take from xml file have 3 digits. Therefore 0.00001 is to small
|
||||
// use 0.002
|
||||
#define epsilon 0.002
|
||||
|
||||
namespace BuildingControlUnit
|
||||
{
|
||||
namespace VWM
|
||||
{
|
||||
namespace Math
|
||||
{
|
||||
|
||||
static NGL::INGLLogger &logger()
|
||||
{
|
||||
return BuildingControlUnit::getLogger("VWM.Math");
|
||||
}
|
||||
|
||||
LtsUtils::QPoint3D rotateAroundYAxis(const LtsUtils::QPoint3D &in, double phi)
|
||||
{
|
||||
LtsUtils::QPoint3D a1( cos(phi), 0, -sin(phi));
|
||||
LtsUtils::QPoint3D a2( 0, 1, 0);
|
||||
LtsUtils::QPoint3D a3( sin(phi), 0, cos(phi));
|
||||
|
||||
return LtsUtils::QPoint3D(LtsUtils::QPoint3D::dotProduct(a1, in),
|
||||
LtsUtils::QPoint3D::dotProduct(a2, in),
|
||||
LtsUtils::QPoint3D::dotProduct(a3, in));
|
||||
}
|
||||
|
||||
|
||||
LtsUtils::QPoint3D rotateAroundZAxis(const LtsUtils::QPoint3D &in, double phi)
|
||||
{
|
||||
LtsUtils::QPoint3D a1( cos(phi), sin(phi), 0);
|
||||
LtsUtils::QPoint3D a2(-sin(phi), cos(phi), 0);
|
||||
LtsUtils::QPoint3D a3( 0, 0, 1);
|
||||
|
||||
return LtsUtils::QPoint3D(LtsUtils::QPoint3D::dotProduct(a1, in),
|
||||
LtsUtils::QPoint3D::dotProduct(a2, in),
|
||||
LtsUtils::QPoint3D::dotProduct(a3, in));
|
||||
}
|
||||
|
||||
static inline double getDet2(double a11, double a12, double a21, double a22)
|
||||
{
|
||||
return a11 * a22 - a12 * a21;
|
||||
}
|
||||
|
||||
static inline double getDet3(const LtsUtils::QPoint3D &a1, const LtsUtils::QPoint3D &a2, const LtsUtils::QPoint3D &a3)
|
||||
{
|
||||
double sum = 0;
|
||||
sum += (a1.x() * a2.y() * a3.z() + a1.y() * a2.z() * a3.x() + a1.z() * a2.x() * a3.y());
|
||||
sum -= (a1.z() * a2.y() * a3.x() - a1.y() * a2.x() * a3.z() - a1.x() * a2.z() * a3.y());
|
||||
return sum;
|
||||
}
|
||||
|
||||
VwmPlane::VwmPlane(const LtsUtils::QPoint3D &pos, const LtsUtils::QPoint3D &vec1, const LtsUtils::QPoint3D &vec2)
|
||||
: m_pos(pos)
|
||||
, m_vec1(vec1 - pos)
|
||||
, m_vec2(vec2 - pos)
|
||||
{
|
||||
m_det.setX(getDet2(m_vec1.y(), m_vec1.z(), m_vec2.y(), m_vec2.z()));
|
||||
m_det.setY(getDet2(m_vec1.z(), m_vec1.x(), m_vec2.z(), m_vec2.x()));
|
||||
m_det.setZ(getDet2(m_vec1.x(), m_vec1.y(), m_vec2.x(), m_vec2.y()));
|
||||
|
||||
m_planeC = LtsUtils::QPoint3D::dotProduct(m_pos, m_det);
|
||||
}
|
||||
|
||||
bool VwmPlane::getPlaneEquation() const
|
||||
{
|
||||
if (qFuzzyIsNull(m_det.z()) && qFuzzyIsNull(m_det.x()) && qFuzzyIsNull(m_det.y()))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VwmPlane::isPlaneLineIntersection(const LtsUtils::QPoint3D &vec) const
|
||||
{
|
||||
const double det = getDet3(vec, m_vec1, m_vec2);
|
||||
if (qFuzzyIsNull(det))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VwmPlane::isPlaneLineOrthogonal(const LtsUtils::QPoint3D &dir) const
|
||||
{
|
||||
const double nv = LtsUtils::QPoint3D::dotProduct(m_det, dir);
|
||||
return (qFuzzyIsNull(nv));
|
||||
}
|
||||
|
||||
double VwmPlane::getPlaneLineSkalar(const LtsUtils::QPoint3D &pos, const LtsUtils::QPoint3D &dir) const
|
||||
{
|
||||
const double nv = LtsUtils::QPoint3D::dotProduct(m_det, dir);
|
||||
if (qFuzzyIsNull(nv))
|
||||
return 0;
|
||||
const double np = LtsUtils::QPoint3D::dotProduct(m_det, pos);
|
||||
return (m_planeC - np) / nv;
|
||||
}
|
||||
|
||||
LtsUtils::QPoint3D VwmPlane::getPlaneLineIntersection(const LtsUtils::QPoint3D &pos, const LtsUtils::QPoint3D &dir) const
|
||||
{
|
||||
const double lambda = getPlaneLineSkalar(pos, dir);
|
||||
|
||||
LtsUtils::QPoint3D ret (pos + (dir * lambda));
|
||||
return ret;
|
||||
}
|
||||
|
||||
double VwmPlane::distPlaneToPoint(const LtsUtils::QPoint3D &pos, const LtsUtils::QPoint3D &dir) const
|
||||
{
|
||||
const double f = m_det.length();
|
||||
const double dp = LtsUtils::QPoint3D::dotProduct(m_det, pos);
|
||||
double dist = (m_planeC - dp) / f;
|
||||
dist *= (LtsUtils::QPoint3D::dotProduct(m_det, dir) < 0) ? -1.0 : 1.0;
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
bool vecIsLinearlyDependent(const LtsUtils::QPoint3D &v1, const LtsUtils::QPoint3D &v2)
|
||||
{
|
||||
double lambda;
|
||||
|
||||
if (fabs(v1.x()) > epsilon)
|
||||
lambda = v2.x() / v1.x();
|
||||
|
||||
else if (fabs(v1.y()) > epsilon)
|
||||
lambda = v2.y() / v1.y();
|
||||
|
||||
else if (fabs(v1.z()) > epsilon)
|
||||
lambda = v2.z() / v1.z();
|
||||
else
|
||||
return true; // dependent
|
||||
|
||||
if ( (fabs(lambda*v1.x()-v2.x()) < epsilon)
|
||||
&& (fabs(lambda*v1.y()-v2.y()) < epsilon)
|
||||
&& (fabs(lambda*v1.z()-v2.z()) < epsilon))
|
||||
{
|
||||
return true; // dependent
|
||||
}
|
||||
|
||||
return false; // independent
|
||||
}
|
||||
|
||||
//
|
||||
// polyIsLinearlyDependent is replaced by getCoordsForPlane();
|
||||
// return value '0' can happen if isLinearlyDependent and as return at the end....
|
||||
//int polyIsLinearlyDependent(const QVector<LtsUtils::QPoint3D> &poly)
|
||||
//{
|
||||
// NGL::INGLLogger &l = logger();
|
||||
|
||||
// // Check, if vector != 0
|
||||
///* for (int i = 0; i < poly.size()-1; ++i)
|
||||
// {
|
||||
// LtsUtils::QPoint3D v1 = poly.at(i+1) - poly.at(i);
|
||||
// DEBUG_LOG(l) << "isLinearlyDependent=" << std::boolalpha << isLinearlyDependent
|
||||
// << " i=" << i
|
||||
// << " v1=" << v1.x() << "/" << v1.y() << "/" << v1.z();
|
||||
// if ( v1.x() * v1.x() + v1.y() * v1.y() + v1.z() * v1.z() > epsilon)
|
||||
// return i;
|
||||
|
||||
// }*/
|
||||
|
||||
|
||||
// // Check, if point is Linearly Dependent
|
||||
// for (int i = 0; i < poly.size()-2; ++i)
|
||||
// {
|
||||
// LtsUtils::QPoint3D v1 = poly.at(i+1) - poly.at(i);
|
||||
// LtsUtils::QPoint3D v2 = poly.at(i+2) - poly.at(i);
|
||||
// bool isLinearlyDependent = vecIsLinearlyDependent(v1, v2);
|
||||
// DEBUG_LOG(l) << "isLinearlyDependent=" << std::boolalpha << isLinearlyDependent
|
||||
// << " i=" << i
|
||||
// << " v1=" << v1.x() << "/" << v1.y() << "/" << v1.z()
|
||||
// << " v2=" << v2.x() << "/" << v2.y() << "/" << v2.z();
|
||||
// if (isLinearlyDependent)
|
||||
// return i;
|
||||
// }
|
||||
// return 0;
|
||||
//}
|
||||
|
||||
|
||||
/*
|
||||
[GUEZE-483] RPL Nr 756: Steilfeuer Mörser auf Gebäude
|
||||
Bei weiterer Betrachtung ist aufgefallen, das bei bldg_1104 AWES auf die Ecken
|
||||
des Bldgs NICHT zu einem Zerstörung führen - Fehler.
|
||||
Bei der Analyse ergeben sich folgende Erkenntnisse:
|
||||
- Der epsilon-Wert war auf 0.0001.
|
||||
Da aber die Werte aus den den XML-Dateien eine Auflösung von 3Stellen (mm)
|
||||
aufweisen, sind Rechenfehler auch in dieser Größenordnung zu erwarten.
|
||||
Der epsilon-Wert wird in dem Zuge auf 0.002 gesetzt.
|
||||
- vecIsLinearlyDependent
|
||||
Hier war der return-Value vertauscht -> korrigiert.
|
||||
- polyIsLinearlyDependent
|
||||
War missverständlich/fehleranfällig, da im Falle von isLinearlyDependent
|
||||
grundsätzlich auch '0' zurückgegeben werden kann. dies aber auch der return-Value
|
||||
am Ende der Funktion ist... Was bedeutet nun also '0'???
|
||||
- getCoordsForPlane
|
||||
wurde als Nachfolger von polyIsLinearlyDependent() eingeführt.
|
||||
Aus dem WallElement werden die ggf. vorhandenen linearen Abhänigkeiten entfernt.
|
||||
Der Aufrufer kann dann daraus eine Plane erstellen (Nutzung der 3 ersten Werte)
|
||||
- Der UnitTest 'VwmMathUnitTest' wurde erweitert:
|
||||
- Es werden einfache Gut- und Schlechtfälle wurden konstruiert.
|
||||
- Es werden realen Polygonzüge geprüft. (bldg 1104, Raum 401; bldg_salchau577...)
|
||||
*/
|
||||
QVector<LtsUtils::QPoint3D> getCoordsForPlane(const QVector<LtsUtils::QPoint3D> &poly)
|
||||
{
|
||||
NGL::INGLLogger &l = logger();
|
||||
|
||||
if (poly.size() < 3)
|
||||
return {};
|
||||
|
||||
QVector<int> idxToRemove;
|
||||
for (int i = 0; i < poly.size()-2; ++i)
|
||||
{
|
||||
const LtsUtils::QPoint3D &v1 = poly.at(i+1) - poly.at(i);
|
||||
const LtsUtils::QPoint3D &v2 = poly.at(i+2) - poly.at(i);
|
||||
bool isLinearlyDependent = vecIsLinearlyDependent(v1, v2);
|
||||
DEBUG_LOG(l) << "isLinearlyDependent=" << isLinearlyDependent
|
||||
<< " i=" << i
|
||||
<< " v1=" << v1.x() << "/" << v1.y() << "/" << v1.z()
|
||||
<< " v2=" << v2.x() << "/" << v2.y() << "/" << v2.z();
|
||||
if (isLinearlyDependent)
|
||||
idxToRemove.push_front(i+1);
|
||||
|
||||
} // for
|
||||
|
||||
QVector<LtsUtils::QPoint3D> ret = poly;
|
||||
for (auto idx : idxToRemove)
|
||||
ret.remove(idx);
|
||||
return ret;
|
||||
// Maybe there is a LinearlyDependency regarding beginning and and of vector.
|
||||
// Until now, this seems to be not relevant, since the first coordinates are taken.
|
||||
}
|
||||
|
||||
bool isPointInPolygon(const QVector<LtsUtils::QPoint3D> &poly, const LtsUtils::QPoint3D &pos)
|
||||
{
|
||||
NGL::INGLLogger &l = logger();
|
||||
double anglesum = 0.0;
|
||||
double vz = 1.0;
|
||||
LtsUtils::QPoint3D vn;
|
||||
|
||||
if (poly.size() < 3)
|
||||
return false;
|
||||
|
||||
for (int i = 0; i < poly.size(); ++i)
|
||||
{
|
||||
DEBUG_LOG(l) << "calculate point i=" << i;
|
||||
const int nextIdx1 = (i + 1) % poly.size();
|
||||
const int nextIdx2 = (i + 2) % poly.size();
|
||||
|
||||
const LtsUtils::QPoint3D &p1 = poly.at(i) - pos;
|
||||
const LtsUtils::QPoint3D &p2 = poly.at(nextIdx1) - pos;
|
||||
if (i == 0)
|
||||
vn = LtsUtils::QPoint3D::crossProduct(p1,p2);
|
||||
|
||||
const double dist1 = p1.length();
|
||||
const double dist2 = p2.length();
|
||||
if (qFuzzyIsNull(dist1 * dist2))
|
||||
{
|
||||
DEBUG_LOG(l) << "isPointInPolygon = false";
|
||||
return false;
|
||||
}
|
||||
|
||||
const double cosTheta = LtsUtils::QPoint3D::dotProduct(p1, p2) / (dist1 * dist2);
|
||||
anglesum += vz * acos(cosTheta);
|
||||
|
||||
const LtsUtils::QPoint3D &n = LtsUtils::QPoint3D::crossProduct(vn, p2);
|
||||
const LtsUtils::QPoint3D &p3 = poly.at(nextIdx2) - pos;
|
||||
const double dp = LtsUtils::QPoint3D::dotProduct(n, p3);
|
||||
|
||||
vz = (dp < 0) ? -1.0 : 1.0;
|
||||
}
|
||||
|
||||
const double delta = 0.01;
|
||||
const double sum = (M_PI * 2) - fabs(anglesum);
|
||||
bool result = -delta < sum && sum < delta;
|
||||
DEBUG_LOG(l) << "isPointInPolygon=" << std::boolalpha << result << " (sum=" << sum << ", anglesum=" << anglesum << ")"
|
||||
<< ", pos=" << pos;
|
||||
return result;
|
||||
}
|
||||
|
||||
static inline bool getHitPositionInternal(const QVector<LtsUtils::QPoint3D> &planePoints,
|
||||
const LtsUtils::QPoint3D &position,
|
||||
const LtsUtils::QPoint3D &direction,
|
||||
HitPosition &hitPos)
|
||||
{
|
||||
NGL::INGLLogger &l = logger();
|
||||
if (planePoints.size() < 3)
|
||||
{
|
||||
WARNING_LOG(l) << "Wall with fewer than 3 points - modelling error!";
|
||||
return false;
|
||||
}
|
||||
QVector<LtsUtils::QPoint3D> localPoly = getCoordsForPlane(planePoints);
|
||||
if (localPoly.size() < 3)
|
||||
{
|
||||
ERROR_LOG(l) << "planePoints are not sufficiant";
|
||||
for (auto coord : planePoints)
|
||||
ERROR_LOG(l) << "planePoints are not sufficiant: << " << coord;
|
||||
for (auto coord : qAsConst(localPoly))
|
||||
ERROR_LOG(l) << "localPoly are not sufficiant: << " << coord;
|
||||
return false;
|
||||
}
|
||||
|
||||
const VwmPlane vwmPlane(localPoly.at(0), localPoly.at(1), localPoly.at(2));
|
||||
for ( int i = 0; i < localPoly.size(); ++i)
|
||||
{
|
||||
DEBUG_LOG(l) << "vwmPlane at(" << i << ")=" << localPoly.at(i).x()
|
||||
<< "/" << localPoly.at(i).y()
|
||||
<< "/" << localPoly.at(i).z() ;
|
||||
}
|
||||
DEBUG_LOG(l) << " plane params: p1 = " << vwmPlane.pos()
|
||||
<< ", p2 = " << vwmPlane.vec1()
|
||||
<< ", p3 = " << vwmPlane.vec2()
|
||||
<< ", det = " << vwmPlane.det()
|
||||
<< ", C = " << vwmPlane.planeC()
|
||||
<< ", isVertical=" << vwmPlane.isVertical();
|
||||
|
||||
if (!vwmPlane.getPlaneEquation())
|
||||
return false;
|
||||
DEBUG_LOG(l) << " getPlaneEquation = true";
|
||||
|
||||
if (!vwmPlane.isPlaneLineIntersection(direction))
|
||||
return false;
|
||||
DEBUG_LOG(l) << " isPlaneLineIntersection = true";
|
||||
|
||||
const double lambda = vwmPlane.getPlaneLineSkalar(position, direction);
|
||||
if (lambda <= 0)
|
||||
return false;
|
||||
DEBUG_LOG(l) << " lambda = " << lambda;
|
||||
|
||||
const LtsUtils::QPoint3D pos = vwmPlane.getPlaneLineIntersection(position, direction);
|
||||
DEBUG_LOG(l) << "pos=" << pos.x() << "/" << pos.y() << "/" << pos.z()
|
||||
<< " position=" << position.x() << "/" << position.y() << "/" << position.z()
|
||||
<< " direction=" << direction.x() << "/" << direction.y() << "/" << direction.z();
|
||||
|
||||
if (!isPointInPolygon(planePoints, pos))
|
||||
return false;
|
||||
DEBUG_LOG(l) << " isPointInPolygon = true (pos=" << pos << ")";
|
||||
|
||||
// never negative since this will bailed out with getPlaneLineSkalar() above
|
||||
const double curDist = (pos - position).length();
|
||||
// const double curDist = vwmPlane.distPlaneToPoint(position, direction);
|
||||
// ERROR_LOG(l) << "l: " << curDist << " = " << (pos - position).length();
|
||||
DEBUG_LOG(l) << " distance = " << curDist;
|
||||
if (curDist > hitPos.distance)
|
||||
return false;
|
||||
hitPos.distance = curDist;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
HitPosition getHitPosition(const LtsUtils::QPoint3D &position, const LtsUtils::QPoint3D &direction, const QMap<int, Room> &rooms)
|
||||
{
|
||||
NGL::INGLLogger &l = logger();
|
||||
INFO_LOG(l) << "XXXXX Searching hit position for position " << position
|
||||
<< ", direction " << direction
|
||||
<< ", #rooms = " << rooms.size();
|
||||
HitPosition hitPos;
|
||||
|
||||
for (const Room &room : rooms)
|
||||
{
|
||||
DEBUG_LOG(l) << "****** roomId: " << room.id() << ", #walls = " << room.outerWalls().size();
|
||||
for (int idxOuterWall = 0; idxOuterWall < room.outerWalls().size(); ++idxOuterWall)
|
||||
{
|
||||
const OuterWall &wall = room.outerWalls().at(idxOuterWall);
|
||||
DEBUG_LOG(l) << " roomId: " << room.id() << ", wallIdx = " << idxOuterWall << ", #elements = " << wall.elements().size();
|
||||
for (int idxWallElement = 0; idxWallElement < wall.elements().size(); ++idxWallElement)
|
||||
{
|
||||
const WallElement &wallElement = wall.elements().at(idxWallElement);
|
||||
DEBUG_LOG(l) << " roomId: " << room.id() << ", wallIdx = " << idxOuterWall << ", wElementIdx = " << idxWallElement;
|
||||
if (!getHitPositionInternal(wallElement, position, direction, hitPos))
|
||||
continue;
|
||||
|
||||
DEBUG_LOG(l) << " nearest distance = " << hitPos.distance << " on outer wall: roomId: " << room.id() << ", wallIdx = " << idxOuterWall << ", wElementIdx = " << idxWallElement;
|
||||
hitPos.state = HitPosition::State::OuterWall;
|
||||
hitPos.roomId = room.id();
|
||||
hitPos.outerWallIdx = idxOuterWall;
|
||||
hitPos.exclusionZoneIdx = -1;
|
||||
|
||||
for (int idxExclusionZone = 0; idxExclusionZone < wall.exclusionZones().size(); ++idxExclusionZone)
|
||||
{
|
||||
const ExclusionZone &ez = wall.exclusionZones().at(idxExclusionZone);
|
||||
DEBUG_LOG(l) << " roomId: " << room.id() << ", wallIdxx = " << idxOuterWall << ", wElementIdx = " << idxWallElement
|
||||
<< " - exclusion zone element idx = " << idxExclusionZone << ", #elements = " << ez.elements().size();
|
||||
for (int idxExclusionZoneElement = 0; idxExclusionZoneElement < ez.elements().size(); ++idxExclusionZoneElement)
|
||||
{
|
||||
const ExclusionZoneElement &ezElement = ez.elements().at(idxExclusionZoneElement);
|
||||
DEBUG_LOG(l) << " roomId: " << room.id() << ", wallIdx = " << idxOuterWall << ", wElementIdx = " << idxWallElement
|
||||
<< " - exclusion zone element idx = " << idxExclusionZoneElement;
|
||||
if (!getHitPositionInternal(ezElement, position, direction, hitPos))
|
||||
continue;
|
||||
|
||||
hitPos.state = HitPosition::State::ExclusionZone;
|
||||
hitPos.exclusionZoneIdx = idxExclusionZone;
|
||||
|
||||
DEBUG_LOG(l) << " nearest distance = " << hitPos.distance << " on exclusion zone";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
INFO_LOG(l) << "XXXXX HitPosition: " << hitPos.state
|
||||
<< ", roomId = " << hitPos.roomId
|
||||
<< ", outerWallIndex = " << hitPos.outerWallIdx
|
||||
<< ", exclusionZoneIndex = " << hitPos.exclusionZoneIdx;
|
||||
return hitPos;
|
||||
}
|
||||
|
||||
// ThorstenMeyer: Beim Treffer eines BSMs wird die HitRoomID wie folgt bestimmt
|
||||
//1. Berechnung des Strahls S durch das BSM in Schußrichtung:
|
||||
// Die Richtung „vwmDir3D“ wird mit der Funktion übergeben:
|
||||
// getHitPosition(VwmPosition3D* vwmCoord,VwmPosition3D* vwmDir3D)
|
||||
|
||||
//2. Berechnung von 2 linear unabhängigen Vektoren durch die Differenzbildung von
|
||||
// Polygoneckpunkten einer OuterWall:
|
||||
// if (vwmPlaneOW.getIsLinearlyIndependent(…) // Achtung isLinearly_In_dependent vs isLinearlyDependent
|
||||
|
||||
//3. Berechnung der durch die 2 linear unabhängigen Vektoren aufgespannten Ebene E:
|
||||
// vwmPlaneOW.getPlaneEquation(…)
|
||||
|
||||
//4. Berechnung der Schnittpunktmenge des Strahls S und der Ebene E:
|
||||
// if (vwmPlaneOW.isPlaneLineIntersection(vwmDir3D))
|
||||
|
||||
//5. Überprüfung, ob die Schnittpunktmenge von S und E ein Punkt P ist:
|
||||
// if (vwmPlaneOW.getPlaneLineSkalar(vwmCoord,vwmDir3D) > 0)
|
||||
|
||||
//6. Im Fall #Schnittpunktmenge=1: Überprüfung, ob der Punkt P im Polygon liegt.
|
||||
// if (vwmPolygonOW->calcPointInPolygon(posOW))
|
||||
|
||||
//7. Im Fall „Punkt in Polygon“: Berechnung Abstand von BSM zum „Punkt im Polygon“.
|
||||
// dist = vwmPlaneOW.distPlaneToPoint(vwmCoord);
|
||||
|
||||
//8. Berechnung Abstände BSM zum „Punkt im Polygon“ für alle OuterWalls.
|
||||
|
||||
//9. Bestimmung OuterWall OW (getroffene Außenwand) mit dem kürzesten Abstand BSM zum Punkt
|
||||
// im OuterWall-Polygon.
|
||||
|
||||
//10. HitRoomID = Raumnummer des Raums, zu dem die OuterWall OW gehört.
|
||||
|
||||
HitPosition checkBsmPosition(const LtsUtils::QPoint3D &position, const LtsUtils::QPoint3D &bsmViewDirection, const QMap<int, Room> &rooms)
|
||||
{
|
||||
NGL::INGLLogger &l = logger();
|
||||
const LtsUtils::QPoint3D direction(bsmViewDirection * -1);
|
||||
INFO_LOG(l) << "XXXXX Searching nearest room for bsm position " << position
|
||||
<< ", bsmViewDirection " << bsmViewDirection
|
||||
<< ", hit direction " << direction
|
||||
<< ", #rooms = " << rooms.size();
|
||||
HitPosition hitPos;
|
||||
|
||||
for (const Room &room : rooms)
|
||||
{
|
||||
DEBUG_LOG(l) << "room[" << room.id() << "] #walls = " << room.outerWalls().size();
|
||||
for (int idxOuterWall = 0; idxOuterWall < room.outerWalls().size(); ++idxOuterWall)
|
||||
{
|
||||
const OuterWall &wall = room.outerWalls().at(idxOuterWall);
|
||||
DEBUG_LOG(l) << "room[" << room.id() << "]" << " wall[" << idxOuterWall << "] #elements = " << wall.elements().size();
|
||||
for (int idxWallElement = 0; idxWallElement < wall.elements().size(); ++idxWallElement)
|
||||
{
|
||||
const WallElement &wallElement = wall.elements().at(idxWallElement);
|
||||
DEBUG_LOG(l) << "room[" << room.id() << "]" << " wallElement[" << idxWallElement << "] #points = " << wallElement.size();
|
||||
if (wallElement.size() < 3)
|
||||
continue;
|
||||
|
||||
// fix wallElement regarding polyIsLinearlyDependent
|
||||
// [GUEZE-483] RPL Nr 756: Steilfeuer Mörser auf Gebäude
|
||||
const QVector<LtsUtils::QPoint3D> &localWallElement = getCoordsForPlane(wallElement);
|
||||
DEBUG_LOG(l) << "room[" << room.id() << "]" << " wallElement[" << idxWallElement << "] #points = " << localWallElement.size() << " after removing Linearly Dependent)";
|
||||
|
||||
if (localWallElement.size() < 3)
|
||||
{
|
||||
ERROR_LOG(l) << "localWallElement are not sufficiant";
|
||||
for (auto coord : wallElement)
|
||||
ERROR_LOG(l) << "wallElement are not sufficiant: << " << coord;
|
||||
for (auto coord : qAsConst(localWallElement))
|
||||
ERROR_LOG(l) << "localWallElement are not sufficiant: << " << coord;
|
||||
continue;
|
||||
}
|
||||
|
||||
for (auto pointOfWallElementLinear : qAsConst(localWallElement))
|
||||
DEBUG_LOG(l) << "room[" << room.id() << "]" << " wall[" << idxOuterWall << "] " << " wallElement[" << idxWallElement << "] #points = " << localWallElement.size() << ", pointOfWallElementLinear=" << pointOfWallElementLinear;
|
||||
|
||||
const VwmPlane vwmPlaneOW(localWallElement.at(0), localWallElement.at(1), localWallElement.at(2));
|
||||
DEBUG_LOG(l) << "room[" << room.id() << "]" << " wall[" << idxOuterWall << "]" << " wallElement[" << idxWallElement << "]"
|
||||
<< ", planeOW params: p1 = " << vwmPlaneOW.pos()
|
||||
<< ", p2 = " << vwmPlaneOW.vec1()
|
||||
<< ", p3 = " << vwmPlaneOW.vec2()
|
||||
<< ", det = " << vwmPlaneOW.det()
|
||||
<< ", C = " << vwmPlaneOW.planeC()
|
||||
<< ", vwmPlaneOW.isVertical=" << vwmPlaneOW.isVertical();
|
||||
|
||||
|
||||
|
||||
|
||||
if (!vwmPlaneOW.getPlaneEquation())
|
||||
continue;
|
||||
DEBUG_LOG(l) << " getPlaneEquation() = true";
|
||||
|
||||
if (!vwmPlaneOW.isPlaneLineIntersection(direction))
|
||||
continue;
|
||||
DEBUG_LOG(l) << " isPlaneLineIntersection() = true -> direction=" << direction;
|
||||
|
||||
// only check for orthogonal, do not check for correct direction (in contrast to getHitPosition() above)
|
||||
if (vwmPlaneOW.isPlaneLineOrthogonal(direction))
|
||||
continue;
|
||||
|
||||
|
||||
|
||||
const LtsUtils::QPoint3D posOW = vwmPlaneOW.getPlaneLineIntersection(position, direction);
|
||||
DEBUG_LOG(l) << " point to check: " << posOW;
|
||||
|
||||
if (!isPointInPolygon(localWallElement, posOW))
|
||||
continue;
|
||||
DEBUG_LOG(l) << " isPointInPolygon() = true";
|
||||
|
||||
const double curDistOW = vwmPlaneOW.distPlaneToPoint(position, direction);
|
||||
const double absDistOW = fabs(curDistOW);
|
||||
DEBUG_LOG(l) << " distance = " << curDistOW << " - room " << room.id();
|
||||
if (absDistOW > fabs(hitPos.distance))
|
||||
continue;
|
||||
hitPos.distance = curDistOW;
|
||||
hitPos.state = HitPosition::State::OuterWall;
|
||||
hitPos.roomId = room.id();
|
||||
hitPos.outerWallIdx = idxOuterWall;
|
||||
DEBUG_LOG(l) << " XXXXX nearest distance = " << curDistOW << " on outer wall in room " << room.id()
|
||||
<< "room[" << room.id() << "]" << " wall[" << idxOuterWall << "]" << " wallElement[" << idxWallElement << "]";
|
||||
// we don't care about exclusion zones since they should be on the same plane as the wall
|
||||
|
||||
} // for
|
||||
} // for
|
||||
} // for
|
||||
INFO_LOG(l) << "XXXXX HitPosition: " << hitPos.state
|
||||
<< ", roomId = " << hitPos.roomId
|
||||
<< ", outerWallIndex = " << hitPos.outerWallIdx
|
||||
<< ", exclusionZoneIndex = " << hitPos.exclusionZoneIdx;
|
||||
return hitPos;
|
||||
}
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
// Intersection methods
|
||||
//
|
||||
|
||||
LtsUtils::QPoint3D getIntersectionInternal(
|
||||
const QVector<LtsUtils::QPoint3D> &planePoints, const LtsUtils::QPoint3D &position, const LtsUtils::QPoint3D &direction
|
||||
)
|
||||
{
|
||||
NGL::INGLLogger &l = logger();
|
||||
if (planePoints.size() < 3)
|
||||
{
|
||||
WARNING_LOG(l) << "Wall with fewer than 3 points - modelling error!";
|
||||
return invalidPosition();
|
||||
}
|
||||
const QVector<LtsUtils::QPoint3D> &localPoly = getCoordsForPlane(planePoints);
|
||||
|
||||
if (localPoly.size() < 3)
|
||||
{
|
||||
ERROR_LOG(l) << "planePoints are not sufficiant";
|
||||
for (auto coord : qAsConst(planePoints))
|
||||
ERROR_LOG(l) << "planePoints are not sufficiant: << " << coord;
|
||||
for (auto coord : qAsConst(localPoly))
|
||||
ERROR_LOG(l) << "localPoly are not sufficiant: << " << coord;
|
||||
return invalidPosition();
|
||||
}
|
||||
|
||||
const VwmPlane vwmPlane(localPoly.at(0), localPoly.at(1), localPoly.at(2));
|
||||
if (l.isDebugEnabled())
|
||||
{
|
||||
for ( int i = 0; i < localPoly.size(); ++i)
|
||||
DEBUG_LOG(l) << "vwmPlane at(" << i << ")=" << localPoly.at(i).x() << "/" << localPoly.at(i).y() << "/"
|
||||
<< localPoly.at(i).z();
|
||||
DEBUG_LOG(l) << "plane params: p1 = " << vwmPlane.pos() << ", p2 = " << vwmPlane.vec1() << ", p3 = " << vwmPlane.vec2()
|
||||
<< ", det = " << vwmPlane.det() << ", C = " << vwmPlane.planeC();
|
||||
} // if
|
||||
|
||||
if (!vwmPlane.getPlaneEquation())
|
||||
return invalidPosition();
|
||||
DEBUG_LOG(l) << "getPlaneEquation() = true";
|
||||
|
||||
if (!vwmPlane.isPlaneLineIntersection(direction))
|
||||
return invalidPosition();
|
||||
DEBUG_LOG(l) << "isPlaneLineIntersection() = true";
|
||||
|
||||
const double planeLineScalar = vwmPlane.getPlaneLineSkalar(position, direction);
|
||||
if (planeLineScalar <= 0)
|
||||
return invalidPosition();
|
||||
DEBUG_LOG(l) << "getPlaneLineSkalar() = " << planeLineScalar;
|
||||
|
||||
const LtsUtils::QPoint3D pos = vwmPlane.getPlaneLineIntersection(position, direction);
|
||||
|
||||
bool bIsPointInPolygon = isPointInPolygon(localPoly, pos);
|
||||
DEBUG_LOG(l) << "bIsPoinliotInPolygon = " << bIsPointInPolygon;
|
||||
|
||||
return bIsPointInPolygon ? pos : invalidPosition();
|
||||
}
|
||||
|
||||
|
||||
LtsUtils::QPoint3D intersection(const LtsUtils::QPoint3D &start, const LtsUtils::QPoint3D &end, const QMap<int, Room> &rooms)
|
||||
{
|
||||
NGL::INGLLogger &l = logger();
|
||||
DEBUG_LOG(l) << "Searching for intersection: start = " << start << ", end = " << end;
|
||||
|
||||
LtsUtils::QPoint3D intersectionPoint = invalidPosition();
|
||||
double distance = (end - start).length();
|
||||
const LtsUtils::QPoint3D direction = (end - start).normalized();
|
||||
|
||||
DEBUG_LOG(l) << "Searching for intersection: START"
|
||||
<< ", intersectionPoint = " << intersectionPoint
|
||||
<< ", distance = " << distance
|
||||
<< ", direction = " << direction;
|
||||
|
||||
int cnt = 0;
|
||||
int roomId4Debugging = 1;
|
||||
for (const Room &room : qAsConst(rooms))
|
||||
for (const OuterWall &outerWall : qAsConst(room.outerWalls()))
|
||||
for (const WallElement &wallElement : qAsConst(outerWall.elements()))
|
||||
{
|
||||
const LtsUtils::QPoint3D point = getIntersectionInternal(wallElement, start, direction);
|
||||
DEBUG_LOG(l) << "getIntersectionInternal:"
|
||||
<< " cnt = " << cnt++
|
||||
<< " room.id = " << room.id()
|
||||
<< " - point=" << point;
|
||||
if (point == invalidPosition())
|
||||
continue;
|
||||
const double dist = (point - start).length();
|
||||
if (dist < distance)
|
||||
{
|
||||
DEBUG_LOG(l) << "Intersection found for room " << room.id() << ": " << point;
|
||||
intersectionPoint = point;
|
||||
distance = dist;
|
||||
roomId4Debugging =room.id();
|
||||
} // if
|
||||
} // for
|
||||
|
||||
if (roomId4Debugging > 0)
|
||||
DEBUG_LOG(l) << "Searching for intersection: FOUND"
|
||||
<< ", intersectionPoint = " << intersectionPoint
|
||||
<< ", distance = " << distance
|
||||
<< ", direction = " << direction
|
||||
<< ", at roomId=" << roomId4Debugging;
|
||||
|
||||
return intersectionPoint;
|
||||
}
|
||||
|
||||
|
||||
bool hasIntersection(const LtsUtils::QPoint3D &start, const LtsUtils::QPoint3D &end, const QMap<int, Room> &rooms)
|
||||
{
|
||||
const double distance = (end - start).length();
|
||||
const LtsUtils::QPoint3D direction = (end - start).normalized();
|
||||
for (const Room &room : qAsConst(rooms))
|
||||
for (const OuterWall &outerWall : qAsConst(room.outerWalls()))
|
||||
for (const WallElement &wallElement : qAsConst(outerWall.elements()))
|
||||
{
|
||||
const LtsUtils::QPoint3D point = getIntersectionInternal(wallElement, start, direction);
|
||||
if (point == invalidPosition())
|
||||
continue;
|
||||
if ((start - point).length() > distance)
|
||||
continue;
|
||||
return true;
|
||||
} // for
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
LtsUtils::QPoint3D invalidPosition()
|
||||
{
|
||||
return {100000., 100000., -10000.};
|
||||
}
|
||||
|
||||
} // namespace Math
|
||||
} // namespace VWM
|
||||
} // namespace BuildingControlUnit
|
||||
Reference in new issue
Block a user