Salome HOME
4811380bd3c7c82f15aecbb85ee4db4fa344f9c8
[modules/hydro.git] / src / HYDROData / HYDROData_StreamAltitude.cxx
1 // Copyright (C) 2014-2015  EDF-R&D
2 // This library is free software; you can redistribute it and/or
3 // modify it under the terms of the GNU Lesser General Public
4 // License as published by the Free Software Foundation; either
5 // version 2.1 of the License, or (at your option) any later version.
6 //
7 // This library is distributed in the hope that it will be useful,
8 // but WITHOUT ANY WARRANTY; without even the implied warranty of
9 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
10 // Lesser General Public License for more details.
11 //
12 // You should have received a copy of the GNU Lesser General Public
13 // License along with this library; if not, write to the Free Software
14 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
15 //
16 // See http://www.salome-platform.org/ or email : webmaster.salome@opencascade.com
17 //
18
19 #include "HYDROData_StreamAltitude.h"
20
21 #include "HYDROData_Channel.h"
22 #include "HYDROData_Document.h"
23 #include "HYDROData_Profile.h"
24 #include "HYDROData_Stream.h"
25 #include "HYDROData_ShapesTool.h"
26 #include <HYDROData_Tool.h>
27 #include <BRep_Tool.hxx>
28
29 #include <BRepBuilderAPI_MakeEdge.hxx>
30 #include <BRepBuilderAPI_MakeFace.hxx>
31 #include <BRepBuilderAPI_MakeWire.hxx>
32
33 #include <Extrema_ExtElC.hxx>
34
35 #include <GeomAPI_ProjectPointOnCurve.hxx>
36
37 #include <gp_Lin.hxx>
38
39 #include <Precision.hxx>
40
41 #include <TopExp.hxx>
42 #include <TopExp_Explorer.hxx>
43
44 #include <TopoDS.hxx>
45 #include <TopoDS_Wire.hxx>
46 #include <TopoDS_Edge.hxx>
47
48 #include <TopTools_SequenceOfShape.hxx>
49 #include <TopTools_ListIteratorOfListOfShape.hxx>
50
51 #include <Geom_Line.hxx>
52
53 #include <QStringList>
54 #include <cmath>
55
56 #define _DEVDEBUG_
57 #include "HYDRO_trace.hxx"
58
59 #ifdef DEB_CLASS2D
60 #include <BRepTools.hxx>
61 #include <BRep_Builder.hxx>
62 #include <BRepBuilderAPI_MakeVertex.hxx>
63 #endif
64 IMPLEMENT_STANDARD_HANDLE(HYDROData_StreamAltitude, HYDROData_IAltitudeObject)
65 IMPLEMENT_STANDARD_RTTIEXT(HYDROData_StreamAltitude, HYDROData_IAltitudeObject)
66
67 HYDROData_StreamAltitude::HYDROData_StreamAltitude()
68 : HYDROData_IAltitudeObject()
69 {
70 }
71
72 HYDROData_StreamAltitude::~HYDROData_StreamAltitude()
73 {
74 }
75
76 bool IsPointBetweenEdges( const gp_Pnt& aFirstPnt1, const gp_Pnt& aLastPnt1,
77                           const gp_Pnt& aFirstPnt2, const gp_Pnt& aLastPnt2,
78                           const gp_Pnt& thePoint) {
79   BRepBuilderAPI_MakeEdge aLeftMakeEdge( aFirstPnt1, aLastPnt1 );
80   BRepBuilderAPI_MakeEdge aBotMakeEdge( aLastPnt1, aLastPnt2 );
81   BRepBuilderAPI_MakeEdge aRightMakeEdge( aLastPnt2, aFirstPnt2 );
82   BRepBuilderAPI_MakeEdge aTopMakeEdge( aFirstPnt2, aFirstPnt1 );
83
84   BRepBuilderAPI_MakeWire aMakeWire( aLeftMakeEdge.Edge(), aBotMakeEdge.Edge(), 
85                                      aRightMakeEdge.Edge(), aTopMakeEdge.Edge() );
86
87   BRepBuilderAPI_MakeFace aMakeFace( aMakeWire.Wire() );
88         
89   TopoDS_Face aFace = aMakeFace.Face();
90 #ifdef DEB_CLASS2D
91   TopoDS_Compound aCmp;
92   BRep_Builder aBB;
93   aBB.MakeCompound(aCmp);
94   aBB.Add(aCmp, aFace);
95   BRepBuilderAPI_MakeVertex aMk(thePoint);
96   aBB.Add(aCmp, aMk.Vertex());
97   BRepTools::Write(aCmp, "ProfileFace.brep");
98 #endif     
99
100   gp_XY anXY( thePoint.X(), thePoint.Y() );
101   TopAbs_State aPointState =  HYDROData_Tool::ComputePointState(anXY, aFace);
102
103 #ifdef DEB_CLASS2D
104           cout << "Point status is = " << aPointState <<endl;
105 #endif
106   return aPointState != TopAbs_OUT;
107 }
108
109 /** compute edge1^(firstPnt1,point) and edge2^(firstPnt2,point):
110  *  if the z values are opposite, the point is between the edges.
111  *  We must also check if the point is near the edges
112  *  (inside the circle defined by the edges: approximation)
113  *  to discriminate false positives in sinuous cases
114  */
115 bool IsPointBetweenEdges2( const gp_Pnt& aFirstPnt1, const gp_Pnt& aLastPnt1,
116                            const gp_Pnt& aFirstPnt2, const gp_Pnt& aLastPnt2,
117                            const gp_Pnt& thePoint) {
118   double x1 = aLastPnt1.X() - aFirstPnt1.X(); // v1
119   double y1 = aLastPnt1.Y() - aFirstPnt1.Y();
120   double x2 = aLastPnt2.X() - aFirstPnt2.X(); // v2
121   double y2 = aLastPnt2.Y() - aFirstPnt2.Y();
122   double xa = thePoint.X() - aFirstPnt1.X();  // va
123   double ya = thePoint.Y() - aFirstPnt1.Y();
124   double xb = thePoint.X() - aFirstPnt2.X();  // vb
125   double yb = thePoint.Y() - aFirstPnt2.Y();
126   double z1 = x1*ya -xa*y1; // v1^va: z component
127   double z2 = x2*yb -xb*y2; // v2^vb: z component
128   bool isBetween = true;
129   if (((z1<0) and (z2<0)) or ((z1>=0) and (z2>=0)))
130     {
131       isBetween = false;
132     }
133   if (isBetween)
134     {
135       double xg = (aFirstPnt1.X() + aLastPnt1.X() + aFirstPnt2.X() + aLastPnt2.X())/4;
136       double yg = (aFirstPnt1.Y() + aLastPnt1.Y() + aFirstPnt2.Y() + aLastPnt2.Y())/4;
137       double df1 = (aFirstPnt1.X()-xg)*(aFirstPnt1.X()-xg) + (aFirstPnt1.Y()-yg)*(aFirstPnt1.Y()-yg);
138       double dl1 = (aLastPnt1.X()-xg)*(aLastPnt1.X()-xg) + (aLastPnt1.Y()-yg)*(aLastPnt1.Y()-yg);
139       double df2 = (aFirstPnt2.X()-xg)*(aFirstPnt2.X()-xg) + (aFirstPnt2.Y()-yg)*(aFirstPnt2.Y()-yg);
140       double dl2 = (aLastPnt2.X()-xg)*(aLastPnt2.X()-xg) + (aLastPnt2.Y()-yg)*(aLastPnt2.Y()-yg);
141       double r2 = fmax(df1,dl1);
142       r2 = fmax(r2,df2);
143       r2 = fmax(r2,dl2);
144       double d2 = (thePoint.X()-xg)*(thePoint.X()-xg) + (thePoint.Y()-yg)*(thePoint.Y()-yg);
145       if (d2 > r2)
146         isBetween = false;
147     }
148   return isBetween;
149 }
150
151 Standard_Real getAltitudeFromWire( const TopoDS_Wire& theWire,
152                                    const Standard_Real& theLeftDist,
153                                    const Standard_Real& theRightDist )
154 {
155   Standard_Real aResAlt = 0.0;
156
157   gp_XY aFirstPoint, aLastPoint;
158   TopoDS_Vertex aFirstVertex, aLastVertex;
159   TopExp::Vertices( theWire, aFirstVertex, aLastVertex );
160
161   gp_Pnt aPnt1( BRep_Tool::Pnt( aFirstVertex ) );
162   aPnt1.SetZ( 0 );
163   gp_Pnt aPnt2( BRep_Tool::Pnt( aLastVertex ) );
164   aPnt2.SetZ( 0 );
165   
166   Standard_Real aProfileDist = aPnt1.Distance( aPnt2 );
167
168   Standard_Real aCoeff = aProfileDist / ( theLeftDist + theRightDist );
169
170   gp_Pnt anIntPoint( aPnt1.XYZ() + ( aCoeff * theLeftDist ) * gp_Dir( gp_Vec( aPnt1, aPnt2 ) ).XYZ() );
171
172   return HYDROData_Tool::GetAltitudeForWire( theWire,
173                                              gp_XY(anIntPoint.X(), anIntPoint.Y()),
174                                              1E-2,
175                                              1E-2,
176                                              HYDROData_IAltitudeObject::GetInvalidAltitude() );
177 }
178
179 bool HYDROData_StreamAltitude::getBoundaryWiresForPoint(
180   const gp_XY& thePoint,
181   TopoDS_Wire& theLeftWire,
182   TopoDS_Wire& theRightWire ) const
183 {
184   gp_Pnt aTestPnt( thePoint.X(),  thePoint.Y(), 0 );
185
186   Handle(HYDROData_Object) anObject =
187     Handle(HYDROData_Object)::DownCast( GetFatherObject() );
188   if ( anObject.IsNull() ) {
189     return false;
190   }
191
192   if ( anObject->GetKind() == KIND_STREAM ) {
193     Handle(HYDROData_Stream) aStream = Handle(HYDROData_Stream)::DownCast( anObject );
194     if ( aStream.IsNull() )
195       return false;
196
197     HYDROData_SequenceOfObjects aStreamProfiles = aStream->GetProfiles();
198     if ( aStreamProfiles.Length() < 2 )
199       return false;
200
201     Handle(HYDROData_Profile) aPrevProfile;
202     gp_Pnt aPrevPnt1, aPrevPnt2;
203     for ( int i = 1, n = aStreamProfiles.Length(); i <= n; ++i )
204     {
205       Handle(HYDROData_Profile) aProfile =
206         Handle(HYDROData_Profile)::DownCast( aStreamProfiles.Value( i ) );
207       if ( aProfile.IsNull() )
208         continue;
209
210       gp_XY aFirstPoint, aLastPoint;
211       if ( !aProfile->GetLeftPoint( aFirstPoint, false ) ||
212            !aProfile->GetRightPoint( aLastPoint, false ) )
213         continue;
214       
215       gp_Pnt aPnt1( aFirstPoint.X(), aFirstPoint.Y(), 0 );
216       gp_Pnt aPnt2( aLastPoint.X(),  aLastPoint.Y(),  0 );
217
218       if ( !aPrevProfile.IsNull() )
219       {        
220         if ( IsPointBetweenEdges2( aPrevPnt1, aPrevPnt2, aPnt1, aPnt2, aTestPnt ) )
221         {
222           DEBTRACE("point is between profiles "<< i-1 << " and " <<i);
223           theLeftWire = TopoDS::Wire( aPrevProfile->GetShape3D() );
224           theRightWire = TopoDS::Wire( aProfile->GetShape3D() );
225           break;
226         }
227       }
228
229       aPrevProfile = aProfile;
230       aPrevPnt1 = aPnt1;
231       aPrevPnt2 = aPnt2;
232     }
233   } else if ( anObject->GetKind() == KIND_CHANNEL ) {
234     Handle(HYDROData_Channel) aChannel = Handle(HYDROData_Channel)::DownCast( anObject );
235     if ( aChannel.IsNull() )
236       return false;
237
238     TopTools_ListOfShape aWiresList;
239     TopExp_Explorer anExp( aChannel->GetShape3D(), TopAbs_WIRE );
240     for ( ; anExp.More(); anExp.Next() ) {
241       if(!anExp.Current().IsNull()) {
242         const TopoDS_Wire& aWire = TopoDS::Wire( anExp.Current() );
243         aWiresList.Append( aWire );
244       }
245     }
246
247     if ( aWiresList.Extent() < 2 )
248       return false;
249
250     TopoDS_Wire aPrevWire;
251     gp_Pnt aPrevPnt1, aPrevPnt2;
252
253     TopTools_ListIteratorOfListOfShape anIt( aWiresList );
254     for ( ; anIt.More(); anIt.Next() ) {
255       TopoDS_Wire& aWire = TopoDS::Wire( anIt.Value() );
256       if ( aWire.IsNull() )
257         continue;
258
259       TopoDS_Vertex aFirstVertex, aLastVertex;
260       TopExp::Vertices( aWire, aFirstVertex, aLastVertex );
261     
262       gp_Pnt aPnt1( BRep_Tool::Pnt( aFirstVertex ) );
263       aPnt1.SetZ( 0 );
264       gp_Pnt aPnt2( BRep_Tool::Pnt( aLastVertex ) );
265       aPnt2.SetZ( 0 );
266
267       if ( !aPrevWire.IsNull() ) {
268         if ( IsPointBetweenEdges2( aPrevPnt1, aPrevPnt2, aPnt1, aPnt2, aTestPnt ) ) {
269           theLeftWire = aPrevWire;
270           theRightWire = aWire;
271           break;
272         }
273       }
274
275       aPrevWire = aWire;
276       aPrevPnt1 = aPnt1;
277       aPrevPnt2 = aPnt2;
278     }
279   }
280
281   return !theLeftWire.IsNull() && !theRightWire.IsNull();
282 }
283
284 double HYDROData_StreamAltitude::GetAltitudeForPoint( const gp_XY& thePoint ) const
285 {
286   DEBTRACE("GetAltitudeForPoint p(" << thePoint.X() << ", " << thePoint.Y() << ")");
287   double aResAltitude = GetInvalidAltitude();
288
289   Handle(HYDROData_Object) anObject =
290     Handle(HYDROData_Object)::DownCast( GetFatherObject() );
291   if ( anObject.IsNull() )
292   {
293     DEBTRACE(" --- aStream.IsNull()");
294     return aResAltitude;
295   }
296   TopoDS_Shape aTopShape = anObject->GetTopShape();
297   if ( aTopShape.IsNull() )
298   {
299     DEBTRACE(" --- aTopShape.IsNull()");
300     return aResAltitude;
301   }
302
303   TopExp_Explorer aFaceExp( aTopShape, TopAbs_FACE );
304   if ( !aFaceExp.More() )
305   {
306     DEBTRACE(" --- !aFaceExp.More()");
307     return aResAltitude;
308   }
309
310   // Get only face because of 2d profile wires is in compound
311   TopoDS_Face aFace = TopoDS::Face( aFaceExp.Current() );
312
313   // Check if point is inside of stream/channel presentation
314   TopAbs_State aPointState = HYDROData_Tool::ComputePointState(thePoint, aFace);
315
316 #ifdef DEB_CLASS2D
317       cout << "Point status is = " << aPointState <<endl;
318           TopoDS_Compound aCmp;
319       BRep_Builder aBB;
320       aBB.MakeCompound(aCmp);
321           aBB.Add(aCmp, aFace);
322           gp_Pnt aPnt (thePoint.X(), thePoint.Y(), 0.);
323           BRepBuilderAPI_MakeVertex aMk(aPnt);
324           aBB.Add(aCmp, aMk.Vertex());
325           BRepTools::Write(aCmp, "FCL2d.brep");
326 #endif
327   if ( aPointState == TopAbs_OUT )
328   {
329         DEBTRACE(" --- aPointState == TopAbs_OUT");
330     return aResAltitude;
331   }
332
333   TopoDS_Edge aLeftEdge, aRightEdge;
334
335   if ( anObject->GetKind() == KIND_STREAM ) {
336     Handle(HYDROData_Stream) aStream =
337       Handle(HYDROData_Stream)::DownCast( GetFatherObject() );
338     if ( !aStream.IsNull() ) {
339       aLeftEdge = TopoDS::Edge( aStream->GetLeftShape() );
340       aRightEdge = TopoDS::Edge( aStream->GetRightShape() );
341     }
342   } else if ( anObject->GetKind() == KIND_CHANNEL ) {
343     Handle(HYDROData_Channel) aChannel =
344       Handle(HYDROData_Channel)::DownCast( GetFatherObject() );
345     if ( !aChannel.IsNull() ) {
346       aLeftEdge = TopoDS::Edge( aChannel->GetLeftShape() );
347       aRightEdge = TopoDS::Edge( aChannel->GetRightShape() );
348     }
349   }
350   
351   // Find the two profiles between which the point is lies
352   TopoDS_Wire aLeftWire, aRightWire;
353   if ( !getBoundaryWiresForPoint( thePoint, aLeftWire, aRightWire ) )
354   {
355       DEBTRACE(" --- !getBoundaryProfilesForPoint( thePoint, aLeftProfile, aRightProfile )");
356       return aResAltitude;
357   }
358
359   // Find the projections of point to borders of stream
360   gp_XYZ aPointToTest( thePoint.X(), thePoint.Y(), 0.0 );
361   
362   Standard_Real aFirst = 0.0, aLast = 0.0;
363   Handle(Geom_Curve) anEdgeLeftCurve = BRep_Tool::Curve( aLeftEdge, aFirst, aLast );
364   Handle(Geom_Curve) anEdgeRightCurve = BRep_Tool::Curve( aRightEdge, aFirst, aLast );
365
366   GeomAPI_ProjectPointOnCurve aLeftProject( aPointToTest, anEdgeLeftCurve );
367   GeomAPI_ProjectPointOnCurve aRightProject( aPointToTest, anEdgeRightCurve );
368
369   int aNbPoints1 = aRightProject.NbPoints();
370   int aNbPoints2 = aLeftProject.NbPoints();
371   if ( aNbPoints1 < 1 || aNbPoints2 < 1)
372     {
373       DEBTRACE(" --- projections " << aNbPoints1 << " " << aNbPoints2);
374       return aResAltitude;
375     }
376   Standard_Real aLeftDist = aLeftProject.LowerDistance();
377   Standard_Real aRightDist = aRightProject.LowerDistance();
378
379   // Find the altitude in profiles
380   Standard_Real aLeftAlt, aRightAlt;
381   gp_Pnt aLeftProfileP1, aLeftProfileP2, aRightProfileP1, aRightProfileP2;
382   aLeftAlt = getAltitudeFromWire( aLeftWire, aLeftDist, aRightDist );
383   aRightAlt = getAltitudeFromWire( aRightWire, aLeftDist, aRightDist );
384   
385   TopoDS_Vertex aFirstVertex, aLastVertex;
386   TopExp::Vertices( aLeftWire, aFirstVertex, aLastVertex );
387   aLeftProfileP1 = BRep_Tool::Pnt( aFirstVertex );
388   aLeftProfileP2 = BRep_Tool::Pnt( aLastVertex );
389
390   TopExp::Vertices( aRightWire, aFirstVertex, aLastVertex );
391   aRightProfileP1 = BRep_Tool::Pnt( aFirstVertex );
392   aRightProfileP2 = BRep_Tool::Pnt( aLastVertex );
393
394   // Interpolate altitudes
395   // Left profile line ( the segment between the first and the last profile point )
396   aLeftProfileP1.SetZ( 0 );
397   aLeftProfileP2.SetZ( 0 );
398   gp_Vec aLeftProfileVec( aLeftProfileP1, aLeftProfileP2 );
399   Handle(Geom_Line) aLeftProfileLine = new Geom_Line( gp_Ax1( aLeftProfileP1, aLeftProfileVec ) );
400   // Right profile line
401   aRightProfileP1.SetZ( 0 );
402   aRightProfileP2.SetZ( 0 );
403   gp_Vec aRightProfileVec( aRightProfileP1, aRightProfileP2 );
404   Handle(Geom_Line) aRightProfileLine = new Geom_Line( gp_Ax1( aRightProfileP1, aRightProfileVec ) );
405   // The point projections on the left and right profiles
406   GeomAPI_ProjectPointOnCurve aLeftProfileProject( aPointToTest, aLeftProfileLine );
407   GeomAPI_ProjectPointOnCurve aRightProfileProject( aPointToTest, aRightProfileLine );
408   // The point distance to the left and right profiles
409   Standard_Real aLeftProfileDist = aLeftProfileProject.LowerDistance();
410   Standard_Real aRightProfileDist = aRightProfileProject.LowerDistance();
411   // The coefficients
412   Standard_Real aFirstCoeff = aLeftProfileDist / ( aLeftProfileDist + aRightProfileDist );
413   Standard_Real aSecCoeff = aRightProfileDist / ( aLeftProfileDist + aRightProfileDist );
414
415   aResAltitude = aLeftAlt * ( 1 - aFirstCoeff ) + aRightAlt * ( 1 - aSecCoeff );
416   DEBTRACE("aResAltitude=" << aResAltitude);
417   return aResAltitude;
418 }
419
420
421
422