Integration of OCCT 6.5.0 from SVN

src/BRepFill/BRepFill_LocationLaw.cxx

@@ -0,0 +1,635 @@
+// File:	BRepFill_LocationLaw.cxx
+// Created:	Wed Jan 14 14:41:23 1998
+// Author:	Philippe MANGIN
+//		<pmn@sgi29>
+
+
+#include <BRepFill_LocationLaw.ixx>
+
+#include <BRepTools_WireExplorer.hxx>
+#include <BRep_Tool.hxx>
+#include <BRep_Builder.hxx>
+#include <BRepAdaptor_Curve.hxx>
+#include <Adaptor3d_HCurve.hxx>
+#include <TopoDS.hxx>
+#include <TopoDS_Edge.hxx>
+#include <TopExp.hxx>
+#include <TopLoc_Location.hxx>
+
+#include <GeomFill_LocationLaw.hxx>
+#include <gp_Vec.hxx>
+#include <gp_Mat.hxx>
+#include <gp_XYZ.hxx>
+#include <gp_Trsf.hxx>
+#include <GCPnts_AbscissaPoint.hxx>
+#include <TColgp_Array1OfPnt2d.hxx>
+#include <TColgp_Array1OfVec2d.hxx>
+#include <TColStd_SequenceOfInteger.hxx>
+#include <Precision.hxx>
+
+
+//=======================================================================
+//function : Norm
+//purpose  : Norme d'une Matrice
+//=======================================================================
+
+static Standard_Real Norm(const gp_Mat& M) {
+  Standard_Real R, Norme;
+  gp_XYZ Coord;
+  Coord = M.Row(1);
+  Norme = Abs(Coord.X()) + Abs(Coord.Y())+ Abs(Coord.Z());
+  Coord = M.Row(2);
+  R = Abs(Coord.X()) + Abs(Coord.Y())+ Abs(Coord.Z());
+  if (R>Norme) Norme = R;
+  Coord = M.Row(3);
+  R = Abs(Coord.X()) + Abs(Coord.Y())+ Abs(Coord.Z());
+  if (R>Norme) Norme = R;
+
+  return Norme;
+}
+
+//=======================================================================
+//function : ToG0
+//purpose  : Cacul une tranformation T tq T.M2 = M1
+//=======================================================================
+
+static void ToG0(const gp_Mat& M1, const gp_Mat& M2, gp_Mat& T) {
+  T =  M2.Inverted();
+  T *= M1;
+}
+			  
+//=======================================================================
+//function : BRepFill_LocationLaw
+//purpose  : 
+//=======================================================================
+
+void BRepFill_LocationLaw::Init(const TopoDS_Wire& Path)
+ 
+{
+  Standard_Integer NbEdge;
+  BRepTools_WireExplorer wexp;
+// Class BRep_Tool without fields and without Constructor :
+//  BRep_Tool B;
+  TopoDS_Edge E;
+
+  myPath = Path;
+  myTol = 1.e-4;
+
+  for (NbEdge=0, wexp.Init(myPath); 
+       wexp.More(); wexp.Next()) 
+//    if (! B.Degenerated(wexp.Current())) NbEdge++;
+    if (! BRep_Tool::Degenerated(wexp.Current())) NbEdge++;
+  
+
+  myLaws = new (GeomFill_HArray1OfLocationLaw)(1, NbEdge);
+  myLength = new (TColStd_HArray1OfReal) (1, NbEdge+1);
+  myLength->Init(-1.);
+  myLength->SetValue(1, 0.);
+  myEdges = new (TopTools_HArray1OfShape) (1, NbEdge);
+  myDisc.Nullify();
+  TangentIsMain();
+}
+
+//=======================================================================
+//function : GetStatus
+//purpose  : 
+//=======================================================================
+ GeomFill_PipeError BRepFill_LocationLaw::GetStatus() const
+{
+  Standard_Integer ii, N = myLaws->Length();
+  GeomFill_PipeError Status =  GeomFill_PipeOk;
+  for (ii=1; ii<=N && (Status == GeomFill_PipeOk); ii++) {
+    Status = myLaws->Value(ii)->ErrorStatus();
+  }
+  return  Status;
+}
+
+//=======================================================================
+//function : TangentIsMain
+//purpose  : 
+//=======================================================================
+void BRepFill_LocationLaw::TangentIsMain() 
+{
+  myType = 1;
+}
+
+//=======================================================================
+//function : NormalIsMain
+//purpose  : 
+//=======================================================================
+void BRepFill_LocationLaw::NormalIsMain() 
+{
+  myType = 2;
+}
+
+//=======================================================================
+//function : BiNormalIsMain
+//purpose  : 
+//=======================================================================
+void BRepFill_LocationLaw::BiNormalIsMain() 
+{
+  myType = 3;
+}
+
+//=======================================================================
+//function : TransformInCompatibleLaw
+//purpose  : Mise en continuite des loi
+//=======================================================================
+ void BRepFill_LocationLaw::TransformInCompatibleLaw(const Standard_Real TolAngular)
+{
+
+  Standard_Real First, Last, Angle;
+  Standard_Integer ipath;
+  gp_Mat Trsf, M1, M2;
+  gp_Vec V, T1, T2, N1, N2;
+  gp_XYZ OZ(0, 0, 1);
+
+  myLaws->Value(1)->GetDomain(First, Last);
+
+  for (ipath=2; ipath<=myLaws->Length(); ipath++) {
+    myLaws->Value(ipath-1)->D0(Last, M1, V);
+    myLaws->Value(ipath)->GetDomain(First, Last);
+    myLaws->Value(ipath)->D0(First, M2, V);
+    T1.SetXYZ(M1.Column(3));
+    T2.SetXYZ(M2.Column(3));
+    N1.SetXYZ(M1.Column(1));
+    N2.SetXYZ(M2.Column(1));
+    if (T1.IsParallel(T2, TolAngular ) && 
+	!T1.IsOpposite(T2, TolAngular)) { // Correction G0
+      ToG0(M1, M2, Trsf);
+    }
+    else {
+      Standard_Real alpha;
+      gp_Vec cross(T1);
+      cross.Cross(T2);
+      alpha = T2.AngleWithRef(T1, cross);
+      gp_Ax1 axe(gp::Origin(), cross.XYZ());
+      N2.Rotate(axe, alpha); 
+
+#if DEB
+      if (N2.Dot(T1) > 1.e-9) {
+	cout << "Imprecision dans TransformInCompatibleLaw" << endl;
+        cout << "--- T1.R(N2) = " << N2.Dot(T1) << endl;
+	gp_Vec tt;
+	tt = T1;
+	tt.Rotate(axe, alpha);
+	cout << "--- T1.R(T2) = " << tt.Dot(T1) << endl;
+	cout << "--- R(N2).R(T2) = " << N2.Dot(tt) << endl;
+      }      
+#endif
+      Angle = N2.AngleWithRef(N1, T1);
+      Trsf.SetRotation(OZ, Angle);
+    }
+    myLaws->Value(ipath)->SetTrsf(Trsf);
+  }
+} 
+
+//=======================================================================
+//function : TransformInG0Law
+//purpose  : Mise en continuite des loi
+//=======================================================================
+ void BRepFill_LocationLaw::TransformInG0Law()
+{
+
+  Standard_Real First, Last;
+  Standard_Integer ipath;
+  gp_Mat  M1, M2, aux;//,Trsf
+  gp_Vec V;
+  myLaws->Value(1)->GetDomain(First, Last);
+  for (ipath=2; ipath<=myLaws->Length(); ipath++) {
+    myLaws->Value(ipath-1)->D0(Last, M1, V);
+    myLaws->Value(ipath)->GetDomain(First, Last);
+    myLaws->Value(ipath)->D0(First, M2, V);
+    ToG0(M1, M2, aux);
+    myLaws->Value(ipath)->SetTrsf(aux);
+  }
+  
+  // La loi est elle periodique ?
+  if  (myPath.Closed()) {
+    myLaws->Value(myLaws->Length())->D0(Last, M1, V);
+    myLaws->Value(1)->GetDomain(First, Last);
+    myLaws->Value(1)->D0(First, M2, V);
+  }
+}
+
+//=======================================================================
+//function : DeleteTransform
+//purpose  : Supprime la Mise en continuite des loi
+//=======================================================================
+ void BRepFill_LocationLaw::DeleteTransform()
+{
+  gp_Mat Id;
+  Id.SetIdentity();
+  for (Standard_Integer ii=1; ii<=myEdges->Length(); ii++) {
+    myLaws->ChangeValue(ii)->SetTrsf(Id);
+  }
+  myDisc.Nullify();
+}
+
+//=======================================================================
+//function : NbHoles
+//purpose  : Rechecherche des "Trous"
+//=======================================================================
+ Standard_Integer BRepFill_LocationLaw::NbHoles(const Standard_Real Tol) 
+{
+  if (myDisc.IsNull()) {
+    TColStd_SequenceOfInteger Seq;
+    Standard_Integer ii, NbDisc;
+    for (ii=2, NbDisc=-1; ii<=myLaws->Length()+1; ii++) {
+      if (IsG1(ii-1, Tol, 1.e-12) == -1) {
+	Seq.Append(ii);
+      }
+    }
+    NbDisc = Seq.Length();
+    if ( NbDisc > 0) {
+      myDisc = new (TColStd_HArray1OfInteger)(1, NbDisc);
+      for (ii=1; ii<=NbDisc; ii++)
+	 myDisc->SetValue(ii, Seq(ii));
+    }   
+  }
+  if (myDisc.IsNull()) return 0;
+  return myDisc->Length();
+
+}
+
+//=======================================================================
+//function : Holes
+//purpose  : 
+//=======================================================================
+ void BRepFill_LocationLaw::Holes(TColStd_Array1OfInteger& Disc) const
+{
+  if (!myDisc.IsNull()) {
+    for (Standard_Integer ii=1; ii<=myDisc->Length(); ii++)
+      Disc(ii) = myDisc->Value(ii);
+  }
+}
+
+//=======================================================================
+//function : NbLaw
+//purpose  : 
+//=======================================================================
+ Standard_Integer BRepFill_LocationLaw::NbLaw() const
+{
+  return myLaws->Length();
+}
+
+//=======================================================================
+//function : Law
+//purpose  : 
+//=======================================================================
+const Handle(GeomFill_LocationLaw)& 
+BRepFill_LocationLaw::Law(const Standard_Integer Index) const
+{
+  return myLaws->Value(Index);
+}
+
+//=======================================================================
+//function : Wire
+//purpose  : 
+//=======================================================================
+const TopoDS_Wire& BRepFill_LocationLaw::Wire() const
+{
+  return myPath;
+}
+
+//=======================================================================
+//function : Edge
+//purpose  : 
+//=======================================================================
+const TopoDS_Edge& BRepFill_LocationLaw::Edge(const Standard_Integer Index) const
+{
+  return TopoDS::Edge(myEdges->Value(Index));
+}
+
+//=======================================================================
+//function : Vertex
+//purpose  : 
+//=======================================================================
+ TopoDS_Vertex BRepFill_LocationLaw::Vertex(const Standard_Integer Index) const
+{
+  TopoDS_Edge E;
+  TopoDS_Vertex V;
+  if (Index <= myEdges->Length()) {
+    E = TopoDS::Edge(myEdges->Value(Index));
+    if (E.Orientation() == TopAbs_REVERSED) 
+       V = TopExp::LastVertex(E);
+    else V = TopExp::FirstVertex(E);
+  }
+  else if (Index == myEdges->Length()+1) {
+    E = TopoDS::Edge(myEdges->Value(Index-1));
+    if (E.Orientation() == TopAbs_REVERSED) 
+      V = TopExp::FirstVertex(E);
+    else V = TopExp::LastVertex(E);
+  }
+  return V;
+}
+
+//=======================================================================
+//function : PerformVertex
+//purpose  : Calcul un vertex du balayage a partir d'un vertex d'une section
+//           et de l'indice de l'edge dans la trajectoire
+//=======================================================================
+void BRepFill_LocationLaw::PerformVertex(const Standard_Integer Index,
+					 const TopoDS_Vertex& Input,
+					 const Standard_Real TolMin,
+					 TopoDS_Vertex& Output,
+					 const Standard_Integer ILoc) const
+{
+  BRep_Builder B;
+  Standard_Boolean IsBary = (ILoc == 0);
+  Standard_Real First, Last;
+  gp_Pnt P;
+  gp_Vec V1, V2;//, V;
+  gp_Mat M1, M2;
+
+  if (Index>0 && Index<myLaws->Length()) {
+    if (ILoc <=0) {
+      myLaws->Value(Index)->GetDomain(First, Last);
+      myLaws->Value(Index)->D0(Last, M1, V1);
+    }
+
+    if (ILoc >= 0) {
+      myLaws->Value(Index+1)->GetDomain(First, Last);
+      if (ILoc == 0)
+	myLaws->Value(Index+1)->D0(First, M2, V2);
+      else
+	myLaws->Value(Index+1)->D0(First, M1, V1);
+    }
+  }
+
+  if (Index == 0 || Index == myLaws->Length()) {
+    if (!myPath.Closed() || (IsG1(Index, TolMin) != 1)) {
+      IsBary = Standard_False;
+      if (Index == 0) {
+	myLaws->Value(1)->GetDomain(First, Last);
+	myLaws->Value(1)->D0(First, M1, V1);
+      }
+      else {
+	myLaws->Value(myLaws->Length())->GetDomain(First, Last);
+	myLaws->Value(myLaws->Length())->D0(Last, M1, V1);
+      } 
+    }
+    else {
+      if (ILoc <=0) {
+	myLaws->Value(myLaws->Length())->GetDomain(First, Last);
+	myLaws->Value(myLaws->Length())->D0(Last, M1, V1);
+      }
+
+      if (ILoc >=0) {      
+	myLaws->Value(1)->GetDomain(First, Last);
+	if (ILoc==0)
+	  myLaws->Value(1)->D0(First, M2, V2);
+	else
+	  myLaws->Value(1)->D0(First, M1, V1);
+      }
+    }
+  }
+
+  P = BRep_Tool::Pnt(Input);
+
+  if (IsBary) { 
+    gp_XYZ P1(P.XYZ()), P2(P.XYZ());
+    P1 *= M1;
+    P1 += V1.XYZ();
+    P2 *= M2;
+    P2 += V2.XYZ();
+  
+    P.ChangeCoord().SetLinearForm(0.5, P1, 0.5, P2);
+    P1 -= P2;
+    Standard_Real Tol =  P1.Modulus()/2;
+    Tol += TolMin;
+    B.MakeVertex(Output, P, Tol);
+  }
+  else {
+    P.ChangeCoord() *= M1;
+    P.ChangeCoord() += V1.XYZ();
+    B.MakeVertex(Output, P, TolMin);    
+  }
+  
+}
+
+//=======================================================================
+//function : CurvilinearBounds
+//purpose  : 
+//=======================================================================
+void BRepFill_LocationLaw::CurvilinearBounds(const Standard_Integer Index,
+					     Standard_Real& First, 
+					     Standard_Real& Last) const
+{
+  First = myLength->Value(Index);
+  Last  = myLength->Value(Index+1);
+  if (Last<0) { //Il faut effectuer le calcul
+    Standard_Integer ii, NbE = myEdges->Length();
+    Standard_Real Length, f, l;
+    GCPnts_AbscissaPoint AbsC;
+
+    for (ii=1, Length=0.; ii<=NbE; ii++) {
+      myLaws->Value(ii)->GetDomain(f, l);
+      Length += AbsC.Length(myLaws->Value(ii)->GetCurve()->GetCurve(), myTol);
+      myLength->SetValue(ii+1, Length);
+    }
+
+    First = myLength->Value(Index);
+    Last  = myLength->Value(Index+1); 
+  }
+}
+
+ Standard_Boolean BRepFill_LocationLaw::IsClosed() const
+{
+  return myPath.Closed();
+}
+
+//=======================================================================
+//function : IsG1
+//purpose  : Evalue la continuite de la loi en un vertex
+//=======================================================================
+ Standard_Integer 
+ BRepFill_LocationLaw::IsG1(const Standard_Integer Index,
+			    const Standard_Real SpatialTolerance,
+			    const Standard_Real AngularTolerance) const
+{
+  gp_Vec V1, DV1, V2, DV2;
+  gp_Mat M1, M2, DM1, DM2;
+  Standard_Real First, Last, EpsNul = 1.e-12;
+  Standard_Real TolEps = SpatialTolerance;
+#ifndef DEB
+  Standard_Boolean Ok_D1 = Standard_False;
+#else
+  Standard_Boolean Ok_D1;
+#endif
+  TopoDS_Vertex V;
+  TopoDS_Edge E;
+  TColgp_Array1OfPnt2d Bid1 (1,1);
+  TColgp_Array1OfVec2d Bid2 (1,1);
+  
+  if (Index>0 && Index<myLaws->Length()) {
+    myLaws->Value(Index)->GetDomain(First, Last);
+    Ok_D1 = myLaws->Value(Index)->D1(Last, M1, V1, DM1, DV1, 
+				     Bid1, Bid2);
+    if (!Ok_D1)  myLaws->Value(Index)->D0(Last, M1, V1);
+
+    myLaws->Value(Index+1)->GetDomain(First, Last);
+    if (Ok_D1)
+      Ok_D1 = myLaws->Value(Index+1)->D1(First, M2, V2, DM2, DV2, 
+					 Bid1, Bid2);
+    if (!Ok_D1)  myLaws->Value(Index+1)->D0(First, M2, V2);
+
+    E = TopoDS::Edge(myEdges->Value(Index+1));
+  }
+  if (Index == 0 || Index == myLaws->Length()) {
+    if (!myPath.Closed()) return -1;
+    myLaws->Value(myLaws->Length())->GetDomain(First, Last);
+    Ok_D1 = myLaws->Value(myLaws->Length())->D1(Last, M1, V1, DM1, DV1, 
+						Bid1, Bid2);
+    if (!Ok_D1)  myLaws->Value(myLaws->Length())->D0(Last, M1, V1);
+
+    myLaws->Value(1)->GetDomain(First, Last);
+    if (Ok_D1)
+      myLaws->Value(1)->D1(First, M2, V2, DM2, DV2, 
+			   Bid1, Bid2);
+    if (!Ok_D1)  myLaws->Value(1)->D0(First, M2, V2);
+
+    E = TopoDS::Edge(myEdges->Value(1));
+  }
+
+  if (E.Orientation() == TopAbs_REVERSED)
+    V = TopExp::LastVertex(E);
+  else 
+    V = TopExp::FirstVertex(E);
+  
+  TolEps += 2*BRep_Tool::Tolerance(V);
+
+  Standard_Boolean isG0 = Standard_True;
+  Standard_Boolean isG1 = Standard_True;
+  
+  if ((V1-V2).Magnitude() > TolEps) isG0 = Standard_False;
+  if (Norm(M1-M2) > SpatialTolerance) isG0 = Standard_False;
+  
+  if (!isG0) return -1;
+  if (!Ok_D1) return 0; // Pas de controle de la derive
+  
+  if ( (DV1.Magnitude()>EpsNul) && (DV2.Magnitude()>EpsNul)
+       && (DV1.Angle(DV2) > AngularTolerance) ) isG1 = Standard_False;
+
+  // Pour la suite, les test sont plutot empirique
+  Standard_Real Norm1 = Norm(DM1);
+  Standard_Real Norm2 = Norm(DM2);
+  // Si les 2 normes sont nulle c'est bon
+  if ((Norm1 > EpsNul) || (Norm2 > EpsNul)) {
+    // sinon on compare les matrice normalise
+    if ((Norm1 > EpsNul) && (Norm2 > EpsNul)) {
+      DM1 /= Norm1;
+      DM2 /= Norm2;
+      if (Norm(DM1 - DM2) > AngularTolerance) isG1 = Standard_False;
+    }
+    else isG1 = Standard_False; // 1 Null l'autre pas   
+  }
+  
+  if (isG1) return 1;
+  else return 0;
+}
+
+
+//=======================================================================
+//function : Parameter
+//purpose  : 
+//=======================================================================
+ void BRepFill_LocationLaw::Parameter(const Standard_Real Abcissa,
+				      Standard_Integer& Index,
+				      Standard_Real& U)
+{
+  Standard_Integer iedge, NbE=myEdges->Length();
+  Standard_Boolean Trouve =  Standard_False;
+
+  //Controle que les longueurs sont calcules
+  if (myLength->Value(NbE+1) < 0) {
+    Standard_Real f, l;
+    CurvilinearBounds(NbE, f, l);
+  }
+
+  // Recherche de l'interval
+  for (iedge=1; iedge<=NbE && !Trouve; ) {
+    if (myLength->Value(iedge+1) >= Abcissa) {
+      Trouve = Standard_True;
+    }
+    else iedge++;
+  }
+  
+  if (Trouve) {
+    Standard_Real f, l;
+    const Handle(GeomFill_LocationLaw)& Law =  myLaws->Value(iedge);
+    Law->GetDomain(f, l);
+
+    if  (Abcissa == myLength->Value(iedge+1)) {
+      U = l;
+    }
+    else if  (Abcissa == myLength->Value(iedge)) {
+      U = f;
+    } 
+    else {
+      GCPnts_AbscissaPoint 
+	AbsC(myTol, 
+	     myLaws->Value(iedge)->GetCurve()->GetCurve(), 
+	     Abcissa-myLength->Value(iedge), f);
+      U =  AbsC.Parameter();
+    }
+    Index = iedge;
+  }
+  else {
+    Index = 0;
+  } 
+}
+
+
+//=======================================================================
+//function : D0
+//purpose  : Positionement d'une section, a une abscisse curviligne donnee
+//=======================================================================
+ void BRepFill_LocationLaw::D0(const Standard_Real Abcissa,
+			       TopoDS_Shape& W)
+{
+  Standard_Real u;
+  Standard_Integer ind;
+  gp_Mat M;
+  gp_Vec V;
+
+  Parameter(Abcissa, ind, u);
+  if (ind != 0) {
+    // Positionement
+    myLaws->Value(ind)->D0(u, M, V);
+    gp_Trsf fila;
+    fila.SetValues(M(1,1), M(1,2), M(1,3), V.X(),
+		   M(2,1), M(2,2), M(2,3), V.Y(),
+		   M(3,1), M(3,2), M(3,3), V.Z(),
+		   1.e-12, 1.e-14);
+    TopLoc_Location Loc(fila);
+    W.Location(Loc.Multiplied(W.Location()));
+  }
+  else {
+    W.Nullify();
+#if DEB    
+    cout << "BRepFill_LocationLaw::D0 : Attention positionement hors borne" 
+         << endl;
+#endif
+  }
+}
+
+//=======================================================================
+//function : Abscissa
+//purpose  : Calcul l'abscisse d'un point
+//=======================================================================
+ Standard_Real BRepFill_LocationLaw::Abscissa(const Standard_Integer Index,
+					      const Standard_Real Param)
+{
+  GCPnts_AbscissaPoint AbsC;
+  Standard_Real Length = myLength->Value(Index);
+  if (Length < 0) {
+    Standard_Real bid;
+    CurvilinearBounds(Index, bid, Length);
+  }
+ 
+  Length += AbsC.Length(myLaws->Value(Index)->GetCurve()->GetCurve(),
+			myLaws->Value(Index)->GetCurve()->FirstParameter(),
+			Param, myTol);
+  return Length;
+}