OCCT

// Created on: 2022-08-08
// Created by: Kseniya NOSULKO
// Copyright (c) 2022 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

#include <BRepExtrema_ProximityDistTool.hxx>

#include <BRepAdaptor_Surface.hxx>
#include <BRep_Tool.hxx>
#include <gp_Pln.hxx>
#include <Precision.hxx>
#include <Poly_Connect.hxx>
#include <Standard_NullValue.hxx>
#include <TopoDS.hxx>

//=======================================================================
// function : BRepExtrema_ProximityDistTool
// purpose  : Creates new unitialized tool
//=======================================================================
BRepExtrema_ProximityDistTool::BRepExtrema_ProximityDistTool()
    : myMinDistance(std::numeric_limits<Standard_Real>::max()),
      myProxDist(-1.),
      myPntStatus1(ProxPnt_Status_UNKNOWN),
      myPntStatus2(ProxPnt_Status_UNKNOWN),
      myNbSamples1(0),
      myProxVtxIdx1(-1)
{
}

//=======================================================================
// function : BRepExtrema_ProximityDistTool
// purpose  : Creates new tool for the given element sets
//=======================================================================
BRepExtrema_ProximityDistTool::BRepExtrema_ProximityDistTool(
  const Handle(BRepExtrema_TriangleSet)&    theSet1,
  const Standard_Integer                    theNbSamples1,
  const BVH_Array3d&                        theAddVertices1,
  const NCollection_Vector<ProxPnt_Status>& theAddStatus1,
  const Handle(BRepExtrema_TriangleSet)&    theSet2,
  const BRepExtrema_ShapeList&              theShapeList1,
  const BRepExtrema_ShapeList&              theShapeList2)
    : myMinDistance(std::numeric_limits<Standard_Real>::max()),
      myProxDist(-1.),
      myPntStatus1(ProxPnt_Status_UNKNOWN),
      myPntStatus2(ProxPnt_Status_UNKNOWN),
      myNbSamples1(theNbSamples1),
      myProxVtxIdx1(-1)
{
  LoadTriangleSets(theSet1, theSet2);
  LoadShapeLists(theShapeList1, theShapeList2);
  LoadAdditionalPointsFirstSet(theAddVertices1, theAddStatus1);
}

//=======================================================================
// function : LoadTriangleSets
// purpose  : Loads the given element sets into the tool
//=======================================================================
void BRepExtrema_ProximityDistTool::LoadTriangleSets(const Handle(BRepExtrema_TriangleSet)& theSet1,
                                                     const Handle(BRepExtrema_TriangleSet)& theSet2)
{
  mySet1 = theSet1;
  mySet2 = theSet2;
}

//=======================================================================
// function : LoadShapeLists
// purpose  : Loads the given list of subshapes into the proximity tool
//=======================================================================
void BRepExtrema_ProximityDistTool::LoadShapeLists(const BRepExtrema_ShapeList& theShapeList1,
                                                   const BRepExtrema_ShapeList& theShapeList2)
{
  myShapeList1 = theShapeList1;
  myShapeList2 = theShapeList2;
}

//=======================================================================
// function : LoadAdditionalPointsFirstSet
// purpose  : Loads given additional vertices and their statuses
//=======================================================================
void BRepExtrema_ProximityDistTool::LoadAdditionalPointsFirstSet(
  const BVH_Array3d&                        theAddVertices1,
  const NCollection_Vector<ProxPnt_Status>& theAddStatus1)
{
  myAddVertices1 = theAddVertices1;
  myAddStatus1   = theAddStatus1;
}

