0027915: Foundation Classes - remove the class NCollection_QuickSort
[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_Vector.hxx>
40 #include <StdFail_NotDone.hxx>
41
42 #include <algorithm>
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   // Used by std::sort function
96   static Standard_Boolean BRepExtrema_CheckPair_Comparator (const BRepExtrema_CheckPair& theLeft,
97                                                             const BRepExtrema_CheckPair& theRight)
98   {
99     return (theLeft.Distance < theRight.Distance);
100   }
101 }
102
103 //=======================================================================
104 //function : DistanceMapMap
105 //purpose  : 
106 //=======================================================================
107
108 void BRepExtrema_DistShapeShape::DistanceMapMap (const TopTools_IndexedMapOfShape& theMap1,
109                                                  const TopTools_IndexedMapOfShape& theMap2,
110                                                  const Bnd_SeqOfBox&               theLBox1,
111                                                  const Bnd_SeqOfBox&               theLBox2)
112 {
113   NCollection_Vector<BRepExtrema_CheckPair> aPairList;
114   const Standard_Integer aCount1 = theMap1.Extent();
115   const Standard_Integer aCount2 = theMap2.Extent();
116   for (Standard_Integer anIdx1 = 1; anIdx1 <= aCount1; ++anIdx1)
117   {
118     for (Standard_Integer anIdx2 = 1; anIdx2 <= aCount2; ++anIdx2)
119     {
120       const Standard_Real aDist = theLBox1.Value (anIdx1).Distance (theLBox2.Value (anIdx2));
121       if (aDist < myDistRef - myEps || fabs (aDist - myDistRef) < myEps)
122       {
123         aPairList.Append (BRepExtrema_CheckPair (anIdx1, anIdx2, aDist));
124       }
125     }
126   }
127
128   std::stable_sort(aPairList.begin(), aPairList.end(), BRepExtrema_CheckPair_Comparator);
129
130   for (NCollection_Vector<BRepExtrema_CheckPair>::Iterator aPairIter (aPairList);
131        aPairIter.More(); aPairIter.Next())
132   {
133     const BRepExtrema_CheckPair& aPair = aPairIter.Value();
134     if (aPair.Distance > myDistRef + myEps)
135     {
136       break; // early search termination
137     }
138
139     const Bnd_Box& aBox1 = theLBox1.Value (aPair.Index1);
140     const Bnd_Box& aBox2 = theLBox2.Value (aPair.Index2);
141
142     const TopoDS_Shape& aShape1 = theMap1 (aPair.Index1);
143     const TopoDS_Shape& aShape2 = theMap2 (aPair.Index2);
144
145     BRepExtrema_DistanceSS aDistTool (aShape1, aShape2, aBox1, aBox2, myDistRef, myEps);
146     if (aDistTool.IsDone())
147     {
148       if (aDistTool.DistValue() < myDistRef - myEps)
149       {
150         mySolutionsShape1.Clear();
151         mySolutionsShape2.Clear();
152
153         BRepExtrema_SeqOfSolution aSeq1 = aDistTool.Seq1Value();
154         BRepExtrema_SeqOfSolution aSeq2 = aDistTool.Seq2Value();
155
156         mySolutionsShape1.Append (aSeq1);
157         mySolutionsShape2.Append (aSeq2);
158
159         myDistRef = aDistTool.DistValue();
160       }
161       else if (fabs (aDistTool.DistValue() - myDistRef) < myEps)
162       {
163         BRepExtrema_SeqOfSolution aSeq1 = aDistTool.Seq1Value();
164         BRepExtrema_SeqOfSolution aSeq2 = aDistTool.Seq2Value();
165
166         mySolutionsShape1.Append (aSeq1);
167         mySolutionsShape2.Append (aSeq2);
168
169         if (myDistRef > aDistTool.DistValue())
170         {
171           myDistRef = aDistTool.DistValue();
172         }
173       }
174     }
175   }
176 }
177
178 //=======================================================================
179 //function : BRepExtrema_DistShapeShape
180 //purpose  : 
181 //=======================================================================
182
183 BRepExtrema_DistShapeShape::BRepExtrema_DistShapeShape()
184 : myDistRef (0.0),
185   myIsDone (Standard_False),
186   myInnerSol (Standard_False),
187   myEps (Precision::Confusion()),
188   myIsInitS1 (Standard_False),
189   myIsInitS2 (Standard_False),
190   myFlag (Extrema_ExtFlag_MINMAX),
191   myAlgo (Extrema_ExtAlgo_Grad)
192 {
193   //
194 }
195
196 //=======================================================================
197 //function : BRepExtrema_DistShapeShape
198 //purpose  : 
199 //=======================================================================
200 BRepExtrema_DistShapeShape::BRepExtrema_DistShapeShape(const TopoDS_Shape& Shape1,
201                                                        const TopoDS_Shape& Shape2,
202                                                        const Extrema_ExtFlag F,
203                                                        const Extrema_ExtAlgo A)
204 : myDistRef (0.0),
205   myIsDone (Standard_False),
206   myInnerSol (Standard_False),
207   myEps (Precision::Confusion()),
208   myIsInitS1 (Standard_False),
209   myIsInitS2 (Standard_False),
210   myFlag (F),
211   myAlgo (A)
212 {
213   LoadS1(Shape1);
214   LoadS2(Shape2);
215   Perform();
216 }
217
218 //=======================================================================
219 //function : BRepExtrema_DistShapeShape
220 //purpose  : 
221 //=======================================================================
222
223 BRepExtrema_DistShapeShape::BRepExtrema_DistShapeShape(const TopoDS_Shape& Shape1,
224                                                        const TopoDS_Shape& Shape2,
225                                                        const Standard_Real theDeflection,
226                                                        const Extrema_ExtFlag F,
227                                                        const Extrema_ExtAlgo A)
228 : myDistRef (0.0),
229   myIsDone (Standard_False),
230   myInnerSol (Standard_False),
231   myEps (theDeflection),
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 : LoadS1
244 //purpose  : 
245 //=======================================================================
246
247 void BRepExtrema_DistShapeShape::LoadS1 (const TopoDS_Shape& Shape1)
248 {
249   myShape1 = Shape1;
250   myIsInitS1 = Standard_False;
251   Decomposition (Shape1, myMapV1, myMapE1, myMapF1);
252 }
253
254 //=======================================================================
255 //function : LoadS2
256 //purpose  : 
257 //=======================================================================
258
259 void BRepExtrema_DistShapeShape::LoadS2 (const TopoDS_Shape& Shape2)
260 {
261   myShape2 = Shape2;
262   myIsInitS2 = Standard_False;
263   Decomposition (Shape2, myMapV2, myMapE2, myMapF2);
264 }
265
266 //=======================================================================
267 //function : Perform
268 //purpose  : 
269 //=======================================================================
270
271 Standard_Boolean BRepExtrema_DistShapeShape::Perform()
272 {
273   myIsDone=Standard_False;
274   myInnerSol=Standard_False;
275   mySolutionsShape1.Clear();
276   mySolutionsShape2.Clear();
277
278   if ( myShape1.IsNull() || myShape2.IsNull() )
279     return Standard_False;
280
281   TopoDS_Vertex V;
282   const Standard_Real tol = 0.001;
283
284   // Treatment of solids
285   const TopAbs_ShapeEnum Type1 = myShape1.ShapeType();
286   if ((Type1==TopAbs_SOLID) || (Type1 == TopAbs_COMPSOLID))
287   {
288     BRepClass3d_SolidClassifier Classi(myShape1);
289     const Standard_Integer nbv2 = myMapV2.Extent();
290     Standard_Integer nbv1 = 0;
291     while ( (nbv1<nbv2) && (!myInnerSol) )
292     {
293       nbv1++;
294       V = TopoDS::Vertex(myMapV2(nbv1));
295       const gp_Pnt &P = BRep_Tool::Pnt(V);
296       Classi.Perform(P,tol);
297       if (Classi.State()==TopAbs_IN)
298       {
299         myInnerSol = Standard_True;
300         myDistRef = 0.;
301         myIsDone = Standard_True; 
302         BRepExtrema_SolutionElem Sol(0,P,BRepExtrema_IsVertex,V);
303         mySolutionsShape1.Append(Sol);
304         mySolutionsShape2.Append(Sol);
305       }  
306     }
307   }
308   
309   const TopAbs_ShapeEnum Type2 = myShape2.ShapeType();
310   if (((Type2==TopAbs_SOLID) || (Type2==TopAbs_COMPSOLID)) && (!myInnerSol))
311   {
312     BRepClass3d_SolidClassifier Classi(myShape2);
313     const Standard_Integer nbv1 = myMapV1.Extent();
314     Standard_Integer nbv2 = 0;
315     while ( (nbv2<nbv1) && (!myInnerSol) )
316     {
317       nbv2++;
318       V = TopoDS::Vertex(myMapV1(nbv2));
319       const gp_Pnt &P = BRep_Tool::Pnt(V);
320       Classi.Perform(P,tol);
321       if (Classi.State()==TopAbs_IN) {
322         myInnerSol = Standard_True;
323         myDistRef = 0;
324         myIsDone = Standard_True; 
325         BRepExtrema_SolutionElem Sol (0,P,BRepExtrema_IsVertex,V);
326         mySolutionsShape1.Append(Sol);
327         mySolutionsShape2.Append(Sol);
328       }
329     }
330   }
331
332   if (!myInnerSol)
333   {
334     if (!myIsInitS1) // rebuild cached data for 1st shape
335     {
336       myBV1.Clear();
337       myBE1.Clear();
338       myBF1.Clear();
339
340       BoxCalculation (myMapV1, myBV1);
341       BoxCalculation (myMapE1, myBE1);
342       BoxCalculation (myMapF1, myBF1);
343
344       myIsInitS1 = Standard_True;
345     }
346
347     if (!myIsInitS2) // rebuild cached data for 2nd shape
348     {
349       myBV2.Clear();
350       myBE2.Clear();
351       myBF2.Clear();
352
353       BoxCalculation (myMapV2, myBV2);
354       BoxCalculation (myMapE2, myBE2);
355       BoxCalculation (myMapF2, myBF2);
356
357       myIsInitS2 = Standard_True;
358     }
359
360     if (myMapV1.Extent() && myMapV2.Extent())
361     {
362       TopoDS_Vertex V1 = TopoDS::Vertex(myMapV1(1));
363       TopoDS_Vertex V2 = TopoDS::Vertex(myMapV2(1));
364       myDistRef = DistanceInitiale(V1, V2);
365     }
366     else
367       myDistRef= 1.e30; //szv:!!!
368
369     DistanceMapMap (myMapV1, myMapV2, myBV1, myBV2);
370     DistanceMapMap (myMapV1, myMapE2, myBV1, myBE2);
371     DistanceMapMap (myMapE1, myMapV2, myBE1, myBV2);
372     DistanceMapMap (myMapV1, myMapF2, myBV1, myBF2);
373     DistanceMapMap (myMapF1, myMapV2, myBF1, myBV2);
374     DistanceMapMap (myMapE1, myMapE2, myBE1, myBE2);
375     DistanceMapMap (myMapE1, myMapF2, myBE1, myBF2);
376     DistanceMapMap (myMapF1, myMapE2, myBF1, myBE2);
377
378     if (fabs (myDistRef) > myEps)
379     {
380       DistanceMapMap (myMapF1, myMapF2, myBF1, myBF2);
381     }
382     
383     //  Modified by Sergey KHROMOV - Tue Mar  6 11:55:03 2001 Begin
384     Standard_Integer i = 1;
385     for (; i <= mySolutionsShape1.Length(); i++)
386       if (mySolutionsShape1.Value(i).Dist() > myDistRef + myEps)
387       {
388         mySolutionsShape1.Remove(i);
389         mySolutionsShape2.Remove(i);
390       }
391     //  Modified by Sergey KHROMOV - Tue Mar  6 11:55:04 2001 End
392     myIsDone = ( mySolutionsShape1.Length() > 0 );
393   }
394   return myIsDone;
395 }
396
397 //=======================================================================
398 //function : Value
399 //purpose  : 
400 //=======================================================================
401
402 Standard_Real BRepExtrema_DistShapeShape::Value() const 
403
404   if (!myIsDone)
405     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::Value: There's no solution ");
406
407   return myDistRef;
408 }
409
410 //=======================================================================
411 //function : SupportOnShape1
412 //purpose  : 
413 //=======================================================================
414
415 TopoDS_Shape BRepExtrema_DistShapeShape::SupportOnShape1(const Standard_Integer N) const
416
417   if (!myIsDone)
418     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::SupportOnShape1: There's no solution ");
419
420   const BRepExtrema_SolutionElem &sol = mySolutionsShape1.Value(N);
421   switch (sol.SupportKind())
422   {
423     case BRepExtrema_IsVertex : return sol.Vertex();
424     case BRepExtrema_IsOnEdge : return sol.Edge();
425     case BRepExtrema_IsInFace : return sol.Face();
426   }
427   return TopoDS_Shape();
428 }
429
430 //=======================================================================
431 //function : SupportOnShape2
432 //purpose  : 
433 //=======================================================================
434
435 TopoDS_Shape BRepExtrema_DistShapeShape::SupportOnShape2(const Standard_Integer N) const 
436
437   if (!myIsDone)
438     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::SupportOnShape2: There's no solution ");
439
440   const BRepExtrema_SolutionElem &sol = mySolutionsShape2.Value(N);
441   switch (sol.SupportKind())
442   {
443     case BRepExtrema_IsVertex : return sol.Vertex();
444     case BRepExtrema_IsOnEdge : return sol.Edge();
445     case BRepExtrema_IsInFace : return sol.Face();
446   }
447   return TopoDS_Shape();
448 }
449
450 //=======================================================================
451 //function : ParOnEdgeS1
452 //purpose  : 
453 //=======================================================================
454
455 void BRepExtrema_DistShapeShape::ParOnEdgeS1(const Standard_Integer N, Standard_Real& t) const 
456
457   if (!myIsDone)
458     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::ParOnEdgeS1: There's no solution");
459
460   const BRepExtrema_SolutionElem &sol = mySolutionsShape1.Value(N);
461   if (sol.SupportKind() != BRepExtrema_IsOnEdge)
462     BRepExtrema_UnCompatibleShape::Raise
463       ("BRepExtrema_DistShapeShape::ParOnEdgeS1: ParOnEdgeS1 is impossible without EDGE");
464
465   sol.EdgeParameter(t);
466 }
467
468 //=======================================================================
469 //function : ParOnEdgeS2
470 //purpose  : 
471 //=======================================================================
472
473 void BRepExtrema_DistShapeShape::ParOnEdgeS2(const Standard_Integer N,  Standard_Real& t) const 
474
475   if (!myIsDone)
476     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::ParOnEdgeS2: There's no solution");
477
478   const BRepExtrema_SolutionElem &sol = mySolutionsShape2.Value(N);
479   if (sol.SupportKind() != BRepExtrema_IsOnEdge)
480     BRepExtrema_UnCompatibleShape::Raise
481       ("BRepExtrema_DistShapeShape::ParOnEdgeS2: ParOnEdgeS2 is impossible without EDGE");
482  
483   sol.EdgeParameter(t);
484 }
485
486 //=======================================================================
487 //function : ParOnFaceS1
488 //purpose  : 
489 //=======================================================================
490
491 void BRepExtrema_DistShapeShape::ParOnFaceS1(const Standard_Integer N,  Standard_Real& u,  Standard_Real& v) const 
492
493   if (!myIsDone)
494     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::ParOnFaceS1: There's no solution");
495
496   const BRepExtrema_SolutionElem &sol = mySolutionsShape1.Value(N);
497   if (sol.SupportKind() != BRepExtrema_IsInFace)
498     BRepExtrema_UnCompatibleShape::Raise
499       ("BRepExtrema_DistShapeShape::ParOnFaceS1: ParOnFaceS1 is impossible without FACE");
500   
501   sol.FaceParameter(u, v);
502 }
503
504 //=======================================================================
505 //function : ParOnFaceS2
506 //purpose  : 
507 //=======================================================================
508
509 void BRepExtrema_DistShapeShape::ParOnFaceS2(const Standard_Integer N,  Standard_Real& u, Standard_Real& v) const 
510
511   if (!myIsDone)
512     StdFail_NotDone::Raise("BRepExtrema_DistShapeShape::ParOnFaceS2: There's no solution");
513
514   const BRepExtrema_SolutionElem &sol = mySolutionsShape2.Value(N);
515   if (sol.SupportKind() != BRepExtrema_IsInFace)
516     BRepExtrema_UnCompatibleShape::Raise
517       ("BRepExtrema_DistShapeShape::ParOnFaceS2:ParOnFaceS2 is impossible without FACE ");
518   
519   sol.FaceParameter(u, v);
520 }
521
522 //=======================================================================
523 //function : Dump
524 //purpose  : 
525 //=======================================================================
526
527 void BRepExtrema_DistShapeShape::Dump(Standard_OStream& o) const 
528 {
529   Standard_Integer i;
530   Standard_Real r1,r2;
531   
532   o<< "the distance  value is :  " << Value()<<endl;
533   o<< "the number of solutions is :"<<NbSolution()<<endl;
534   o<<endl;
535   for (i=1;i<=NbSolution();i++)
536   {
537     o<<"solution number "<<i<<": "<< endl;
538     o<<"the type of the solution on the first shape is " <<Standard_Integer( SupportTypeShape1(i)) <<endl; 
539     o<<"the type of the solution on the second shape is "<<Standard_Integer( SupportTypeShape2(i))<< endl;
540     o<< "the coordinates of  the point on the first shape are: "<<endl; 
541     o<<"X=" <<PointOnShape1(i).X()<<" Y=" <<PointOnShape1(i).Y()<<" Z="<<PointOnShape1(i).Z()<<endl;
542     o<< "the coordinates of  the point on the second shape are: "<<endl; 
543     o<<"X="<< PointOnShape2(i).X()<< " Y="<<PointOnShape2(i).Y()<<" Z="<< PointOnShape2(i).Z()<<endl;
544     
545     switch (SupportTypeShape1(i))
546     {
547       case BRepExtrema_IsVertex:
548         break;
549       case BRepExtrema_IsOnEdge:
550         ParOnEdgeS1(i,r1);
551         o << "parameter on the first edge :  t= " << r1 << endl;
552         break;
553       case BRepExtrema_IsInFace:
554         ParOnFaceS1(i,r1,r2);
555         o << "parameters on the first face :  u= " << r1 << " v=" <<  r2 << endl;
556         break;
557     }
558     switch (SupportTypeShape2(i))
559     {
560       case BRepExtrema_IsVertex:
561         break;
562       case BRepExtrema_IsOnEdge:
563         ParOnEdgeS2(i,r1);
564         o << "parameter on the second edge : t=" << r1 << endl;
565         break;
566       case BRepExtrema_IsInFace:
567         ParOnFaceS2(i,r1,r2);
568         o << "parameters on the second face : u= " << r1 << " v=" <<  r2 << endl;
569         break;
570     }
571     o<<endl;
572   } 
573 }