3c20114bc4f8cf5b6d44252e950b77dae9f10628
[occt.git] / src / BRepExtrema / BRepExtrema_DistShapeShape.cxx
1 // Created on: 1996-04-22
2 // Created by: Herve LOUESSARD
3 // Copyright (c) 1996-1999 Matra Datavision
4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
5 //
6 // This file is part of Open CASCADE Technology software library.
7 //
8 // This library is free software; you can redistribute it and/or modify it under
9 // the terms of the GNU Lesser General Public License version 2.1 as published
10 // by the Free Software Foundation, with special exception defined in the file
11 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
12 // distribution for complete text of the license and disclaimer of any warranty.
13 //
14 // Alternatively, this file may be used under the terms of Open CASCADE
15 // commercial license or contractual agreement.
16
17 // Modified: Mps(10-04-97) portage WNT
18
19 #include <BRepExtrema_DistShapeShape.hxx>
20
21 #include <Standard_OStream.hxx>
22 #include <TopTools_IndexedMapOfShape.hxx>
23 #include <BRepBndLib.hxx>
24 #include <Bnd_Box.hxx>
25 #include <TopExp.hxx>
26 #include <BRepExtrema_DistanceSS.hxx>
27 #include <TopoDS.hxx>
28 #include <TopAbs.hxx>
29 #include <TopoDS_Vertex.hxx>
30 #include <TopoDS_Edge.hxx>
31 #include <TopoDS_Face.hxx>
32 #include <TopAbs_ShapeEnum.hxx>
33 #include <Precision.hxx>
34 #include <Bnd_SeqOfBox.hxx>
35 #include <BRepExtrema_UnCompatibleShape.hxx>
36 #include <BRep_Tool.hxx>
37 #include <BRepClass3d_SolidClassifier.hxx>
38 #include <NCollection_Comparator.hxx>
39 #include <NCollection_QuickSort.hxx>
40 #include <NCollection_Vector.hxx>
41 #include <StdFail_NotDone.hxx>
42
43 namespace
44 {
45
46   static void Decomposition(const TopoDS_Shape&         S,
47                             TopTools_IndexedMapOfShape& MapV,
48                             TopTools_IndexedMapOfShape& MapE,
49                             TopTools_IndexedMapOfShape& MapF)
50   {
51     MapV.Clear();
52     MapE.Clear();
53     MapF.Clear();
54     TopExp::MapShapes(S,TopAbs_VERTEX,MapV);
55     TopExp::MapShapes(S,TopAbs_EDGE,MapE);
56     TopExp::MapShapes(S,TopAbs_FACE,MapF);
57   }
58
59   static void BoxCalculation(const TopTools_IndexedMapOfShape& Map,
60                              Bnd_SeqOfBox&                     SBox)
61   {
62     for (Standard_Integer i = 1; i <= Map.Extent(); i++)
63     {
64       Bnd_Box box;
65       BRepBndLib::Add(Map(i), box);
66       SBox.Append(box);
67     }
68   }
69
70   inline Standard_Real DistanceInitiale(const TopoDS_Vertex V1,
71                                         const TopoDS_Vertex V2)
72   {
73     return (BRep_Tool::Pnt(V1).Distance(BRep_Tool::Pnt(V2)));
74   }
75
76   //! Pair of objects to check extrema.
77   struct BRepExtrema_CheckPair
78   {
79     Standard_Integer Index1;   //!< Index of the 1st sub-shape
80     Standard_Integer Index2;   //!< Index of the 2nd sub-shape
81     Standard_Real    Distance; //!< Distance between sub-shapes
82
83     //! Uninitialized constructor for collection.
84     BRepExtrema_CheckPair() {}
85
86     //! Creates new pair of sub-shapes.
87     BRepExtrema_CheckPair (Standard_Integer theIndex1,
88                            Standard_Integer theIndex2,
89                            Standard_Real    theDistance)
90     : Index1   (theIndex1),
91       Index2   (theIndex2),
92       Distance (theDistance) {}
93   };
94 }
95
96 template<>
97 class NCollection_Comparator<BRepExtrema_CheckPair>
98 {
99 public:
100
101   Standard_Boolean IsLower (const BRepExtrema_CheckPair& theLeft, const BRepExtrema_CheckPair& theRight) const
102   {
103     return theLeft.Distance < theRight.Distance;
104   }
105
106   Standard_Boolean IsGreater (const BRepExtrema_CheckPair& theLeft, const BRepExtrema_CheckPair& theRight) const
107   {
108     return theLeft.Distance > theRight.Distance;
109   }
110
111   Standard_Boolean IsEqual (const BRepExtrema_CheckPair& theLeft, const BRepExtrema_CheckPair& theRight) const
112   {
113     return theLeft.Distance == theRight.Distance;
114   }
115
116   Standard_Boolean IsLowerEqual (const BRepExtrema_CheckPair& theLeft, const BRepExtrema_CheckPair& theRight) const
117   {
118     return theLeft.Distance <= theRight.Distance;
119   }
120
121   Standard_Boolean IsGreaterEqual (const BRepExtrema_CheckPair& theLeft, const BRepExtrema_CheckPair& theRight) const
122   {
123     return theLeft.Distance >= theRight.Distance;
124   }
125 };
126
127 //=======================================================================
128 //function : DistanceMapMap
129 //purpose  : 
130 //=======================================================================
131
132 void BRepExtrema_DistShapeShape::DistanceMapMap (const TopTools_IndexedMapOfShape& theMap1,
133                                                  const TopTools_IndexedMapOfShape& theMap2,
134                                                  const Bnd_SeqOfBox&               theLBox1,
135                                                  const Bnd_SeqOfBox&               theLBox2)
136 {
137   NCollection_Vector<BRepExtrema_CheckPair> aPairList;
138   const Standard_Integer aCount1 = theMap1.Extent();
139   const Standard_Integer aCount2 = theMap2.Extent();
140   for (Standard_Integer anIdx1 = 1; anIdx1 <= aCount1; ++anIdx1)
141   {
142     for (Standard_Integer anIdx2 = 1; anIdx2 <= aCount2; ++anIdx2)
143     {
144       const Standard_Real aDist = theLBox1.Value (anIdx1).Distance (theLBox2.Value (anIdx2));
145       if (aDist < myDistRef - myEps || fabs (aDist - myDistRef) < myEps)
146       {
147         aPairList.Append (BRepExtrema_CheckPair (anIdx1, anIdx2, aDist));
148       }
149     }
150   }
151
152   NCollection_QuickSort<NCollection_Vector<BRepExtrema_CheckPair>, BRepExtrema_CheckPair>::Perform (aPairList, NCollection_Comparator<BRepExtrema_CheckPair>(),
153                                                                                                     aPairList.Lower(), aPairList.Upper());
154   for (NCollection_Vector<BRepExtrema_CheckPair>::Iterator aPairIter (aPairList);
155        aPairIter.More(); aPairIter.Next())
156   {
157     const BRepExtrema_CheckPair& aPair = aPairIter.Value();
158     if (aPair.Distance > myDistRef + myEps)
159     {
160       break; // early search termination
161     }
162
163     const Bnd_Box& aBox1 = theLBox1.Value (aPair.Index1);
164     const Bnd_Box& aBox2 = theLBox2.Value (aPair.Index2);
165
166     const TopoDS_Shape& aShape1 = theMap1 (aPair.Index1);
167     const TopoDS_Shape& aShape2 = theMap2 (aPair.Index2);
168
169     BRepExtrema_DistanceSS aDistTool (aShape1, aShape2, aBox1, aBox2, myDistRef, myEps);
170     if (aDistTool.IsDone())
171     {
172       if (aDistTool.DistValue() < myDistRef - myEps)
173       {
174         mySolutionsShape1.Clear();
175         mySolutionsShape2.Clear();
176
177         BRepExtrema_SeqOfSolution aSeq1 = aDistTool.Seq1Value();
178         BRepExtrema_SeqOfSolution aSeq2 = aDistTool.Seq2Value();
179
180         mySolutionsShape1.Append (aSeq1);
181         mySolutionsShape2.Append (aSeq2);
182
183         myDistRef = aDistTool.DistValue();
184       }
185       else if (fabs (aDistTool.DistValue() - myDistRef) < myEps)
186       {
187         BRepExtrema_SeqOfSolution aSeq1 = aDistTool.Seq1Value();
188         BRepExtrema_SeqOfSolution aSeq2 = aDistTool.Seq2Value();
189
190         mySolutionsShape1.Append (aSeq1);
191         mySolutionsShape2.Append (aSeq2);
192
193         if (myDistRef > aDistTool.DistValue())
194         {
195           myDistRef = aDistTool.DistValue();
196         }
197       }
198     }
199   }
200 }
201
202 //=======================================================================
203 //function : BRepExtrema_DistShapeShape
204 //purpose  : 
205 //=======================================================================
206
207 BRepExtrema_DistShapeShape::BRepExtrema_DistShapeShape()
208 : myDistRef (0.0),
209   myIsDone (Standard_False),
210   myInnerSol (Standard_False),
211   myEps (Precision::Confusion()),
212   myIsInitS1 (Standard_False),
213   myIsInitS2 (Standard_False),
214   myFlag (Extrema_ExtFlag_MINMAX),
215   myAlgo (Extrema_ExtAlgo_Grad)
216 {
217   //
218 }
219
220 //=======================================================================
221 //function : BRepExtrema_DistShapeShape
222 //purpose  : 
223 //=======================================================================
224 BRepExtrema_DistShapeShape::BRepExtrema_DistShapeShape(const TopoDS_Shape& Shape1,
225                                                        const TopoDS_Shape& Shape2,
226                                                        const Extrema_ExtFlag F,
227                                                        const Extrema_ExtAlgo A)
228 : myDistRef (0.0),
229   myIsDone (Standard_False),
230   myInnerSol (Standard_False),
231   myEps (Precision::Confusion()),
232   myIsInitS1 (Standard_False),
233   myIsInitS2 (Standard_False),
234   myFlag (F),
235   myAlgo (A)
236 {
237   LoadS1(Shape1);
238   LoadS2(Shape2);
239   Perform();
240 }
241
242 //=======================================================================
243 //function : BRepExtrema_DistShapeShape
244 //purpose  : 
245 //=======================================================================
246
247 BRepExtrema_DistShapeShape::BRepExtrema_DistShapeShape(const TopoDS_Shape& Shape1,
248                                                        const TopoDS_Shape& Shape2,
249                                                        const Standard_Real theDeflection,
250                                                        const Extrema_ExtFlag F,
251                                                        const Extrema_ExtAlgo A)
252 : myDistRef (0.0),
253   myIsDone (Standard_False),
254   myInnerSol (Standard_False),
255   myEps (theDeflection),
256   myIsInitS1 (Standard_False),
257   myIsInitS2 (Standard_False),
258   myFlag (F),
259   myAlgo (A)
260 {
261   LoadS1(Shape1);
262   LoadS2(Shape2);
263   Perform();
264 }
265
266 //=======================================================================
267 //function : LoadS1
268 //purpose  : 
269 //=======================================================================
270
271 void BRepExtrema_DistShapeShape::LoadS1 (const TopoDS_Shape& Shape1)
272 {
273   myShape1 = Shape1;
274   myIsInitS1 = Standard_False;
275   Decomposition (Shape1, myMapV1, myMapE1, myMapF1);
276 }
277
278 //=======================================================================
279 //function : LoadS2
280 //purpose  : 
281 //=======================================================================
282
283 void BRepExtrema_DistShapeShape::LoadS2 (const TopoDS_Shape& Shape2)
284 {
285   myShape2 = Shape2;
286   myIsInitS2 = Standard_False;
287   Decomposition (Shape2, myMapV2, myMapE2, myMapF2);
288 }
289
290 //=======================================================================
291 //function : Perform
292 //purpose  : 
293 //=======================================================================
294
295 Standard_Boolean BRepExtrema_DistShapeShape::Perform()
296 {
297   myIsDone=Standard_False;
298   myInnerSol=Standard_False;
299   mySolutionsShape1.Clear();
300   mySolutionsShape2.Clear();
301
302   if ( myShape1.IsNull() || myShape2.IsNull() )
303     return Standard_False;
304
305   TopoDS_Vertex V;
306   const Standard_Real tol = 0.001;
307
308   // Treatment of solids
309   const TopAbs_ShapeEnum Type1 = myShape1.ShapeType();
310   if ((Type1==TopAbs_SOLID) || (Type1 == TopAbs_COMPSOLID))
311   {
312     BRepClass3d_SolidClassifier Classi(myShape1);
313     const Standard_Integer nbv2 = myMapV2.Extent();
314     Standard_Integer nbv1 = 0;
315     while ( (nbv1<nbv2) && (!myInnerSol) )
316     {
317       nbv1++;
318       V = TopoDS::Vertex(myMapV2(nbv1));
319       const gp_Pnt &P = BRep_Tool::Pnt(V);
320       Classi.Perform(P,tol);
321       if (Classi.State()==TopAbs_IN)
322       {
323         myInnerSol = Standard_True;
324         myDistRef = 0.;
325         myIsDone = Standard_True; 
326         BRepExtrema_SolutionElem Sol(0,P,BRepExtrema_IsVertex,V);
327         mySolutionsShape1.Append(Sol);
328         mySolutionsShape2.Append(Sol);
329       }  
330     }
331   }
332   
333   const TopAbs_ShapeEnum Type2 = myShape2.ShapeType();
334   if (((Type2==TopAbs_SOLID) || (Type2==TopAbs_COMPSOLID)) && (!myInnerSol))
335   {
336     BRepClass3d_SolidClassifier Classi(myShape2);
337     const Standard_Integer nbv1 = myMapV1.Extent();
338     Standard_Integer nbv2 = 0;
339     while ( (nbv2<nbv1) && (!myInnerSol) )
340     {
341       nbv2++;
342       V = TopoDS::Vertex(myMapV1(nbv2));
343       const gp_Pnt &P = BRep_Tool::Pnt(V);
344       Classi.Perform(P,tol);
345       if (Classi.State()==TopAbs_IN) {
346         myInnerSol = Standard_True;
347         myDistRef = 0;
348         myIsDone = Standard_True; 
349         BRepExtrema_SolutionElem Sol (0,P,BRepExtrema_IsVertex,V);
350         mySolutionsShape1.Append(Sol);
351         mySolutionsShape2.Append(Sol);
352       }
353     }
354   }
355
356   if (!myInnerSol)
357   {
358     if (!myIsInitS1) // rebuild cached data for 1st shape
359     {
360       myBV1.Clear();
361       myBE1.Clear();
362       myBF1.Clear();
363
364       BoxCalculation (myMapV1, myBV1);
365       BoxCalculation (myMapE1, myBE1);
366       BoxCalculation (myMapF1, myBF1);
367
368       myIsInitS1 = Standard_True;
369     }
370
371     if (!myIsInitS2) // rebuild cached data for 2nd shape
372     {
373       myBV2.Clear();
374       myBE2.Clear();
375       myBF2.Clear();
376
377       BoxCalculation (myMapV2, myBV2);
378       BoxCalculation (myMapE2, myBE2);
379       BoxCalculation (myMapF2, myBF2);
380
381       myIsInitS2 = Standard_True;
382     }
383
384     if (myMapV1.Extent() && myMapV2.Extent())
385     {
386       TopoDS_Vertex V1 = TopoDS::Vertex(myMapV1(1));
387       TopoDS_Vertex V2 = TopoDS::Vertex(myMapV2(1));
388       myDistRef = DistanceInitiale(V1, V2);
389     }
390     else
391       myDistRef= 1.e30; //szv:!!!
392
393     DistanceMapMap (myMapV1, myMapV2, myBV1, myBV2);
394     DistanceMapMap (myMapV1, myMapE2, myBV1, myBE2);
395     DistanceMapMap (myMapE1, myMapV2, myBE1, myBV2);
396     DistanceMapMap (myMapV1, myMapF2, myBV1, myBF2);
397     DistanceMapMap (myMapF1, myMapV2, myBF1, myBV2);
398     DistanceMapMap (myMapE1, myMapE2, myBE1, myBE2);
399     DistanceMapMap (myMapE1, myMapF2, myBE1, myBF2);
400     DistanceMapMap (myMapF1, myMapE2, myBF1, myBE2);
401
402     if (fabs (myDistRef) > myEps)
403     {
404       DistanceMapMap (myMapF1, myMapF2, myBF1, myBF2);
405     }
406     
407     //  Modified by Sergey KHROMOV - Tue Mar  6 11:55:03 2001 Begin
408     Standard_Integer i = 1;
409     for (; i <= mySolutionsShape1.Length(); i++)
410       if (mySolutionsShape1.Value(i).Dist() > myDistRef + myEps)
411       {
412         mySolutionsShape1.Remove(i);
413         mySolutionsShape2.Remove(i);
414       }
415     //  Modified by Sergey KHROMOV - Tue Mar  6 11:55:04 2001 End
416     myIsDone = ( mySolutionsShape1.Length() > 0 );
417   }
418   return myIsDone;
419 }
420
421 //=======================================================================
422 //function : Value
423 //purpose  : 
424 //=======================================================================
425
426 Standard_Real BRepExtrema_DistShapeShape::Value() const 
427
428   if (!myIsDone)
429     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::Value: There's no solution ");
430
431   return myDistRef;
432 }
433
434 //=======================================================================
435 //function : SupportOnShape1
436 //purpose  : 
437 //=======================================================================
438
439 TopoDS_Shape BRepExtrema_DistShapeShape::SupportOnShape1(const Standard_Integer N) const
440
441   if (!myIsDone)
442     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::SupportOnShape1: There's no solution ");
443
444   const BRepExtrema_SolutionElem &sol = mySolutionsShape1.Value(N);
445   switch (sol.SupportKind())
446   {
447     case BRepExtrema_IsVertex : return sol.Vertex();
448     case BRepExtrema_IsOnEdge : return sol.Edge();
449     case BRepExtrema_IsInFace : return sol.Face();
450   }
451   return TopoDS_Shape();
452 }
453
454 //=======================================================================
455 //function : SupportOnShape2
456 //purpose  : 
457 //=======================================================================
458
459 TopoDS_Shape BRepExtrema_DistShapeShape::SupportOnShape2(const Standard_Integer N) const 
460
461   if (!myIsDone)
462     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::SupportOnShape2: There's no solution ");
463
464   const BRepExtrema_SolutionElem &sol = mySolutionsShape2.Value(N);
465   switch (sol.SupportKind())
466   {
467     case BRepExtrema_IsVertex : return sol.Vertex();
468     case BRepExtrema_IsOnEdge : return sol.Edge();
469     case BRepExtrema_IsInFace : return sol.Face();
470   }
471   return TopoDS_Shape();
472 }
473
474 //=======================================================================
475 //function : ParOnEdgeS1
476 //purpose  : 
477 //=======================================================================
478
479 void BRepExtrema_DistShapeShape::ParOnEdgeS1(const Standard_Integer N, Standard_Real& t) const 
480
481   if (!myIsDone)
482     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::ParOnEdgeS1: There's no solution");
483
484   const BRepExtrema_SolutionElem &sol = mySolutionsShape1.Value(N);
485   if (sol.SupportKind() != BRepExtrema_IsOnEdge)
486     BRepExtrema_UnCompatibleShape::Raise
487       ("BRepExtrema_DistShapeShape::ParOnEdgeS1: ParOnEdgeS1 is impossible without EDGE");
488
489   sol.EdgeParameter(t);
490 }
491
492 //=======================================================================
493 //function : ParOnEdgeS2
494 //purpose  : 
495 //=======================================================================
496
497 void BRepExtrema_DistShapeShape::ParOnEdgeS2(const Standard_Integer N,  Standard_Real& t) const 
498
499   if (!myIsDone)
500     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::ParOnEdgeS2: There's no solution");
501
502   const BRepExtrema_SolutionElem &sol = mySolutionsShape2.Value(N);
503   if (sol.SupportKind() != BRepExtrema_IsOnEdge)
504     BRepExtrema_UnCompatibleShape::Raise
505       ("BRepExtrema_DistShapeShape::ParOnEdgeS2: ParOnEdgeS2 is impossible without EDGE");
506  
507   sol.EdgeParameter(t);
508 }
509
510 //=======================================================================
511 //function : ParOnFaceS1
512 //purpose  : 
513 //=======================================================================
514
515 void BRepExtrema_DistShapeShape::ParOnFaceS1(const Standard_Integer N,  Standard_Real& u,  Standard_Real& v) const 
516
517   if (!myIsDone)
518     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::ParOnFaceS1: There's no solution");
519
520   const BRepExtrema_SolutionElem &sol = mySolutionsShape1.Value(N);
521   if (sol.SupportKind() != BRepExtrema_IsInFace)
522     BRepExtrema_UnCompatibleShape::Raise
523       ("BRepExtrema_DistShapeShape::ParOnFaceS1: ParOnFaceS1 is impossible without FACE");
524   
525   sol.FaceParameter(u, v);
526 }
527
528 //=======================================================================
529 //function : ParOnFaceS2
530 //purpose  : 
531 //=======================================================================
532
533 void BRepExtrema_DistShapeShape::ParOnFaceS2(const Standard_Integer N,  Standard_Real& u, Standard_Real& v) const 
534
535   if (!myIsDone)
536     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::ParOnFaceS2: There's no solution");
537
538   const BRepExtrema_SolutionElem &sol = mySolutionsShape2.Value(N);
539   if (sol.SupportKind() != BRepExtrema_IsInFace)
540     BRepExtrema_UnCompatibleShape::Raise
541       ("BRepExtrema_DistShapeShape::ParOnFaceS2:ParOnFaceS2 is impossible without FACE ");
542   
543   sol.FaceParameter(u, v);
544 }
545
546 //=======================================================================
547 //function : Dump
548 //purpose  : 
549 //=======================================================================
550
551 void BRepExtrema_DistShapeShape::Dump(Standard_OStream& o) const 
552 {
553   Standard_Integer i;
554   Standard_Real r1,r2;
555   
556   o<< "the distance  value is :  " << Value()<<endl;
557   o<< "the number of solutions is :"<<NbSolution()<<endl;
558   o<<endl;
559   for (i=1;i<=NbSolution();i++)
560   {
561     o<<"solution number "<<i<<": "<< endl;
562     o<<"the type of the solution on the first shape is " <<Standard_Integer( SupportTypeShape1(i)) <<endl; 
563     o<<"the type of the solution on the second shape is "<<Standard_Integer( SupportTypeShape2(i))<< endl;
564     o<< "the coordinates of  the point on the first shape are: "<<endl; 
565     o<<"X=" <<PointOnShape1(i).X()<<" Y=" <<PointOnShape1(i).Y()<<" Z="<<PointOnShape1(i).Z()<<endl;
566     o<< "the coordinates of  the point on the second shape are: "<<endl; 
567     o<<"X="<< PointOnShape2(i).X()<< " Y="<<PointOnShape2(i).Y()<<" Z="<< PointOnShape2(i).Z()<<endl;
568     
569     switch (SupportTypeShape1(i))
570     {
571       case BRepExtrema_IsVertex:
572         break;
573       case BRepExtrema_IsOnEdge:
574         ParOnEdgeS1(i,r1);
575         o << "parameter on the first edge :  t= " << r1 << endl;
576         break;
577       case BRepExtrema_IsInFace:
578         ParOnFaceS1(i,r1,r2);
579         o << "parameters on the first face :  u= " << r1 << " v=" <<  r2 << endl;
580         break;
581     }
582     switch (SupportTypeShape2(i))
583     {
584       case BRepExtrema_IsVertex:
585         break;
586       case BRepExtrema_IsOnEdge:
587         ParOnEdgeS2(i,r1);
588         o << "parameter on the second edge : t=" << r1 << endl;
589         break;
590       case BRepExtrema_IsInFace:
591         ParOnFaceS2(i,r1,r2);
592         o << "parameters on the second face : u= " << r1 << " v=" <<  r2 << endl;
593         break;
594     }
595     o<<endl;
596   } 
597 }