//=======================================================================
// function : goThroughtSet1
// purpose  : Goes through vertices from the 1st set
//=======================================================================
void BRepExtrema_ProximityDistTool::goThroughtSet1(const BVH_Array3d&     theVertices1,
                                                   const Standard_Boolean theIsAdditionalSet)
{
  Standard_Integer aVtxSize = (Standard_Integer)theVertices1.size();
  Standard_Integer aVtxStep = Max(myNbSamples1 <= 0 ? 1 : aVtxSize / myNbSamples1, 1);
  for (Standard_Integer aVtxIdx = 0; aVtxIdx < aVtxSize; aVtxIdx += aVtxStep)
  {
    myDistance    = std::numeric_limits<Standard_Real>::max();
    myMinDistance = std::numeric_limits<Standard_Real>::max();
    myIsDone      = Standard_False;
    SetObject(theVertices1[aVtxIdx]);

    ComputeDistance();

    if (!IsDone() && myProxDist < 0.)
      return;

    if (IsDone() && myDistance > myProxDist)
    {
      myPnt1                 = theVertices1[aVtxIdx];
      myPnt2                 = myExtremaPoint;
      myProxDist             = myDistance;
      myProxVtxIdx1          = aVtxIdx;
      myIsProxVtx1FromAddSet = theIsAdditionalSet;
      myProxPrjState         = myExtPrjState;
    }
  }
}

//=======================================================================
// function : Perform
// purpose  : Performs searching of the proximity distance
//=======================================================================
void BRepExtrema_ProximityDistTool::Perform()
{
  SetBVHSet(mySet2.get());
  goThroughtSet1(mySet1->GetVertices(), Standard_False);
  goThroughtSet1(myAddVertices1, Standard_True);

  myIsDone = myProxDist > -1.;
  if (myIsDone)
  {
    DefineStatusProxPnt();
  }
}

static Standard_Real pointBoxSquareMaxDistance(const BVH_Vec3d& thePoint,
                                               const BVH_Vec3d& theCMin,
                                               const BVH_Vec3d& theCMax)
{
  Standard_Real aDist = 0;
  for (int i = 0; i < 3; ++i)
  {
    if (thePoint[i] <= 0.5 * (theCMax[i] + theCMin[i]))
    {
      Standard_Real d = theCMax[i] - thePoint[i];
      d *= d;
      aDist += d;
    }
    else
    {
      Standard_Real d = thePoint[i] - theCMin[i];
      d *= d;
      aDist += d;
    }
  }
  return aDist;
}

//=======================================================================
// function : Branch rejection
// purpose  : Defines the rules for node rejection by bounding box
//=======================================================================
Standard_Boolean BRepExtrema_ProximityDistTool::RejectNode(const BVH_Vec3d& theCornerMin,
                                                           const BVH_Vec3d& theCornerMax,
                                                           Standard_Real&   theMetric) const
{
  theMetric =
    sqrt(BVH_Tools<Standard_Real, 3>::PointBoxSquareDistance(myObject, theCornerMin, theCornerMax));

  Standard_Real aMaxMetric = sqrt(pointBoxSquareMaxDistance(myObject, theCornerMin, theCornerMax));

  return theMetric > myDistance || aMaxMetric < myProxDist;
}

