TYVisibilityCone.cpp

Go to the documentation of this file.

/*
 * Copyright (C) <2012-2024> <EDF-DTG> <FRANCE>
 * This file is part of Code_TYMPAN (R).
 * Code_TYMPAN (R) is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * Code_TYMPAN (R) is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 * You should have received a copy of the GNU General Public License along
 * with Code_TYMPAN (R). If not, see <https://www.gnu.org/licenses/>.
 */
 
#include "TYVisibilityCone.h"
 
VisibilityCone::VisibilityCone(const OPoint3D& pA, const OPoint3D& pB, const OPoint3D& pC)
{
    _pA2D = OPoint3D{pA._x, pA._y, 0.};
    _pB2D = OPoint3D{pB._x, pB._y, 0.};
    _pC2D = OPoint3D{pC._x, pC._y, 0.};
    _vAB = OVector3D{_pA2D, _pB2D};
    _vAC = OVector3D{_pA2D, _pC2D};
    setupNormalsOfCone();
}
 
bool VisibilityCone::segmentIsVisibleInCone(const OSegment3D& segmentToTest) const
{
    // unpack points
    const OPoint3D& pM{segmentToTest._ptA};
    const OPoint3D& pN{segmentToTest._ptB};
 
    // project all points on horizontal plane z = 0
    const OPoint3D pM2D{pM._x, pM._y, 0.};
    const OPoint3D pN2D{pN._x, pN._y, 0.};
 
    return pointIsInCone(pM2D) || pointIsInCone(pN2D) || segmentIntersectsBorderCone(pM2D, pN2D);
}
 
bool VisibilityCone::pointIsInCone(const OPoint3D& pM2D) const
{
    const OVector3D vMB = buildVector2D(pM2D, _pB2D);
    const OVector3D vMC = buildVector2D(pM2D, _pC2D);
 
    return vMB.scalar(_nAB) >= 0 && vMC.scalar(_nAC) >= 0;
}
 
bool VisibilityCone::segmentIntersectsBorderCone(const OPoint3D& pM2D, const OPoint3D& pN2D) const
{
    const OVector3D vMN = buildVector2D(pM2D, pN2D);
    const OVector3D vAM = buildVector2D(_pA2D, pM2D);
    return segmentIntersectsSemiLine2D(_vAB, vMN, vAM) || segmentIntersectsSemiLine2D(_vAC, vMN, vAM);
}
 
void VisibilityCone::setupNormalsOfCone()
{
    const OVector3D ez{0., 0., 1.};
    const OVector3D vBC{_pB2D, _pC2D};
 
    _nAB = _vAB.cross(ez);
    if (_nAB.scalar(vBC) > 0)
    {
        _nAB.invert();
    }
 
    _nAC = _vAC.cross(ez);
    if (_nAC.scalar(vBC) < 0)
    {
        _nAC.invert();
    }
}
 
bool segmentIntersectsSemiLine2D(const OVector3D& vAB, const OVector3D& vMN, const OVector3D& vAM)
{
    // there is intersection in 1 point <=> AB * s - MN * t = AM has a unique solution with t in (0, 1) and s
    // > 0
 
    double s, t;
 
    if (solveLin2x2(vAB._x, -vMN._x, vAB._y, -vMN._y, vAM._x, vAM._y, s, t))
    {
        if (t >= 0. && t <= 1. && s >= 0.)
        {
            return true;
        }
    }
 
    return false;
}
 
bool solveLin2x2(double a11, double a12, double a21, double a22, double b1, double b2, double& x1, double& x2)
{
    const double det{a11 * a22 - a21 * a12};
 
    // infinity of solution or no solution case
    if (std::abs(det) < EPSILON_13)
    {
        return false;
    }
 
    // unique solution case
    const double det_inv{1. / det};
    x1 = (a22 * b1 - a12 * b2) * det_inv;
    x2 = (a11 * b2 - a21 * b1) * det_inv;
 
    return true;
}