973c2be1 |
1 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
8b7c5e47 |
2 | // |
973c2be1 |
3 | // This file is part of Open CASCADE Technology software library. |
8b7c5e47 |
4 | // |
d5f74e42 |
5 | // This library is free software; you can redistribute it and/or modify it under |
6 | // the terms of the GNU Lesser General Public License version 2.1 as published |
973c2be1 |
7 | // by the Free Software Foundation, with special exception defined in the file |
8 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
9 | // distribution for complete text of the license and disclaimer of any warranty. |
8b7c5e47 |
10 | // |
973c2be1 |
11 | // Alternatively, this file may be used under the terms of Open CASCADE |
12 | // commercial license or contractual agreement. |
8b7c5e47 |
13 | |
14 | #include <ChFi2d_FilletAlgo.hxx> |
15 | |
16 | #include <GeomProjLib.hxx> |
17 | #include <BRep_Tool.hxx> |
18 | #include <Precision.hxx> |
19 | #include <ElSLib.hxx> |
20 | #include <ElCLib.hxx> |
21 | |
22 | #include <Geom2dAPI_ProjectPointOnCurve.hxx> |
23 | #include <GeomAPI_ProjectPointOnCurve.hxx> |
24 | #include <Geom2dAPI_InterCurveCurve.hxx> |
25 | |
26 | #include <TopoDS.hxx> |
27 | #include <TopoDS_Iterator.hxx> |
28 | #include <TColStd_ListIteratorOfListOfReal.hxx> |
29 | |
30 | #include <gp_Circ.hxx> |
31 | #include <Geom_Circle.hxx> |
32 | #include <Geom2d_Line.hxx> |
33 | |
34 | #include <BRepBuilderAPI_MakeEdge.hxx> |
35 | #include <BRepAdaptor_Curve.hxx> |
36 | |
37 | ChFi2d_FilletAlgo::ChFi2d_FilletAlgo() |
cbff1e55 |
38 | : myStart1(0.0), |
39 | myEnd1 (0.0), |
40 | myStart2(0.0), |
41 | myEnd2 (0.0), |
42 | myRadius(0.0), |
43 | myStartSide (Standard_False), |
44 | myEdgesExchnged(Standard_False), |
45 | myDegreeOfRecursion(0) |
8b7c5e47 |
46 | { |
8b7c5e47 |
47 | } |
48 | |
cbff1e55 |
49 | ChFi2d_FilletAlgo::ChFi2d_FilletAlgo(const TopoDS_Wire& theWire, const gp_Pln& thePlane) |
50 | : myStart1(0.0), |
51 | myEnd1 (0.0), |
52 | myStart2(0.0), |
53 | myEnd2 (0.0), |
54 | myRadius(0.0), |
55 | myStartSide (Standard_False), |
56 | myEdgesExchnged(Standard_False), |
57 | myDegreeOfRecursion(0) |
8b7c5e47 |
58 | { |
59 | Init(theWire, thePlane); |
60 | } |
61 | |
62 | ChFi2d_FilletAlgo::ChFi2d_FilletAlgo(const TopoDS_Edge& theEdge1, |
63 | const TopoDS_Edge& theEdge2, |
64 | const gp_Pln& thePlane) |
cbff1e55 |
65 | : myEdge1(theEdge1), |
66 | myEdge2(theEdge2), |
67 | myStart1(0.0), |
68 | myEnd1 (0.0), |
69 | myStart2(0.0), |
70 | myEnd2 (0.0), |
71 | myRadius(0.0), |
72 | myStartSide (Standard_False), |
73 | myEdgesExchnged(Standard_False), |
74 | myDegreeOfRecursion(0) |
8b7c5e47 |
75 | { |
76 | Init(theEdge1, theEdge2, thePlane); |
77 | } |
78 | |
79 | void ChFi2d_FilletAlgo::Init(const TopoDS_Wire& theWire, const gp_Pln& thePlane) |
80 | { |
81 | TopoDS_Edge theEdge1, theEdge2; |
82 | TopoDS_Iterator itr(theWire); |
83 | for (; itr.More(); itr.Next()) |
84 | { |
85 | if (theEdge1.IsNull()) |
86 | theEdge1 = TopoDS::Edge(itr.Value()); |
87 | else if (theEdge2.IsNull()) |
88 | theEdge2 = TopoDS::Edge(itr.Value()); |
89 | else |
90 | break; |
91 | } |
92 | if (theEdge1.IsNull() || theEdge2.IsNull()) |
9775fa61 |
93 | throw Standard_ConstructionError("The fillet algorithms expects a wire consisting of two edges."); |
8b7c5e47 |
94 | Init(theEdge1, theEdge2, thePlane); |
95 | } |
96 | |
97 | void ChFi2d_FilletAlgo::Init(const TopoDS_Edge& theEdge1, |
98 | const TopoDS_Edge& theEdge2, |
99 | const gp_Pln& thePlane) |
100 | { |
101 | myPlane = new Geom_Plane(thePlane); |
102 | |
103 | myEdgesExchnged = Standard_False; |
104 | |
105 | BRepAdaptor_Curve aBAC1(theEdge1); |
106 | BRepAdaptor_Curve aBAC2(theEdge2); |
107 | if (aBAC1.GetType() < aBAC2.GetType()) |
108 | { // first curve must be more complicated |
109 | myEdge1 = theEdge2; |
110 | myEdge2 = theEdge1; |
111 | myEdgesExchnged = Standard_True; |
112 | } |
113 | else |
114 | { |
115 | myEdge1 = theEdge1; |
116 | myEdge2 = theEdge2; |
117 | } |
118 | |
119 | Handle(Geom_Curve) aCurve1 = BRep_Tool::Curve(myEdge1, myStart1, myEnd1); |
120 | Handle(Geom_Curve) aCurve2 = BRep_Tool::Curve(myEdge2, myStart2, myEnd2); |
121 | |
122 | myCurve1 = GeomProjLib::Curve2d(aCurve1, myStart1, myEnd1, myPlane); |
123 | myCurve2 = GeomProjLib::Curve2d(aCurve2, myStart2, myEnd2, myPlane); |
124 | |
125 | while (myCurve1->IsPeriodic() && myStart1 >= myEnd1) |
126 | myEnd1 += myCurve1->Period(); |
127 | while (myCurve2->IsPeriodic() && myStart2 >= myEnd2) |
128 | myEnd2 += myCurve2->Period(); |
129 | |
130 | if (aBAC1.GetType() == aBAC2.GetType()) |
131 | { |
132 | if (myEnd2 - myStart2 < myEnd1 - myStart1) |
133 | { // first curve must be parametrically shorter |
134 | TopoDS_Edge anEdge = myEdge1; |
135 | myEdge1 = myEdge2; |
136 | myEdge2 = anEdge; |
137 | Handle(Geom2d_Curve) aCurve = myCurve1; |
138 | myCurve1 = myCurve2; |
139 | myCurve2 = aCurve; |
140 | Standard_Real a = myStart1; |
141 | myStart1 = myStart2; |
142 | myStart2 = a; |
143 | a = myEnd1; |
144 | myEnd1 = myEnd2; |
145 | myEnd2 = a; |
146 | myEdgesExchnged = Standard_True; |
147 | } |
148 | } |
149 | } |
150 | |
151 | //! This function returns true if linear segment from start point of the |
152 | //! fillet arc to the end point is intersected by the first or second |
153 | //! curve: in this case fillet is invalid. |
154 | static Standard_Boolean IsRadiusIntersected(const Handle(Geom2d_Curve)& theCurve, const Standard_Real theCurveMin, const double theCurveMax, |
155 | const gp_Pnt2d theStart, const gp_Pnt2d theEnd, const Standard_Boolean theStartConnected) |
156 | { |
157 | //Check the given start and end if they are identical. If yes |
158 | //return false |
159 | if (theStart.SquareDistance(theEnd) < Precision::SquareConfusion()) |
160 | { |
161 | return Standard_False; |
162 | } |
163 | Handle(Geom2d_Line) line = new Geom2d_Line(theStart, gp_Dir2d(gp_Vec2d(theStart, theEnd))); |
164 | Geom2dAPI_InterCurveCurve anInter(theCurve, line, Precision::Confusion()); |
165 | Standard_Integer a; |
166 | gp_Pnt2d aPoint; |
167 | for(a = anInter.NbPoints(); a > 0; a--) |
168 | { |
169 | aPoint = anInter.Point(a); |
170 | Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, theCurve, theCurveMin, theCurveMax); |
171 | if (aProjInt.NbPoints() < 1 || aProjInt.LowerDistanceParameter() > Precision::Confusion()) |
172 | continue; // point is not on edge |
173 | |
174 | if (aPoint.Distance(theStart) < Precision::Confusion()) |
175 | { |
176 | if (!theStartConnected) |
177 | return Standard_True; |
178 | } |
179 | if (aPoint.Distance(theEnd) < Precision::Confusion()) |
180 | return Standard_True; |
181 | if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), Precision::Angular())) |
182 | return Standard_True; |
183 | } |
184 | Handle(Geom2d_Curve) aCurve = theCurve; |
185 | for(a = anInter.NbSegments(); a > 0; a--) |
186 | { |
187 | //anInter.Segment(a, aCurve); //not implemented (bug in OCC) |
188 | aPoint = aCurve->Value(aCurve->FirstParameter()); |
189 | |
190 | Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, theCurve, theCurveMin, theCurveMax); |
191 | if (aProjInt.NbPoints() && aProjInt.LowerDistanceParameter() < Precision::Confusion()) |
192 | { // point is on edge |
193 | if (aPoint.Distance(theStart) < Precision::Confusion()) |
194 | if (!theStartConnected) |
195 | return Standard_True; |
196 | if (aPoint.Distance(theEnd) < Precision::Confusion()) |
197 | return Standard_True; |
198 | if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), Precision::Angular())) |
199 | return Standard_True; |
200 | } |
201 | aPoint = aCurve->Value(aCurve->LastParameter()); |
202 | |
203 | aProjInt.Init(aPoint, theCurve, theCurveMin, theCurveMax); |
204 | if (aProjInt.NbPoints() && aProjInt.LowerDistanceParameter() < Precision::Confusion()) |
205 | { // point is on edge |
206 | if (aPoint.Distance(theStart) < Precision::Confusion()) |
207 | if (!theStartConnected) |
208 | return Standard_True; |
209 | if (aPoint.Distance(theEnd) < Precision::Confusion()) |
210 | return Standard_True; |
211 | if (gp_Vec2d(aPoint, theStart).IsOpposite(gp_Vec2d(aPoint, theEnd), Precision::Angular())) |
212 | return Standard_True; |
213 | } |
214 | } |
215 | return Standard_False; |
216 | } |
217 | |
218 | void ChFi2d_FilletAlgo::FillPoint(FilletPoint* thePoint, const Standard_Real theLimit) |
219 | { |
220 | |
221 | // on the intersection point |
222 | Standard_Boolean aValid = Standard_False; |
223 | Standard_Real aStep = Precision::Confusion(); |
224 | gp_Pnt2d aCenter, aPoint; // center of fillet and point on curve1 |
225 | Standard_Real aParam = thePoint->getParam(); |
226 | if (theLimit < aParam) aStep = -aStep; |
227 | for(aValid = Standard_False; !aValid; aParam += aStep) |
228 | { |
229 | if ((aParam - aStep - theLimit) * (aParam - theLimit) <= 0) |
230 | break; // limit was exceeded |
231 | aStep *= 2; |
232 | gp_Vec2d aVec; |
233 | myCurve1->D1(aParam, aPoint, aVec); |
234 | if (aVec.SquareMagnitude() < Precision::Confusion()) |
235 | continue; |
236 | |
237 | gp_Vec2d aPerp(((myStartSide)?-1:1) * aVec.Y(), ((myStartSide)?1:-1) * aVec.X()); |
238 | aPerp.Normalize(); |
239 | aPerp.Multiply(myRadius); |
240 | aCenter = aPoint.Translated(aPerp); |
241 | |
242 | |
243 | Geom2dAPI_ProjectPointOnCurve aProjInt(aPoint, myCurve2, myStart2, myEnd2); |
244 | if (aProjInt.NbPoints() == 0 || aPoint.Distance(aProjInt.NearestPoint()) > Precision::Confusion()) |
245 | { |
246 | aValid = Standard_True; |
247 | break; |
248 | } |
249 | } |
250 | if (aValid) |
251 | { |
252 | thePoint->setParam(aParam); |
253 | thePoint->setCenter(aCenter); |
254 | aValid = !IsRadiusIntersected(myCurve2, myStart2, myEnd2, aPoint, aCenter, Standard_True); |
255 | } |
256 | |
257 | Geom2dAPI_ProjectPointOnCurve aProj(aCenter, myCurve2); |
258 | int a, aNB = aProj.NbPoints(); |
259 | for(a = aNB; a > 0; a--) |
260 | { |
261 | if (aPoint.SquareDistance(aProj.Point(a)) < Precision::Confusion()) |
262 | continue; |
263 | |
264 | Standard_Boolean aValid2 = aValid; |
265 | if (aValid2) |
266 | aValid2 = !IsRadiusIntersected(myCurve1, myStart1, myEnd1, aCenter, aProj.Point(a), Standard_False); |
267 | |
268 | // checking the right parameter |
51740958 |
269 | Standard_Real aParamProj = aProj.Parameter(a); |
270 | while(myCurve2->IsPeriodic() && aParamProj < myStart2) |
271 | aParamProj += myCurve2->Period(); |
8b7c5e47 |
272 | |
273 | const Standard_Real d = aProj.Distance(a); |
51740958 |
274 | thePoint->appendValue(d * d - myRadius * myRadius, (aParamProj >= myStart2 && aParamProj <= myEnd2 && aValid2)); |
8b7c5e47 |
275 | if (Abs(d - myRadius) < Precision::Confusion()) |
51740958 |
276 | thePoint->setParam2(aParamProj); |
8b7c5e47 |
277 | } |
278 | } |
279 | |
280 | void ChFi2d_FilletAlgo::FillDiff(FilletPoint* thePoint, Standard_Real theDiffStep, Standard_Boolean theFront) |
281 | { |
282 | Standard_Real aDelta = theFront?(theDiffStep):(-theDiffStep); |
283 | FilletPoint* aDiff = new FilletPoint(thePoint->getParam() + aDelta); |
284 | FillPoint(aDiff, aDelta * 999.); |
285 | if (!thePoint->calculateDiff(aDiff)) |
286 | { |
287 | aDiff->setParam(thePoint->getParam() - aDelta); |
288 | FillPoint(aDiff, - aDelta * 999); |
289 | thePoint->calculateDiff(aDiff); |
290 | } |
291 | delete aDiff; |
292 | } |
293 | |
294 | // returns true, if at least one result was found |
295 | Standard_Boolean ChFi2d_FilletAlgo::Perform(const Standard_Real theRadius) |
296 | { |
297 | myDegreeOfRecursion = 0; |
298 | myResultParams.Clear(); |
299 | myResultOrientation.Clear(); |
300 | |
301 | Standard_Real aNBSteps; |
302 | Geom2dAdaptor_Curve aGAC(myCurve1); |
303 | switch (aGAC.GetType()) |
304 | { |
305 | case GeomAbs_Line: |
306 | aNBSteps = 2; |
307 | break; |
308 | case GeomAbs_Circle: |
309 | aNBSteps = 4; |
310 | break; |
311 | case GeomAbs_Ellipse: |
312 | aNBSteps = 5; |
313 | break; |
314 | case GeomAbs_BSplineCurve: |
315 | aNBSteps = 2 + aGAC.Degree() * aGAC.NbPoles(); |
316 | break; |
317 | default: // unknown: maximum |
318 | aNBSteps = 100; |
319 | } |
04232180 |
320 | //std::cout<<"aNBSteps = "<<aNBSteps<<std::endl; |
8b7c5e47 |
321 | |
322 | myRadius = theRadius; |
323 | Standard_Real aParam, aStep, aDStep; |
324 | aStep = (myEnd1 - myStart1) / aNBSteps; |
325 | aDStep = 1.e-4 * aStep; |
326 | |
327 | int aCycle; |
328 | for(aCycle = 2, myStartSide = Standard_False; aCycle; myStartSide = !myStartSide, aCycle--) |
329 | { |
330 | FilletPoint *aLeft = NULL, *aRight; |
331 | |
332 | for(aParam = myStart1 + aStep; aParam < myEnd1 || Abs(myEnd1 - aParam) < Precision::Confusion(); aParam += aStep) |
333 | { |
334 | if (!aLeft) |
335 | { |
336 | aLeft = new FilletPoint(aParam - aStep); |
337 | FillPoint(aLeft, aParam); |
338 | FillDiff(aLeft, aDStep, Standard_True); |
339 | } |
340 | |
341 | aRight = new FilletPoint(aParam); |
342 | FillPoint(aRight, aParam - aStep); |
343 | FillDiff(aRight, aDStep, Standard_False); |
344 | |
345 | aLeft->FilterPoints(aRight); |
346 | PerformNewton(aLeft, aRight); |
347 | |
348 | delete aLeft; |
349 | aLeft = aRight; |
350 | }//for |
351 | delete aLeft; |
352 | }//for |
353 | |
354 | return !myResultParams.IsEmpty(); |
355 | } |
356 | |
357 | Standard_Boolean ChFi2d_FilletAlgo::ProcessPoint(FilletPoint* theLeft, FilletPoint* theRight, Standard_Real theParameter) |
358 | { |
359 | if (theParameter >= theLeft->getParam() && theParameter < theRight->getParam()) |
360 | { |
361 | Standard_Real aDX = (theRight->getParam() - theLeft->getParam()); |
362 | if (theParameter - theLeft->getParam() < aDX/100.0) |
363 | { |
364 | theParameter = theLeft->getParam() + aDX/100.0; |
365 | } |
366 | if (theRight->getParam() - theParameter < aDX/100.0) |
367 | { |
368 | theParameter = theRight->getParam() - aDX/100.0; |
369 | } |
370 | |
371 | // Protection on infinite loops. |
372 | myDegreeOfRecursion++; |
373 | Standard_Real diffx = 0.001 * aDX; |
374 | if (myDegreeOfRecursion > 100) |
375 | { |
376 | diffx *= 10.0; |
377 | if (myDegreeOfRecursion > 1000) |
378 | { |
379 | diffx *= 10.0; |
380 | if (myDegreeOfRecursion > 3000) |
381 | { |
382 | return Standard_True; |
383 | } |
384 | } |
385 | } |
386 | |
387 | FilletPoint* aPoint1 = theLeft->Copy(); |
388 | FilletPoint* aPoint2 = new FilletPoint(theParameter); |
389 | FillPoint(aPoint2, aPoint1->getParam()); |
390 | FillDiff(aPoint2, diffx, Standard_True); |
391 | |
392 | aPoint1->FilterPoints(aPoint2); |
393 | PerformNewton(aPoint1, aPoint2); |
394 | aPoint2->FilterPoints(theRight); |
395 | PerformNewton(aPoint2, theRight); |
396 | |
397 | delete aPoint1; |
398 | delete aPoint2; |
399 | return Standard_True; |
400 | } |
401 | |
402 | return Standard_False; |
403 | } |
404 | |
405 | void ChFi2d_FilletAlgo::PerformNewton(FilletPoint* theLeft, FilletPoint* theRight) |
406 | { |
407 | int a; |
408 | // check the left: if this is solution store it and remove it from the list of researching points of theLeft |
409 | a = theLeft->hasSolution(myRadius); |
410 | if (a) |
411 | { |
412 | if (theLeft->isValid(a)) |
413 | { |
414 | myResultParams.Append(theLeft->getParam()); |
415 | myResultOrientation.Append(myStartSide); |
416 | } |
417 | return; |
418 | } |
419 | |
420 | Standard_Real aDX = theRight->getParam() - theLeft->getParam(); |
421 | if (aDX < 1.e-6 * Precision::Confusion()) |
422 | { |
423 | a = theRight->hasSolution(myRadius); |
424 | if (a && theRight->isValid(a)) |
425 | { |
426 | myResultParams.Append(theRight->getParam()); |
427 | myResultOrientation.Append(myStartSide); |
428 | } |
429 | return; |
430 | } |
431 | for(a = 1; a <= theLeft->getNBValues(); a++) |
432 | { |
433 | int aNear = theLeft->getNear(a); |
434 | |
435 | Standard_Real aA = (theRight->getDiff(aNear) - theLeft->getDiff(a)) / aDX; |
436 | Standard_Real aB = theLeft->getDiff(a) - aA * theLeft->getParam(); |
437 | Standard_Real aC = theLeft->getValue(a) - theLeft->getDiff(a) * theLeft->getParam() + aA * theLeft->getParam() * theLeft->getParam() / 2.0; |
438 | Standard_Real aDet = aB * aB - 2.0 * aA * aC; |
439 | |
440 | if (Abs(aA) < Precision::Confusion()) |
441 | { // linear case |
04232180 |
442 | //std::cout<<"###"<<std::endl; |
8b7c5e47 |
443 | if (Abs(aB) > 10e-20) |
444 | { |
445 | Standard_Real aX0 = - aC / aB; // use extremum |
446 | if (aX0 > theLeft->getParam() && aX0 < theRight->getParam()) |
447 | ProcessPoint(theLeft, theRight, aX0); |
448 | } |
449 | else |
450 | { |
451 | ProcessPoint(theLeft, theRight, theLeft->getParam() + aDX / 2.0); // linear division otherwise |
452 | } |
453 | } |
454 | else |
455 | { |
456 | if (Abs(aB) > Abs(aDet * 1000000.)) |
457 | { // possible floating point operations accurancy errors |
04232180 |
458 | //std::cout<<"*"; |
8b7c5e47 |
459 | ProcessPoint(theLeft, theRight, theLeft->getParam() + aDX / 2.0); // linear division otherwise |
460 | } |
461 | else |
462 | { |
463 | if (aDet > 0) |
464 | { // two solutions |
465 | aDet = sqrt(aDet); |
466 | Standard_Boolean aRes = ProcessPoint(theLeft, theRight, (- aB + aDet) / aA); |
467 | if (!aRes) |
468 | aRes = ProcessPoint(theLeft, theRight, (- aB - aDet) / aA); |
469 | if (!aRes) |
470 | ProcessPoint(theLeft, theRight, theLeft->getParam() + aDX / 2.0); // linear division otherwise |
471 | } |
472 | else |
473 | { |
04232180 |
474 | //std::cout<<"%%%"<<std::endl; |
8b7c5e47 |
475 | Standard_Real aX0 = - aB / aA; // use extremum |
476 | if (aX0 > theLeft->getParam() && aX0 < theRight->getParam()) |
477 | ProcessPoint(theLeft, theRight, aX0); |
478 | else |
479 | ProcessPoint(theLeft, theRight, theLeft->getParam() + aDX / 2.0); // linear division otherwise |
480 | } |
481 | } |
482 | } |
483 | }//for |
484 | } |
485 | |
486 | // returns number of possible solutions. |
487 | int ChFi2d_FilletAlgo::NbResults(const gp_Pnt& thePoint) |
488 | { |
489 | Standard_Real aX, aY; |
490 | gp_Pnt2d aTargetPoint2d; |
491 | ElSLib::PlaneParameters(myPlane->Pln().Position(), thePoint, aX, aY); |
492 | aTargetPoint2d.SetCoord(aX, aY); |
493 | |
494 | //iterate through all possible solutions. |
495 | int i = 1, nb = 0; |
496 | TColStd_ListIteratorOfListOfReal anIter(myResultParams); |
497 | for(; anIter.More(); anIter.Next(), i++) |
498 | { |
499 | myStartSide = (myResultOrientation.Value(i)) ? Standard_True : Standard_False; |
500 | FilletPoint *aPoint = new FilletPoint(anIter.Value()); |
501 | FillPoint(aPoint, anIter.Value() + 1.); |
502 | if (aPoint->hasSolution(myRadius)) |
503 | nb++; |
504 | delete aPoint; |
505 | }//for |
506 | |
507 | return nb; |
508 | } |
509 | |
510 | // returns result (fillet edge, modified edge1, modified edge2), neares to the given point <thePoint> |
511 | TopoDS_Edge ChFi2d_FilletAlgo::Result(const gp_Pnt& thePoint, TopoDS_Edge& theEdge1, TopoDS_Edge& theEdge2, const int iSolution) |
512 | { |
513 | TopoDS_Edge aResult; |
514 | gp_Pnt2d aTargetPoint2d; |
515 | Standard_Real aX, aY; |
516 | ElSLib::PlaneParameters(myPlane->Pln().Position(), thePoint, aX, aY); |
517 | aTargetPoint2d.SetCoord(aX, aY); |
518 | |
519 | // choose the nearest circle |
520 | Standard_Real aDistance = 0.0, aP; |
521 | FilletPoint *aNearest; |
522 | int a, iSol = 1; |
523 | TColStd_ListIteratorOfListOfReal anIter(myResultParams); |
524 | for(aNearest = NULL, a = 1; anIter.More(); anIter.Next(), a++) |
525 | { |
526 | myStartSide = (myResultOrientation.Value(a))?Standard_True:Standard_False; |
527 | FilletPoint *aPoint = new FilletPoint(anIter.Value()); |
528 | FillPoint(aPoint, anIter.Value() + 1.); |
529 | if (!aPoint->hasSolution(myRadius)) |
530 | { |
531 | delete aPoint; |
532 | continue; |
533 | } |
534 | aP = DBL_MAX; |
535 | if (iSolution == -1) |
536 | { |
537 | aP = Abs(aPoint->getCenter().Distance(aTargetPoint2d) - myRadius); |
538 | } |
539 | else if (iSolution == iSol) |
540 | { |
541 | aP = 0.0; |
542 | } |
543 | if (!aNearest || aP < aDistance) |
544 | { |
545 | aNearest = aPoint; |
546 | aDistance = aP; |
547 | } |
548 | else |
549 | { |
550 | delete aPoint; |
551 | } |
552 | if (iSolution == iSol) |
553 | break; |
554 | iSol++; |
555 | }//for |
556 | |
557 | if (!aNearest) |
558 | return aResult; |
559 | |
560 | // create circle edge |
561 | gp_Pnt aCenter = ElSLib::PlaneValue(aNearest->getCenter().X(), aNearest->getCenter().Y(), myPlane->Pln().Position()); |
562 | Handle(Geom_Circle) aCircle = new Geom_Circle(gp_Ax2(aCenter, myPlane->Pln().Axis().Direction()), myRadius); |
563 | gp_Pnt2d aPoint2d1, aPoint2d2; |
564 | myCurve1->D0(aNearest->getParam(), aPoint2d1); |
565 | myCurve2->D0(aNearest->getParam2(), aPoint2d2); |
566 | gp_Pnt aPoint1 = ElSLib::PlaneValue(aPoint2d1.X(), aPoint2d1.Y(), myPlane->Pln().Position()); |
567 | gp_Pnt aPoint2 = ElSLib::PlaneValue(aPoint2d2.X(), aPoint2d2.Y(), myPlane->Pln().Position()); |
568 | |
569 | GeomAPI_ProjectPointOnCurve aProj(thePoint, aCircle); |
570 | Standard_Real aTargetParam = aProj.LowerDistanceParameter(); |
571 | gp_Pnt aPointOnCircle = aProj.NearestPoint(); |
572 | |
573 | // There is a bug in Open CASCADE in calculation of nearest point to a circle near the parameter 0.0 |
574 | // Therefore I check this extrema point manually: |
575 | gp_Pnt p0 = ElCLib::Value(0.0, aCircle->Circ()); |
576 | if (p0.Distance(thePoint) < aPointOnCircle.Distance(thePoint)) |
577 | { |
578 | aTargetParam = 0.0; |
579 | aPointOnCircle = p0; |
580 | } |
581 | |
582 | aProj.Perform(aPoint1); |
583 | Standard_Real aParam1 = aProj.LowerDistanceParameter(); |
584 | aProj.Perform(aPoint2); |
585 | Standard_Real aParam2 = aProj.LowerDistanceParameter(); |
586 | Standard_Boolean aIsOut = ((aParam1 < aTargetParam && aParam2 < aTargetParam) || (aParam1 > aTargetParam && aParam2 > aTargetParam)); |
587 | if (aParam1 > aParam2) |
588 | aIsOut = !aIsOut; |
589 | BRepBuilderAPI_MakeEdge aBuilder(aCircle->Circ(), aIsOut ? aParam2 : aParam1, aIsOut? aParam1 : aParam2); |
590 | aResult = aBuilder.Edge(); |
591 | |
592 | // divide edges |
593 | Standard_Real aStart, anEnd; |
594 | Handle(Geom_Curve) aCurve = BRep_Tool::Curve(myEdge1, aStart, anEnd); |
595 | gp_Vec aDir; |
596 | aCurve->D1(aNearest->getParam(), aPoint1, aDir); |
597 | |
598 | gp_Vec aCircleDir; |
599 | aCircle->D1(aParam1, aPoint1, aCircleDir); |
600 | |
601 | if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ aIsOut) |
602 | aStart = aNearest->getParam(); |
603 | else |
604 | anEnd = aNearest->getParam(); |
605 | |
606 | //Check the case when start and end are identical. This happens |
607 | //when the edge decreases to size 0. Old ww5 allows such |
608 | //cases. So we are again bug compatible |
609 | if (fabs(aStart - anEnd) < Precision::Confusion()) |
610 | anEnd = aStart + Precision::Confusion(); |
611 | //Divide edge |
612 | BRepBuilderAPI_MakeEdge aDivider1(aCurve, aStart, anEnd); |
613 | if (myEdgesExchnged) |
614 | theEdge2 = aDivider1.Edge(); |
615 | else |
616 | theEdge1 = aDivider1.Edge(); |
617 | |
618 | aCurve = BRep_Tool::Curve(myEdge2, aStart, anEnd); |
619 | aCurve->D1(aNearest->getParam2(), aPoint2, aDir); |
620 | |
621 | aCircle->D1(aParam2, aPoint2, aCircleDir); |
622 | |
623 | if ((aCircleDir.Angle(aDir) > M_PI / 2.0) ^ (!aIsOut)) |
624 | aStart = aNearest->getParam2(); |
625 | else |
626 | anEnd = aNearest->getParam2(); |
627 | |
628 | //Check the case when start and end are identical. This happens |
629 | //when the edge decreases to size 0. Old ww5 allows such |
630 | //cases. So we are again bug compatible |
631 | if (fabs(aStart - anEnd) < Precision::Confusion()) |
632 | anEnd = aStart + Precision::Confusion(); |
633 | BRepBuilderAPI_MakeEdge aDivider2(aCurve, aStart, anEnd); |
634 | if (myEdgesExchnged) |
635 | theEdge1 = aDivider2.Edge(); |
636 | else |
637 | theEdge2 = aDivider2.Edge(); |
638 | |
639 | delete aNearest; |
640 | return aResult; |
641 | } |
642 | |
cbff1e55 |
643 | FilletPoint::FilletPoint(const Standard_Real theParam) |
644 | : myParam (theParam), |
645 | myParam2(0.0) |
646 | { |
647 | } |
648 | |
8b7c5e47 |
649 | void FilletPoint::appendValue(Standard_Real theValue, Standard_Boolean theValid) |
650 | { |
651 | Standard_Integer a; |
652 | for(a = 1; a <= myV.Length(); a++) |
653 | { |
654 | if (theValue < myV.Value(a)) |
655 | { |
656 | myV.InsertBefore(a, theValue); |
dde68833 |
657 | myValid.InsertBefore(a, theValid); |
8b7c5e47 |
658 | return; |
659 | } |
660 | } |
661 | myV.Append(theValue); |
dde68833 |
662 | myValid.Append(theValid); |
8b7c5e47 |
663 | } |
664 | |
665 | Standard_Boolean FilletPoint::calculateDiff(FilletPoint* thePoint) |
666 | { |
667 | Standard_Integer a; |
668 | Standard_Boolean aDiffsSet = (myD.Length() != 0); |
669 | Standard_Real aDX = thePoint->getParam() - myParam, aDY = 0.0; |
670 | if (thePoint->myV.Length() == myV.Length()) |
671 | { // absolutely the same points |
672 | for(a = 1; a <= myV.Length(); a++) |
673 | { |
674 | aDY = thePoint->myV.Value(a) - myV.Value(a); |
675 | if (aDiffsSet) |
676 | myD.SetValue(a, aDY / aDX); |
677 | else |
678 | myD.Append(aDY / aDX); |
679 | } |
680 | return Standard_True; |
681 | } |
682 | // between the diffeerent points searching for nearest analogs |
683 | Standard_Integer b; |
684 | for(a = 1; a <= myV.Length(); a++) |
685 | { |
686 | for(b = 1; b <= thePoint->myV.Length(); b++) |
687 | { |
688 | if (b == 1 || Abs(thePoint->myV.Value(b) - myV.Value(a)) < Abs(aDY)) |
689 | aDY = thePoint->myV.Value(b) - myV.Value(a); |
690 | } |
691 | if (aDiffsSet) |
692 | { |
693 | if (Abs(aDY / aDX) < Abs(myD.Value(a))) |
694 | myD.SetValue(a, aDY / aDX); |
695 | } |
696 | else |
697 | { |
698 | myD.Append(aDY / aDX); |
699 | } |
700 | }//for |
701 | |
702 | return Standard_False; |
703 | } |
704 | |
705 | void FilletPoint::FilterPoints(FilletPoint* thePoint) |
706 | { |
707 | Standard_Integer a, b; |
708 | TColStd_SequenceOfReal aDiffs; |
709 | Standard_Real aY, aY2, aDX = thePoint->getParam() - myParam; |
710 | for(a = 1; a <= myV.Length(); a++) |
711 | { |
712 | // searching for near point from thePoint |
713 | Standard_Integer aNear = 0; |
714 | Standard_Real aDiff = aDX * 10000.; |
715 | aY = myV.Value(a) + myD.Value(a) * aDX; |
716 | for(b = 1; b <= thePoint->myV.Length(); b++) |
717 | { |
718 | // calculate hypothesis value of the Y2 with the constant first and second derivative |
719 | aY2 = aY + aDX * (thePoint->myD.Value(b) - myD.Value(a)) / 2.0; |
720 | if (aNear == 0 || Abs(aY2 - thePoint->myV.Value(b)) < Abs(aDiff)) |
721 | { |
722 | aNear = b; |
723 | aDiff = aY2 - thePoint->myV.Value(b); |
724 | } |
725 | }//for b... |
726 | |
727 | if (aNear) |
728 | { |
729 | if (myV.Value(a) * thePoint->myV.Value(aNear) > 0) |
730 | {// the same sign at the same sides of the interval |
731 | if (myV.Value(a) * myD.Value(a) > 0) |
732 | { |
733 | if (Abs(myD.Value(a)) > Precision::Confusion()) |
734 | aNear = 0; |
735 | } |
736 | else |
737 | { |
738 | if (Abs(myV.Value(a)) > Abs(thePoint->myV.Value(aNear))) |
739 | if (thePoint->myV.Value(aNear) * thePoint->myD.Value(aNear) < 0 && Abs(thePoint->myD.Value(aNear)) > Precision::Confusion()) |
740 | { |
741 | aNear = 0; |
742 | } |
743 | } |
744 | } |
745 | }//if aNear |
746 | |
747 | if (aNear) |
748 | { |
749 | if (myV.Value(a) * thePoint->myV.Value(aNear) > 0) |
750 | { |
751 | if ((myV.Value(a) + myD.Value(a) * aDX) * myV.Value(a) > Precision::Confusion() && |
752 | (thePoint->myV.Value(aNear) + thePoint->myD.Value(aNear) * aDX) * thePoint->myV.Value(aNear) > Precision::Confusion()) |
753 | { |
754 | aNear = 0; |
755 | } |
756 | } |
757 | }//if aNear |
758 | |
759 | if (aNear) |
760 | { |
761 | if (Abs(aDiff / aDX) > 1.e+7) |
762 | { |
763 | aNear = 0; |
764 | } |
765 | } |
766 | |
767 | if (aNear == 0) |
768 | { // there is no near: remove it from the list |
769 | myV.Remove(a); |
770 | myD.Remove(a); |
771 | myValid.Remove(a); |
772 | a--; |
773 | } |
774 | else |
775 | { |
776 | Standard_Boolean aFound = Standard_False; |
777 | for(b = 1; b <= myNear.Length(); b++) |
778 | { |
779 | if (myNear.Value(b) == aNear) |
780 | { |
781 | if (Abs(aDiffs.Value(b)) < Abs(aDiff)) |
782 | { // return this 'near' |
783 | aFound = Standard_True; |
784 | myV.Remove(a); |
785 | myD.Remove(a); |
786 | myValid.Remove(a); |
787 | a--; |
788 | break; |
789 | } |
790 | else |
791 | { // remove the old 'near' |
792 | myV.Remove(b); |
793 | myD.Remove(b); |
794 | myValid.Remove(b); |
795 | myNear.Remove(b); |
796 | aDiffs.Remove(b); |
797 | a--; |
798 | break; |
799 | } |
800 | } |
801 | }//for b... |
802 | if (!aFound) |
803 | { |
804 | myNear.Append(aNear); |
805 | aDiffs.Append(aDiff); |
806 | } |
807 | }//else |
808 | }//for a... |
809 | } |
810 | |
811 | FilletPoint* FilletPoint::Copy() |
812 | { |
813 | FilletPoint* aCopy = new FilletPoint(myParam); |
814 | Standard_Integer a; |
815 | for(a = 1; a <= myV.Length(); a++) |
816 | { |
817 | aCopy->myV.Append(myV.Value(a)); |
818 | aCopy->myD.Append(myD.Value(a)); |
819 | aCopy->myValid.Append(myValid.Value(a)); |
820 | } |
821 | return aCopy; |
822 | } |
823 | |
824 | int FilletPoint::hasSolution(const Standard_Real theRadius) |
825 | { |
826 | Standard_Integer a; |
827 | for(a = 1; a <= myV.Length(); a++) |
828 | { |
829 | if (Abs(sqrt(Abs(Abs(myV.Value(a)) + theRadius * theRadius)) - theRadius) < Precision::Confusion()) |
830 | return a; |
831 | } |
832 | return 0; |
833 | } |
834 | |
835 | void FilletPoint::remove(int theIndex) |
836 | { |
837 | myV.Remove(theIndex); |
838 | myD.Remove(theIndex); |
839 | myValid.Remove(theIndex); |
840 | myNear.Remove(theIndex); |
841 | } |