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