0031565: Visualization - SIGFPE, Arithmetic exception if SelectMgr_TriangularFrustumS...
[occt.git] / src / SelectMgr / SelectMgr_TriangularFrustumSet.cxx
1 // Created on: 2014-11-21
2 // Created by: Varvara POSKONINA
3 // Copyright (c) 2005-2014 OPEN CASCADE SAS
4 //
5 // This file is part of Open CASCADE Technology software library.
6 //
7 // This library is free software; you can redistribute it and/or modify it under
8 // the terms of the GNU Lesser General Public License version 2.1 as published
9 // by the Free Software Foundation, with special exception defined in the file
10 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
11 // distribution for complete text of the license and disclaimer of any warranty.
12 //
13 // Alternatively, this file may be used under the terms of Open CASCADE
14 // commercial license or contractual agreement.
15
16 #include <BRepMesh_DataStructureOfDelaun.hxx>
17 #include <BRepMesh_Delaun.hxx>
18 #include <NCollection_IncAllocator.hxx>
19
20 #include <SelectMgr_TriangularFrustumSet.hxx>
21 #include <SelectMgr_TriangularFrustum.hxx>
22
23 #define MEMORY_BLOCK_SIZE 512 * 7
24
25 // =======================================================================
26 // function : SelectMgr_TriangularFrustumSet
27 // purpose  :
28 // =======================================================================
29 SelectMgr_TriangularFrustumSet::SelectMgr_TriangularFrustumSet()
30 :  myToAllowOverlap (Standard_False)
31 {}
32
33 // =======================================================================
34 // function : BuildSelectingVolume
35 // purpose  : Meshes polygon bounded by polyline. Than organizes a set of
36 //            triangular frustums, where each triangle's projection onto
37 //            near and far view frustum planes is considered as a frustum
38 //            base
39 // =======================================================================
40 void SelectMgr_TriangularFrustumSet::Build (const TColgp_Array1OfPnt2d& thePoints)
41 {
42   myFrustums.Clear();
43
44   Handle(NCollection_IncAllocator) anAllocator = new NCollection_IncAllocator (MEMORY_BLOCK_SIZE);
45   Handle(BRepMesh_DataStructureOfDelaun) aMeshStructure = new BRepMesh_DataStructureOfDelaun (anAllocator);
46   Standard_Integer aPtsLower = thePoints.Lower();
47   Standard_Integer aPtsUpper = thePoints.Upper();
48   IMeshData::VectorOfInteger anIndexes (thePoints.Size(), anAllocator);
49   myBoundaryPoints.Resize (aPtsLower, aPtsLower + 2 * (thePoints.Size()) - 1, Standard_False);
50
51   for (Standard_Integer aPtIdx = aPtsLower; aPtIdx <= aPtsUpper; ++aPtIdx)
52   {
53     BRepMesh_Vertex aVertex (thePoints.Value (aPtIdx).XY(), aPtIdx, BRepMesh_Frontier);
54     anIndexes.Append (aMeshStructure->AddNode (aVertex));
55     const gp_Pnt aNearPnt = myBuilder->ProjectPntOnViewPlane (aVertex.Coord().X(), aVertex.Coord().Y(), 0.0);
56     const gp_Pnt aFarPnt  = myBuilder->ProjectPntOnViewPlane (aVertex.Coord().X(), aVertex.Coord().Y(), 1.0);
57     myBoundaryPoints.SetValue (aPtIdx, aNearPnt);
58     myBoundaryPoints.SetValue (aPtIdx + thePoints.Size(), aFarPnt);
59   }
60
61   Standard_Real aPtSum = 0;
62   for (Standard_Integer aIdx = aPtsLower; aIdx <= aPtsUpper; ++aIdx)
63   {
64     Standard_Integer aNextIdx = (aIdx % thePoints.Length()) + 1;
65     aPtSum += (thePoints.Value (aNextIdx).Coord().X() - thePoints.Value (aIdx).Coord().X())
66               * (thePoints.Value (aNextIdx).Coord().Y() + thePoints.Value (aIdx).Coord().Y());
67   }
68   Standard_Boolean isClockwiseOrdered = aPtSum < 0;
69
70   for (Standard_Integer aIdx = 0; aIdx < anIndexes.Length(); ++aIdx)
71   {
72     Standard_Integer aPtIdx = isClockwiseOrdered ? aIdx : (aIdx + 1) % anIndexes.Length();
73     Standard_Integer aNextPtIdx = isClockwiseOrdered ? (aIdx + 1) % anIndexes.Length() : aIdx;
74     BRepMesh_Edge anEdge (anIndexes.Value (aPtIdx),
75                           anIndexes.Value (aNextPtIdx),
76                           BRepMesh_Frontier);
77     aMeshStructure->AddLink (anEdge);
78   }
79
80   BRepMesh_Delaun aTriangulation (aMeshStructure, anIndexes);
81   const IMeshData::MapOfInteger& aTriangles = aMeshStructure->ElementsOfDomain();
82   if (aTriangles.Extent() < 1)
83     return;
84
85   IMeshData::IteratorOfMapOfInteger aTriangleIt (aTriangles);
86   for (; aTriangleIt.More(); aTriangleIt.Next())
87   {
88     const Standard_Integer aTriangleId = aTriangleIt.Key();
89     const BRepMesh_Triangle& aCurrentTriangle = aMeshStructure->GetElement (aTriangleId);
90
91     if (aCurrentTriangle.Movability() == BRepMesh_Deleted)
92       continue;
93
94     Standard_Integer aTriangleVerts[3];
95     aMeshStructure->ElementNodes (aCurrentTriangle, aTriangleVerts);
96
97     gp_Pnt2d aPts[3];
98     for (Standard_Integer aVertIdx = 0; aVertIdx < 3; ++aVertIdx)
99     {
100       const BRepMesh_Vertex& aVertex = aMeshStructure->GetNode (aTriangleVerts[aVertIdx]);
101       aPts[aVertIdx] = aVertex.Coord();
102     }
103
104     Handle(SelectMgr_TriangularFrustum) aTrFrustum = new SelectMgr_TriangularFrustum();
105     aTrFrustum->SetBuilder (myBuilder);
106     aTrFrustum->Build (aPts[0], aPts[1], aPts[2]);
107     myFrustums.Append (aTrFrustum);
108   }
109
110   aMeshStructure.Nullify();
111   anAllocator.Nullify();
112 }
113
114 // =======================================================================
115 // function : ScaleAndTransform
116 // purpose  : IMPORTANT: Scaling makes sense only for frustum built on a single point!
117 //            Note that this method does not perform any checks on type of the frustum.
118 //            Returns a copy of the frustum resized according to the scale factor given
119 //            and transforms it using the matrix given.
120 //            There are no default parameters, but in case if:
121 //                - transformation only is needed: @theScaleFactor must be initialized
122 //                  as any negative value;
123 //                - scale only is needed: @theTrsf must be set to gp_Identity.
124 // =======================================================================
125 Handle(SelectMgr_BaseFrustum) SelectMgr_TriangularFrustumSet::ScaleAndTransform (const Standard_Integer theScale,
126                                                                                  const gp_GTrsf& theTrsf) const
127 {
128   Handle(SelectMgr_TriangularFrustumSet) aRes = new SelectMgr_TriangularFrustumSet();
129
130   for (SelectMgr_TriangFrustumsIter anIter (myFrustums); anIter.More(); anIter.Next())
131   {
132     aRes->myFrustums.Append (Handle(SelectMgr_TriangularFrustum)::DownCast (anIter.Value()->ScaleAndTransform (theScale, theTrsf)));
133   }
134
135   aRes->myBoundaryPoints.Resize (myBoundaryPoints.Lower(), myBoundaryPoints.Upper(), Standard_False);
136   for (Standard_Integer anIdx = myBoundaryPoints.Lower(); anIdx <= myBoundaryPoints.Upper(); anIdx++)
137   {
138     gp_Pnt aPoint = myBoundaryPoints.Value (anIdx);
139     theTrsf.Transforms (aPoint.ChangeCoord());
140     aRes->myBoundaryPoints.SetValue (anIdx, aPoint);
141   }
142
143   return aRes;
144 }
145
146 // =======================================================================
147 // function : Overlaps
148 // purpose  :
149 // =======================================================================
150 Standard_Boolean SelectMgr_TriangularFrustumSet::Overlaps (const SelectMgr_Vec3& theMinPnt,
151                                                            const SelectMgr_Vec3& theMaxPnt,
152                                                            const SelectMgr_ViewClipRange& theClipRange,
153                                                            SelectBasics_PickResult& thePickResult) const
154 {
155   for (SelectMgr_TriangFrustumsIter anIter (myFrustums); anIter.More(); anIter.Next())
156   {
157     if (anIter.Value()->Overlaps (theMinPnt, theMaxPnt, theClipRange, thePickResult))
158       return Standard_True;
159   }
160
161   return Standard_False;
162 }
163
164 // =======================================================================
165 // function : Overlaps
166 // purpose  :
167 // =======================================================================
168 Standard_Boolean SelectMgr_TriangularFrustumSet::Overlaps (const SelectMgr_Vec3& theMinPnt,
169                                                            const SelectMgr_Vec3& theMaxPnt,
170                                                            Standard_Boolean* theInside) const
171 {
172   for (SelectMgr_TriangFrustumsIter anIter (myFrustums); anIter.More(); anIter.Next())
173   {
174     if (anIter.Value()->Overlaps (theMinPnt, theMaxPnt, NULL))
175     {
176       if (myToAllowOverlap || theInside == NULL)
177       {
178         return Standard_True;
179       }
180       else
181       {
182         gp_Pnt aMinMaxPnts[2] = { gp_Pnt (theMinPnt.x(), theMinPnt.y(), theMinPnt.z()),
183                                   gp_Pnt (theMaxPnt.x(), theMaxPnt.y(), theMaxPnt.z())};
184
185         gp_Pnt anOffset[3] = { gp_Pnt (aMinMaxPnts[1].X() - aMinMaxPnts[0].X(), 0.0, 0.0),
186                                gp_Pnt (0.0, aMinMaxPnts[1].Y() - aMinMaxPnts[0].Y(), 0.0),
187                                gp_Pnt (0.0, 0.0, aMinMaxPnts[1].Z() - aMinMaxPnts[0].Z()) };
188
189         Standard_Integer aSign = 1;
190         for (Standard_Integer aPntsIdx = 0; aPntsIdx < 2; aPntsIdx++)
191         {
192           for (Standard_Integer aCoordIdx = 0; aCoordIdx < 3; aCoordIdx++)
193           {
194             gp_Pnt anOffsetPnt = aMinMaxPnts [aPntsIdx].XYZ() + aSign * anOffset [aCoordIdx].XYZ();
195             if (isIntersectBoundary (aMinMaxPnts [aPntsIdx], anOffsetPnt)
196              || isIntersectBoundary (anOffsetPnt, anOffsetPnt.XYZ() + aSign * anOffset [(aCoordIdx + 1) % 3].XYZ()))
197             {
198               *theInside &= Standard_False;
199               return Standard_True;
200             }
201           }
202           aSign = -aSign;
203         }
204         return Standard_True;
205       }
206     }
207   }
208
209   return Standard_False;
210 }
211
212 // =======================================================================
213 // function : Overlaps
214 // purpose  :
215 // =======================================================================
216 Standard_Boolean SelectMgr_TriangularFrustumSet::Overlaps (const gp_Pnt& thePnt,
217                                                            const SelectMgr_ViewClipRange& theClipRange,
218                                                            SelectBasics_PickResult& thePickResult) const
219 {
220   for (SelectMgr_TriangFrustumsIter anIter (myFrustums); anIter.More(); anIter.Next())
221   {
222     if (anIter.Value()->Overlaps (thePnt, theClipRange, thePickResult))
223       return Standard_True;
224   }
225
226   return Standard_False;
227 }
228
229 // =======================================================================
230 // function : Overlaps
231 // purpose  :
232 // =======================================================================
233 Standard_Boolean SelectMgr_TriangularFrustumSet::Overlaps (const TColgp_Array1OfPnt& theArrayOfPts,
234                                                            Select3D_TypeOfSensitivity theSensType,
235                                                            const SelectMgr_ViewClipRange& theClipRange,
236                                                            SelectBasics_PickResult& thePickResult) const
237 {
238   for (SelectMgr_TriangFrustumsIter anIter (myFrustums); anIter.More(); anIter.Next())
239   {
240     if (anIter.Value()->Overlaps (theArrayOfPts, theSensType, theClipRange, thePickResult))
241     {
242       if (myToAllowOverlap)
243       {
244         return Standard_True;
245       }
246       else
247       {
248         Standard_Integer aPtsLower = theArrayOfPts.Lower();
249         Standard_Integer aPtsUpper = theArrayOfPts.Upper();
250         for (Standard_Integer anIdx = aPtsLower; anIdx <= aPtsUpper; anIdx++)
251         {
252           if (isIntersectBoundary (theArrayOfPts.Value (anIdx), theArrayOfPts.Value (anIdx < aPtsUpper ? anIdx + 1 : aPtsLower)))
253           {
254             return Standard_False;
255           }
256         }
257         return Standard_True;
258       }
259     }
260   }
261
262   return Standard_False;
263 }
264
265 // =======================================================================
266 // function : Overlaps
267 // purpose  :
268 // =======================================================================
269 Standard_Boolean SelectMgr_TriangularFrustumSet::Overlaps (const gp_Pnt& thePnt1,
270                                                            const gp_Pnt& thePnt2,
271                                                            const SelectMgr_ViewClipRange& theClipRange,
272                                                            SelectBasics_PickResult& thePickResult) const
273 {
274   for (SelectMgr_TriangFrustumsIter anIter (myFrustums); anIter.More(); anIter.Next())
275   {
276     if (anIter.Value()->Overlaps (thePnt1, thePnt2, theClipRange, thePickResult))
277     {
278       if (myToAllowOverlap)
279       {
280         return Standard_True;
281       }
282       else
283       {
284         if (isIntersectBoundary (thePnt1, thePnt2))
285         {
286           return Standard_False;
287         }
288         return Standard_True;
289       }
290     }
291   }
292
293   return Standard_False;
294 }
295
296 // =======================================================================
297 // function : Overlaps
298 // purpose  :
299 // =======================================================================
300 Standard_Boolean SelectMgr_TriangularFrustumSet::Overlaps (const gp_Pnt& thePnt1,
301                                                            const gp_Pnt& thePnt2,
302                                                            const gp_Pnt& thePnt3,
303                                                            Select3D_TypeOfSensitivity theSensType,
304                                                            const SelectMgr_ViewClipRange& theClipRange,
305                                                            SelectBasics_PickResult& thePickResult) const
306 {
307   for (SelectMgr_TriangFrustumsIter anIter (myFrustums); anIter.More(); anIter.Next())
308   {
309     if (anIter.Value()->Overlaps (thePnt1, thePnt2, thePnt3, theSensType, theClipRange, thePickResult))
310     {
311       if (myToAllowOverlap)
312       {
313         return Standard_True;
314       }
315       else
316       {
317         if (isIntersectBoundary (thePnt1, thePnt2)
318          || isIntersectBoundary (thePnt2, thePnt3)
319          || isIntersectBoundary (thePnt3, thePnt1))
320         {
321           return Standard_False;
322         }
323         return Standard_True;
324       }
325     }
326   }
327
328   return Standard_False;
329 }
330
331 // =======================================================================
332 // function : GetPlanes
333 // purpose  :
334 // =======================================================================
335 void SelectMgr_TriangularFrustumSet::GetPlanes (NCollection_Vector<SelectMgr_Vec4>& thePlaneEquations) const
336 {
337   thePlaneEquations.Clear();
338
339   for (SelectMgr_TriangFrustumsIter anIter (myFrustums); anIter.More(); anIter.Next())
340   {
341     anIter.Value()->GetPlanes (thePlaneEquations);
342   }
343 }
344
345 //=======================================================================
346 // function : SetAllowOverlapDetection
347 // purpose  :
348 //=======================================================================
349 void SelectMgr_TriangularFrustumSet::SetAllowOverlapDetection (const Standard_Boolean theIsToAllow)
350 {
351   myToAllowOverlap = theIsToAllow;
352 }
353
354 //=======================================================================
355 // function : isIntersectBoundary
356 // purpose  :
357 //=======================================================================
358 Standard_Boolean SelectMgr_TriangularFrustumSet::isIntersectBoundary (const gp_Pnt& thePnt1, const gp_Pnt& thePnt2) const
359 {
360   Standard_Integer aFacesNb = myBoundaryPoints.Size() / 2;
361   gp_Vec aDir = thePnt2.XYZ() - thePnt1.XYZ();
362   gp_Pnt anOrig = thePnt1;
363
364   for (Standard_Integer anIdx = myBoundaryPoints.Lower(); anIdx < aFacesNb + myBoundaryPoints.Lower(); anIdx++)
365   {
366     gp_Pnt aFace[4] = { myBoundaryPoints.Value (anIdx),
367                         myBoundaryPoints.Value (anIdx + aFacesNb),
368                         myBoundaryPoints.Value (anIdx % aFacesNb + 1 + aFacesNb),
369                         myBoundaryPoints.Value (anIdx % aFacesNb + 1) };
370
371     if (segmentTriangleIntersection (anOrig, aDir, aFace[0], aFace[1], aFace[2])
372      || segmentTriangleIntersection (anOrig, aDir, aFace[0], aFace[2], aFace[3]))
373     {
374       return Standard_True;
375     }
376   }
377   return Standard_False;
378 }
379
380 //=======================================================================
381 // function : segmentTriangleIntersection
382 // purpose  : Moller-Trumbore ray-triangle intersection test
383 //=======================================================================
384 Standard_Boolean SelectMgr_TriangularFrustumSet::segmentTriangleIntersection (const gp_Pnt& theOrig, const gp_Vec& theDir,
385                                                                               const gp_Pnt& theV1, const gp_Pnt& theV2, const gp_Pnt& theV3) const
386 {
387   gp_Vec        aPVec, aTVec, aQVec;
388   Standard_Real aD, aInvD, anU, aV, aT;
389
390   gp_Vec anEdge1 = theV2.XYZ() - theV1.XYZ();
391   gp_Vec anEdge2 = theV3.XYZ() - theV1.XYZ();
392
393   aPVec = theDir.Crossed (anEdge2);
394   aD = anEdge1.Dot (aPVec);
395   if (fabs (aD) < gp::Resolution())
396   {
397     return Standard_False;
398   }
399
400   aInvD = 1.0 / aD;
401   aTVec = theOrig.XYZ() - theV1.XYZ();
402   anU = aInvD * aTVec.Dot (aPVec);
403   if (anU < 0.0 || anU > 1.0)
404   {
405     return Standard_False;
406   }
407
408   aQVec = aTVec.Crossed (anEdge1);
409   aV = aInvD * theDir.Dot (aQVec);
410   if (aV < 0.0 || anU + aV > 1.0)
411   {
412     return Standard_False;
413   }
414
415   aT = aInvD * anEdge2.Dot (aQVec);
416   if (aT < 0 || aT > 1)
417   {
418     return Standard_False;
419   }
420
421   return Standard_True;
422 }
423
424 #undef MEMORY_BLOCK_SIZE