//=======================================================================
// function : Leaf acceptance
// purpose  : Defines the rules for leaf acceptance
//=======================================================================
Standard_Boolean BRepExtrema_ProximityDistTool::Accept(const Standard_Integer theTrgIdx,
                                                       const Standard_Real&)
{
  BVH_Vec3d aTrgVert1;
  BVH_Vec3d aTrgVert2;
  BVH_Vec3d aTrgVert3;

  BVH_PrjState     aBVH_PrjState;
  Standard_Integer aNumberOfFirstNode = -1;
  Standard_Integer aNumberOfLastNode  = -1;

  mySet2->GetVertices(theTrgIdx, aTrgVert1, aTrgVert2, aTrgVert3);

  BVH_Vec3d aNearestPnt = BVH_Tools<Standard_Real, 3>::PointTriangleProjection(myObject,
                                                                               aTrgVert1,
                                                                               aTrgVert2,
                                                                               aTrgVert3,
                                                                               &aBVH_PrjState,
                                                                               &aNumberOfFirstNode,
                                                                               &aNumberOfLastNode);

  PrjState      aPrjState(theTrgIdx, aBVH_PrjState, aNumberOfFirstNode, aNumberOfLastNode);
  BVH_Vec3d     aDirect     = myObject - aNearestPnt;
  Standard_Real aSqDistance = aDirect.Dot(aDirect);

  if (aSqDistance > Precision::SquareConfusion()) // point belongs to triangle
  {
    const BVH_Vec3d aAB = aTrgVert2 - aTrgVert1;

    BVH_Vec3d aNorm;
    if (aTrgVert2.IsEqual(aTrgVert3)) // is this degenerate triangle (= segment)
    {
      const BVH_Vec3d aAP = myObject - aTrgVert1;
      aNorm               = BVH_Vec3d::Cross(BVH_Vec3d::Cross(aAP, aAB), aAB);
    }
    else
    {
      const BVH_Vec3d aAC = aTrgVert3 - aTrgVert1;
      aNorm               = BVH_Vec3d::Cross(aAB, aAC);
    }

    Standard_Real aNormSqLen = aNorm.Dot(aNorm);

    // check if the distance is under perpendicular
    const BVH_Vec3d aCrossCross      = BVH_Vec3d::Cross(aDirect, aNorm);
    Standard_Real   aCrossCrossSqLen = aCrossCross.Dot(aCrossCross);
    if (aCrossCrossSqLen > Precision::SquareConfusion() * aSqDistance * aNormSqLen)
    {
      // the distance is not under perpendicular
      if (myMinDistance - sqrt(aSqDistance) > Precision::Confusion())
      {
        myMinDistance  = sqrt(aSqDistance);
        myMinDistPoint = aNearestPnt;
        myMinPrjState  = aPrjState;
      }

      return Standard_False;
    }
  }

  // the distance is under perpendicular
  if (myDistance - sqrt(aSqDistance) > Precision::Confusion())
  {
    myDistance     = sqrt(aSqDistance);
    myExtremaPoint = aNearestPnt;
    myExtPrjState  = aPrjState;

    return Standard_True;
  }

  return Standard_False;
}

//=======================================================================
// function : ComputeDistance
// purpose  : Computes the distance between object and BVH tree
//=======================================================================
Standard_Real BRepExtrema_ProximityDistTool::ComputeDistance()
{
  myIsDone = this->Select() > 0;

  if (!myIsDone)
  {
    if (myMinDistance < std::numeric_limits<Standard_Real>::max())
    {
      myExtremaPoint = myMinDistPoint;
      myExtPrjState  = myMinPrjState;
      myIsDone       = true;
    }

    myDistance = myMinDistance;
  }

  return myDistance;
}

//=======================================================================
// function : IsNodeOnBorder
// purpose  : Returns true if the node is on the boarder
//=======================================================================
Standard_Boolean BRepExtrema_ProximityDistTool::IsNodeOnBorder(
  const Standard_Integer            theNodeIdx,
  const Handle(Poly_Triangulation)& theTr)
{
  Poly_Connect aPolyConnect(theTr);

  Standard_Integer aContTrg; // index of triangle containing exploring node
  for (aPolyConnect.Initialize(theNodeIdx); aPolyConnect.More(); aPolyConnect.Next())
  {
    aContTrg = aPolyConnect.Value();

    Standard_Integer aContTrgNodes[3];
    // clang-format off
    theTr->Triangle (aContTrg).Get (aContTrgNodes[0], aContTrgNodes[1], aContTrgNodes[2]); //indices of nodes of the triangle

    Standard_Integer aAdjTrg[3];
    aPolyConnect.Triangles (aContTrg, aAdjTrg[0], aAdjTrg[1], aAdjTrg[2]); //indices of adjacent triangles
    // clang-format on

    for (Standard_Integer j = 0; j < 3; j++)
    {
      Standard_Integer k = (j + 1) % 3;
      if (aAdjTrg[j] == 0) // free segment of triangle
      {
        // aContTrgNodes[j], aContTrgNodes[k] are ends of free segment and it is a part of border
        if (aContTrgNodes[j] == theNodeIdx || aContTrgNodes[k] == theNodeIdx)
        {
          return Standard_True;
        }
      }
    }
  }

  return Standard_False;
}

//=======================================================================
// function : IsEdgeOnBorder
// purpose  : Returns true if the edge is on the boarder
//=======================================================================
Standard_Boolean BRepExtrema_ProximityDistTool::IsEdgeOnBorder(
  const Standard_Integer            theTrgIdx,
  const Standard_Integer            theFirstEdgeNodeIdx,
  const Standard_Integer            theSecondEdgeNodeIdx,
  const Handle(Poly_Triangulation)& theTr)
{
  Poly_Connect aPolyConnect(theTr);

  Standard_Integer aAdjTrg[3];
  // clang-format off
  aPolyConnect.Triangles (theTrgIdx, aAdjTrg[0], aAdjTrg[1], aAdjTrg[2]); //indices of adjacent triangles
  // clang-format on

  for (Standard_Integer j = 0; j < 3; j++)
  {
    Standard_Integer k = (j + 1) % 3;
    if (aAdjTrg[j] == 0) // free segment of triangle
    {
      // are ends of free segment and it is a part of border
      if (j == theFirstEdgeNodeIdx && k == theSecondEdgeNodeIdx)
      {
        return Standard_True;
      }
    }
  }

  return Standard_False;
}

//=======================================================================
// function : defineStatusProxPnt1
// purpose  : Defines the status of proximity point from 1st BVH
//=======================================================================
void BRepExtrema_ProximityDistTool::defineStatusProxPnt1()
{
  if (myIsProxVtx1FromAddSet)
  {
    myPntStatus1 = myAddStatus1[myProxVtxIdx1];
    return;
  }

  Standard_Integer aFaceID1 = mySet1->GetShapeIDOfVtx(myProxVtxIdx1);

  if (myShapeList1(aFaceID1).ShapeType() == TopAbs_EDGE)
  {
    const BVH_Array3d& aVertices1 = mySet1->GetVertices();
    Standard_Integer   aVtxSize   = (Standard_Integer)aVertices1.size();
    Standard_Integer   aLastIdx   = aVtxSize - 1;

    if ((aVertices1[0] - aVertices1[aLastIdx]).Modulus() < Precision::Confusion()) // if closed
    {
      myPntStatus1 = ProxPnt_Status_MIDDLE;
      return;
    }

    if (myProxVtxIdx1 == 0 || myProxVtxIdx1 == aLastIdx)
    {
      myPntStatus1 = ProxPnt_Status_BORDER;
    }
    else
    {
      myPntStatus1 = ProxPnt_Status_MIDDLE;
    }
  }
  else if (myShapeList1(aFaceID1).ShapeType() == TopAbs_FACE)
  {
    Standard_Integer aNodeIdx = mySet1->GetVtxIdxInShape(myProxVtxIdx1) + 1;

    TopLoc_Location            aLocation;
    const TopoDS_Face&         aF  = TopoDS::Face(myShapeList1(aFaceID1));
    Handle(Poly_Triangulation) aTr = BRep_Tool::Triangulation(aF, aLocation);

    if (IsNodeOnBorder(aNodeIdx, aTr))
    {
      myPntStatus1 = ProxPnt_Status_BORDER;
    }
    else
    {
      myPntStatus1 = ProxPnt_Status_MIDDLE;
    }
  }
}

//=======================================================================
// function : defineStatusProxPnt2
// purpose  : Defines the status of proximity point from 2nd BVH
//=======================================================================
void BRepExtrema_ProximityDistTool::defineStatusProxPnt2()
{
  Standard_Integer aTrgIdx  = myProxPrjState.GetTrgIdx();
  Standard_Integer aFaceID2 = mySet2->GetFaceID(aTrgIdx);

  if (myShapeList2(aFaceID2).ShapeType() == TopAbs_EDGE)
  {
    if (myProxPrjState.GetPrjState() == BVH_PrjState::BVH_PrjStateInTriangle_INNER)
    {
      return;
    }
    else
    {
      const BVH_Array3d& aVertices2 = mySet2->GetVertices();
      Standard_Integer   aVtxSize   = (Standard_Integer)aVertices2.size();
      Standard_Integer   aLastIdx   = aVtxSize - 1;

      if ((aVertices2[0] - aVertices2[aLastIdx]).Modulus() < Precision::Confusion()) // if closed
      {
        myPntStatus2 = ProxPnt_Status_MIDDLE;
        return;
      }

      NCollection_Array1<Standard_Integer> aVtxIndicesOfTrg;
      mySet2->GetVtxIndices(aTrgIdx, aVtxIndicesOfTrg);

      Standard_Integer aFirstNodeNum = myProxPrjState.GetNumberOfFirstNode();
      Standard_Integer aFirstVtxIdx  = aVtxIndicesOfTrg[aFirstNodeNum];

      if (myProxPrjState.GetPrjState() == BVH_PrjState::BVH_PrjStateInTriangle_VERTEX)
      {
        if (aFirstVtxIdx == 0 || aFirstVtxIdx == aLastIdx)
        {
          myPntStatus2 = ProxPnt_Status_BORDER;
        }
        else
        {
          myPntStatus2 = ProxPnt_Status_MIDDLE;
        }
      }
      else if (myProxPrjState.GetPrjState() == BVH_PrjState::BVH_PrjStateInTriangle_EDGE)
      {
        Standard_Integer aLastNodeNum = myProxPrjState.GetNumberOfLastNode();
        Standard_Integer aLastVtxIdx  = aVtxIndicesOfTrg[aLastNodeNum];

        // it could be border only in case projection is on a degenerated edge
        if (aFirstVtxIdx == aLastVtxIdx && (aFirstVtxIdx == 0 || aFirstVtxIdx == aLastIdx))
        {
          myPntStatus2 = ProxPnt_Status_BORDER;
        }
        else
        {
          myPntStatus2 = ProxPnt_Status_MIDDLE;
        }
      }
    }
  }
  else if (myShapeList2(aFaceID2).ShapeType() == TopAbs_FACE)
  {
    if (myProxPrjState.GetPrjState() == BVH_PrjState::BVH_PrjStateInTriangle_INNER)
    {
      myPntStatus2 = ProxPnt_Status_MIDDLE;
    }
    else
    {
      TopLoc_Location            aLocation;
      const TopoDS_Face&         aF  = TopoDS::Face(myShapeList2(aFaceID2));
      Handle(Poly_Triangulation) aTr = BRep_Tool::Triangulation(aF, aLocation);

      NCollection_Array1<Standard_Integer> aVtxIndicesOfTrg;
      mySet2->GetVtxIndices(aTrgIdx, aVtxIndicesOfTrg);

      if (myProxPrjState.GetPrjState() == BVH_PrjState::BVH_PrjStateInTriangle_VERTEX)
      {
        Standard_Integer aNodeNum = myProxPrjState.GetNumberOfFirstNode();
        Standard_Integer aNodeIdx = mySet2->GetVtxIdxInShape(aVtxIndicesOfTrg[aNodeNum]) + 1;

        if (IsNodeOnBorder(aNodeIdx, aTr))
        {
          myPntStatus2 = ProxPnt_Status_BORDER;
        }
        else
        {
          myPntStatus2 = ProxPnt_Status_MIDDLE;
        }
      }
      else if (myProxPrjState.GetPrjState() == BVH_PrjState::BVH_PrjStateInTriangle_EDGE)
      {
        Standard_Integer aTrgIdxInShape = mySet2->GetTrgIdxInShape(aTrgIdx) + 1;

        if (IsEdgeOnBorder(aTrgIdxInShape,
                           myProxPrjState.GetNumberOfFirstNode(),
                           myProxPrjState.GetNumberOfLastNode(),
                           aTr))
        {
          myPntStatus2 = ProxPnt_Status_BORDER;
        }
        else
        {
          myPntStatus2 = ProxPnt_Status_MIDDLE;
        }
      } // else if (myProxPrjState.GetPrjState() == BVH_PrjState::BVH_PrjStateInTriangle_EDGE)
    }
  } // else if (myShapeList1 (aFaceID1).ShapeType() == TopAbs_FACE)
}

//=======================================================================
// function : DefineStatusProxPnt
// purpose  : Defines the status of proximity points
//=======================================================================
void BRepExtrema_ProximityDistTool::DefineStatusProxPnt()
{
  // define the status of proximity point from 1st BVH
  defineStatusProxPnt1();

  // define the status of proximity point from 2nd BVH
  defineStatusProxPnt2();
}