8b7c5e47 |
1 | // Copyright (c) 1999-2012 OPEN CASCADE SAS |
2 | // |
3 | // The content of this file is subject to the Open CASCADE Technology Public |
4 | // License Version 6.5 (the "License"). You may not use the content of this file |
5 | // except in compliance with the License. Please obtain a copy of the License |
6 | // at http://www.opencascade.org and read it completely before using this file. |
7 | // |
8 | // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its |
9 | // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France. |
10 | // |
11 | // The Original Code and all software distributed under the License is |
12 | // distributed on an "AS IS" basis, without warranty of any kind, and the |
13 | // Initial Developer hereby disclaims all such warranties, including without |
14 | // limitation, any warranties of merchantability, fitness for a particular |
15 | // purpose or non-infringement. Please see the License for the specific terms |
16 | // and conditions governing the rights and limitations under the License. |
17 | |
18 | #include <ChFi2d_AnaFilletAlgo.hxx> |
19 | |
20 | #include <gp_Ax3.hxx> |
21 | #include <gp_Circ.hxx> |
22 | #include <gp_Lin2d.hxx> |
23 | #include <gp_Circ2d.hxx> |
24 | |
25 | #include <Standard_TypeMismatch.hxx> |
26 | |
27 | #include <BRepBuilderAPI_MakeEdge.hxx> |
28 | #include <BRepBuilderAPI_MakeWire.hxx> |
29 | #include <BRepBuilderAPI_MakeFace.hxx> |
30 | |
31 | #include <GeomAPI_ExtremaCurveCurve.hxx> |
32 | #include <IntAna2d_AnaIntersection.hxx> |
33 | #include <ShapeAnalysis_Wire.hxx> |
34 | #include <Geom_Circle.hxx> |
35 | |
36 | #include <BRepAdaptor_Curve.hxx> |
37 | #include <BRep_Tool.hxx> |
38 | |
39 | #include <TopoDS.hxx> |
40 | #include <TopoDS_Iterator.hxx> |
41 | |
42 | #include <ProjLib.hxx> |
43 | #include <TopExp.hxx> |
44 | #include <ElSLib.hxx> |
45 | |
46 | // Compute the flag: CW || CCW |
47 | static Standard_Boolean isCW(const BRepAdaptor_Curve& AC) |
48 | { |
49 | const Standard_Real f = AC.FirstParameter(); |
50 | const Standard_Real l = AC.LastParameter(); |
51 | Handle(Geom_Circle) circle = Handle(Geom_Circle)::DownCast(AC.Curve().Curve()); |
52 | gp_Pnt start = AC.Value(f); |
53 | gp_Pnt end = AC.Value(l); |
54 | gp_Pnt center = AC.Circle().Location(); |
55 | gp_Ax3 plane = AC.Circle().Position(); |
56 | |
57 | // Get point on circle at half angle |
58 | gp_Pnt m; |
59 | circle->D0(0.5 * (f + l), m); |
60 | |
61 | // Compare angles between vectors to middle point and to the end point. |
62 | gp_Vec startv(center, start), endv(center, end), middlev(center, m); |
63 | double middlea = startv.AngleWithRef(middlev, plane.Direction()); |
64 | while(middlea < 0.0) |
65 | middlea += 2.0 * M_PI; |
66 | double enda = startv.AngleWithRef(endv, plane.Direction()); |
67 | while(enda < 0.0) |
68 | enda += 2.0 * M_PI; |
69 | |
70 | Standard_Boolean is_cw = middlea > enda ? Standard_True : Standard_False; |
71 | return is_cw; |
72 | } |
73 | |
74 | // Equality of points computed through square distance between the points. |
75 | static Standard_Boolean IsEqual(const gp_Pnt& p1, const gp_Pnt& p2) |
76 | { |
77 | return p1.SquareDistance(p2) < Precision::SquareConfusion(); |
78 | } |
79 | static Standard_Boolean IsEqual(const gp_Pnt2d& p1, const gp_Pnt2d& p2) |
80 | { |
81 | return p1.SquareDistance(p2) < Precision::SquareConfusion(); |
82 | } |
83 | |
84 | // An empty constructor. |
85 | // Use the method Init() to initialize the class. |
86 | ChFi2d_AnaFilletAlgo::ChFi2d_AnaFilletAlgo() |
87 | { |
88 | |
89 | } |
90 | |
91 | // An constructor. |
92 | // It expects two edges having a common point of type: |
93 | // - segment |
94 | // - arc of circle. |
95 | ChFi2d_AnaFilletAlgo::ChFi2d_AnaFilletAlgo(const TopoDS_Wire& theWire, |
96 | const gp_Pln& thePlane) |
97 | { |
98 | Init(theWire, thePlane); |
99 | } |
100 | |
101 | // A constructor. |
102 | // It expects two edges having a common point of type: |
103 | // - segment |
104 | // - arc of circle. |
105 | ChFi2d_AnaFilletAlgo::ChFi2d_AnaFilletAlgo(const TopoDS_Edge& theEdge1, |
106 | const TopoDS_Edge& theEdge2, |
107 | const gp_Pln& thePlane) |
108 | { |
109 | // Make a wire consisting of two edges. |
110 | Init(theEdge1, theEdge2, thePlane); |
111 | } |
112 | |
113 | // Initializes the class by a wire consisting of two edges. |
114 | void ChFi2d_AnaFilletAlgo::Init(const TopoDS_Wire& theWire, const gp_Pln& thePlane) |
115 | { |
116 | plane = thePlane; |
117 | TopoDS_Iterator itr(theWire); |
118 | for (; itr.More(); itr.Next()) |
119 | { |
120 | if (e1.IsNull()) |
121 | e1 = TopoDS::Edge(itr.Value()); |
122 | else if (e2.IsNull()) |
123 | e2 = TopoDS::Edge(itr.Value()); |
124 | } |
125 | if (e1.IsNull() || e2.IsNull()) |
126 | Standard_TypeMismatch::Raise("The algorithm expects a wire consisting of two linear or circular edges."); |
127 | |
128 | // Left neighbour. |
129 | BRepAdaptor_Curve AC1(e1); |
130 | if (AC1.GetType() != GeomAbs_Line && AC1.GetType() != GeomAbs_Circle) |
131 | Standard_TypeMismatch::Raise("A segment or an arc of circle is expected."); |
132 | |
133 | TopoDS_Vertex v1, v2; |
134 | TopExp::Vertices(e1, v1, v2, Standard_True); |
135 | if (v1.IsNull() || v2.IsNull()) |
136 | Standard_Failure::Raise("An infinite edge."); |
137 | |
138 | gp_Pnt P1 = BRep_Tool::Pnt(v1); |
139 | gp_Pnt P2 = BRep_Tool::Pnt(v2); |
140 | gp_Pnt2d p1 = ProjLib::Project(thePlane, P1); |
141 | gp_Pnt2d p2 = ProjLib::Project(thePlane, P2); |
142 | p1.Coord(x11, y11); |
143 | p2.Coord(x12, y12); |
144 | |
145 | segment1 = true; |
146 | if (AC1.GetType() == GeomAbs_Circle) |
147 | { |
148 | segment1 = false; |
149 | gp_Circ c = AC1.Circle(); |
150 | |
151 | gp_Pnt2d loc = ProjLib::Project(thePlane, c.Location()); |
152 | loc.Coord(xc1, yc1); |
153 | |
154 | radius1 = c.Radius(); |
155 | cw1 = isCW(AC1); |
156 | } |
157 | |
158 | // Right neighbour. |
159 | BRepAdaptor_Curve AC2(e2); |
160 | if (AC2.GetType() != GeomAbs_Line && AC2.GetType() != GeomAbs_Circle) |
161 | Standard_TypeMismatch::Raise("A segment or an arc of circle is expected."); |
162 | |
163 | TopExp::Vertices(e2, v1, v2, Standard_True); |
164 | if (v1.IsNull() || v2.IsNull()) |
165 | Standard_Failure::Raise("An infinite edge."); |
166 | |
167 | P1 = BRep_Tool::Pnt(v1); |
168 | P2 = BRep_Tool::Pnt(v2); |
169 | p1 = ProjLib::Project(thePlane, P1); |
170 | p2 = ProjLib::Project(thePlane, P2); |
171 | p1.Coord(x21, y21); |
172 | p2.Coord(x22, y22); |
173 | |
174 | segment2 = true; |
175 | if (AC2.GetType() == GeomAbs_Circle) |
176 | { |
177 | segment2 = false; |
178 | gp_Circ c = AC2.Circle(); |
179 | |
180 | gp_Pnt2d loc = ProjLib::Project(thePlane, c.Location()); |
181 | loc.Coord(xc2, yc2); |
182 | |
183 | radius2 = c.Radius(); |
184 | cw2 = isCW(AC2); |
185 | } |
186 | } |
187 | |
188 | // Initializes the class by two edges. |
189 | void ChFi2d_AnaFilletAlgo::Init(const TopoDS_Edge& theEdge1, const TopoDS_Edge& theEdge2, |
190 | const gp_Pln& thePlane) |
191 | { |
192 | // Make a wire consisting of two edges. |
193 | |
194 | // Get common point. |
195 | TopoDS_Vertex v11, v12, v21, v22; |
196 | TopExp::Vertices(theEdge1, v11, v12, Standard_True); |
197 | TopExp::Vertices(theEdge2, v21, v22, Standard_True); |
198 | if (v11.IsNull() || v12.IsNull() || v21.IsNull() || v22.IsNull()) |
199 | Standard_Failure::Raise("An infinite edge."); |
200 | |
201 | gp_Pnt p11 = BRep_Tool::Pnt(v11); |
202 | gp_Pnt p12 = BRep_Tool::Pnt(v12); |
203 | gp_Pnt p21 = BRep_Tool::Pnt(v21); |
204 | gp_Pnt p22 = BRep_Tool::Pnt(v22); |
205 | |
206 | gp_Pnt pcommon; |
207 | if (IsEqual(p11, p21) || IsEqual(p11, p22)) |
208 | { |
209 | pcommon = p11; |
210 | } |
211 | else if (IsEqual(p12, p21) || IsEqual(p12, p22)) |
212 | { |
213 | pcommon = p12; |
214 | } |
215 | else |
216 | Standard_Failure::Raise("The edges have no common point."); |
217 | |
218 | // Reverse the edges in case of need (to construct a wire). |
219 | Standard_Boolean is1stReversed(Standard_False), is2ndReversed(Standard_False); |
220 | if (IsEqual(pcommon, p11)) |
221 | is1stReversed = Standard_True; |
222 | else if (IsEqual(pcommon, p22)) |
223 | is2ndReversed = Standard_True; |
224 | |
225 | // Make a wire. |
226 | BRepBuilderAPI_MakeWire mkWire; |
227 | if (is1stReversed) |
228 | mkWire.Add(TopoDS::Edge(theEdge1.Reversed())); |
229 | else |
230 | mkWire.Add(theEdge1); |
231 | if (is2ndReversed) |
232 | mkWire.Add(TopoDS::Edge(theEdge2.Reversed())); |
233 | else |
234 | mkWire.Add(theEdge2); |
235 | if (!mkWire.IsDone()) |
236 | Standard_Failure::Raise("Can't make a wire."); |
237 | |
238 | const TopoDS_Wire& W = mkWire.Wire(); |
239 | Init(W, thePlane); |
240 | } |
241 | |
242 | // Calculates a fillet. |
243 | Standard_Boolean ChFi2d_AnaFilletAlgo::Perform(const Standard_Real radius) |
244 | { |
245 | Standard_Boolean bRet(false); |
246 | if (e1.IsNull() || e2.IsNull() || |
247 | radius < Precision::Confusion()) |
248 | { |
249 | return bRet; |
250 | } |
251 | |
252 | // Fillet definition. |
253 | Standard_Real xc = 0.0, yc = 0.0; |
254 | Standard_Real start = 0.0, end = 0.0; // parameters on neighbours |
255 | Standard_Real xstart = DBL_MAX, ystart = DBL_MAX; // point on left neighbour |
256 | Standard_Real xend = DBL_MAX, yend = DBL_MAX; // point on right neighbour |
257 | Standard_Boolean cw = Standard_False; |
258 | |
259 | // Analytical algorithm works for non-intersecting arcs only. |
260 | // Check arcs on self-intersection. |
261 | Standard_Boolean isCut(Standard_False); |
262 | if (!segment1 || !segment2) |
263 | { |
264 | BRepBuilderAPI_MakeWire mkWire(e1, e2); |
265 | if (mkWire.IsDone()) |
266 | { |
267 | const TopoDS_Wire& W = mkWire.Wire(); |
268 | BRepBuilderAPI_MakeFace mkFace(plane); |
269 | if (mkFace.IsDone()) |
270 | { |
271 | const TopoDS_Face& F = mkFace.Face(); |
272 | ShapeAnalysis_Wire analyzer(W, F, Precision::Confusion()); |
273 | if (analyzer.CheckSelfIntersection() == Standard_True) |
274 | { |
275 | // Cut the edges at the point of intersection. |
276 | isCut = Standard_True; |
277 | if (!Cut(plane, e1, e2)) |
278 | { |
279 | return Standard_False; |
280 | } |
281 | } |
282 | } |
283 | } |
284 | }// a case of segment - segment |
285 | |
286 | // Choose the case. |
287 | BRepAdaptor_Curve AC1(e1), AC2(e2); |
288 | if (segment1 && segment2) |
289 | { |
290 | bRet = SegmentFilletSegment(radius, xc, yc, cw, start, end); |
291 | } |
292 | else if (segment1 && !segment2) |
293 | { |
294 | bRet = SegmentFilletArc(radius, xc, yc, cw, start, end, xend, yend); |
295 | } |
296 | else if (!segment1 && segment2) |
297 | { |
298 | bRet = ArcFilletSegment(radius, xc, yc, cw, start, end, xstart, ystart); |
299 | } |
300 | else if (!segment1 && !segment2) |
301 | { |
302 | bRet = ArcFilletArc(radius, xc, yc, cw, start, end); |
303 | } |
304 | |
305 | if (!bRet) |
306 | return Standard_False; |
307 | |
308 | // Invert the fillet for left-handed plane. |
309 | if (plane.Position().Direct() == Standard_False) |
310 | cw = !cw; |
311 | |
312 | // Construct a fillet. |
313 | // Make circle. |
314 | gp_Pnt center = ElSLib::Value(xc, yc, plane); |
315 | const gp_Dir& normal = plane.Position().Direction(); |
316 | gp_Circ circ(gp_Ax2(center, cw ? -normal : normal), radius); |
317 | |
318 | // Fillet may only shrink a neighbour edge, it can't prolongate it. |
319 | const Standard_Real delta1 = AC1.LastParameter() - AC1.FirstParameter(); |
320 | const Standard_Real delta2 = AC2.LastParameter() - AC2.FirstParameter(); |
321 | if (!isCut && (start > delta1 || end > delta2)) |
322 | { |
323 | // Check a case when a neighbour edge almost disappears: |
324 | // try to reduce the fillet radius for a little (1.e-5 mm). |
325 | const Standard_Real little = 100.0 * Precision::Confusion(); |
326 | const Standard_Real d1 = fabs(start - delta1); |
327 | const Standard_Real d2 = fabs(end - delta2); |
328 | if (d1 < little || d2 < little) |
329 | { |
330 | if (segment1 && segment2) |
331 | { |
332 | bRet = SegmentFilletSegment(radius - little, xc, yc, cw, start, end); |
333 | } |
334 | else if (segment1 && !segment2) |
335 | { |
336 | bRet = SegmentFilletArc(radius - little, xc, yc, cw, start, end, xend, yend); |
337 | } |
338 | else if (!segment1 && segment2) |
339 | { |
340 | bRet = ArcFilletSegment(radius - little, xc, yc, cw, start, end, xstart, ystart); |
341 | } |
342 | else if (!segment1 && !segment2) |
343 | { |
344 | bRet = ArcFilletArc(radius - little, xc, yc, cw, start, end); |
345 | } |
346 | if (bRet) |
347 | { |
348 | // Invert the fillet for left-handed planes. |
349 | if (plane.Position().Direct() == Standard_False) |
350 | cw = !cw; |
351 | |
352 | // Make the circle again. |
353 | center = ElSLib::Value(xc, yc, plane); |
354 | circ.SetLocation(center); |
355 | circ.SetRadius(radius - little); |
356 | } |
357 | else |
358 | { |
359 | return Standard_False; |
360 | } |
361 | } |
362 | else |
363 | { |
364 | return Standard_False; |
365 | } |
366 | } |
367 | if (bRet) |
368 | { |
369 | // start: (xstart, ystart) - pstart. |
370 | gp_Pnt pstart; |
371 | if (xstart != DBL_MAX) |
372 | { |
373 | pstart = ElSLib::Value(xstart, ystart, plane); |
374 | } |
375 | else |
376 | { |
377 | if (e1.Orientation() == TopAbs_FORWARD) |
378 | pstart = AC1.Value(AC1.LastParameter() - start); |
379 | else |
380 | pstart = AC1.Value(AC1.FirstParameter() + start); |
381 | } |
382 | // end: (xend, yend) -> pend. |
383 | gp_Pnt pend; |
384 | if (xend != DBL_MAX) |
385 | { |
386 | pend = ElSLib::Value(xend, yend, plane); |
387 | } |
388 | else |
389 | { |
390 | if (e2.Orientation() == TopAbs_FORWARD) |
391 | pend = AC2.Value(AC2.FirstParameter() + end); |
392 | else |
393 | pend = AC2.Value(AC2.LastParameter() - end); |
394 | } |
395 | |
396 | // Make arc. |
397 | BRepBuilderAPI_MakeEdge mkEdge(circ, pstart, pend); |
398 | bRet = mkEdge.IsDone(); |
399 | if (bRet) |
400 | { |
401 | fillet = mkEdge.Edge(); |
402 | |
403 | // Limit the neighbours. |
404 | // Left neighbour. |
405 | shrinke1.Nullify(); |
406 | if (segment1) |
407 | { |
408 | BRepBuilderAPI_MakeEdge mkSegment1; |
409 | if (e1.Orientation() == TopAbs_FORWARD) |
410 | mkSegment1.Init(AC1.Curve().Curve(), AC1.FirstParameter(), AC1.LastParameter() - start); |
411 | else |
412 | mkSegment1.Init(AC1.Curve().Curve(), AC1.FirstParameter() + start, AC1.LastParameter()); |
413 | if (mkSegment1.IsDone()) |
414 | shrinke1 = mkSegment1.Edge(); |
415 | } |
416 | else |
417 | { |
418 | BRepBuilderAPI_MakeEdge mkCirc1; |
419 | if (e1.Orientation() == TopAbs_FORWARD) |
420 | mkCirc1.Init(AC1.Curve().Curve(), AC1.FirstParameter(), AC1.LastParameter() - start); |
421 | else |
422 | mkCirc1.Init(AC1.Curve().Curve(), AC1.FirstParameter() + start, AC1.LastParameter()); |
423 | if (mkCirc1.IsDone()) |
424 | shrinke1 = mkCirc1.Edge(); |
425 | } |
426 | |
427 | // Right neighbour. |
428 | shrinke2.Nullify(); |
429 | if (segment2) |
430 | { |
431 | BRepBuilderAPI_MakeEdge mkSegment2; |
432 | if (e2.Orientation() == TopAbs_FORWARD) |
433 | mkSegment2.Init(AC2.Curve().Curve(), AC2.FirstParameter() + end, AC2.LastParameter()); |
434 | else |
435 | mkSegment2.Init(AC2.Curve().Curve(), AC2.FirstParameter(), AC2.LastParameter() - end); |
436 | if (mkSegment2.IsDone()) |
437 | shrinke2 = mkSegment2.Edge(); |
438 | } |
439 | else |
440 | { |
441 | BRepBuilderAPI_MakeEdge mkCirc2; |
442 | if (e2.Orientation() == TopAbs_FORWARD) |
443 | mkCirc2.Init(AC2.Curve().Curve(), AC2.FirstParameter() + end, AC2.LastParameter()); |
444 | else |
445 | mkCirc2.Init(AC2.Curve().Curve(), AC2.FirstParameter(), AC2.LastParameter() - end); |
446 | if (mkCirc2.IsDone()) |
447 | shrinke2 = mkCirc2.Edge(); |
448 | } |
449 | |
450 | bRet = !shrinke1.IsNull() && !shrinke2.IsNull(); |
451 | }// fillet edge is done |
452 | }// shrinking is good |
453 | |
454 | return bRet; |
455 | } |
456 | |
457 | // Retrieves a result (fillet and shrinked neighbours). |
458 | const TopoDS_Edge& ChFi2d_AnaFilletAlgo::Result(TopoDS_Edge& e1, TopoDS_Edge& e2) |
459 | { |
460 | e1 = shrinke1; |
461 | e2 = shrinke2; |
462 | return fillet; |
463 | } |
464 | |
465 | // WW5 method to compute fillet. |
466 | // It returns a constructed fillet definition: |
467 | // center point (xc, yc) |
468 | // point on the 1st segment (xstart, ystart) |
469 | // point on the 2nd segment (xend, yend) |
470 | // is the arc of fillet clockwise (cw = true) or counterclockwise (cw = false). |
471 | Standard_Boolean ChFi2d_AnaFilletAlgo::SegmentFilletSegment(const Standard_Real radius, |
472 | Standard_Real& xc, Standard_Real& yc, |
473 | Standard_Boolean& cw, |
474 | Standard_Real& start, Standard_Real& end) |
475 | { |
476 | // Make normalized vectors at p12. |
477 | gp_Pnt2d p11(x11, y11); |
478 | gp_Pnt2d p12(x12, y12); |
479 | gp_Pnt2d p22(x22, y22); |
480 | |
481 | // Check length of segments. |
482 | if (IsEqual(p12, p11) || IsEqual(p12, p22)) |
483 | { |
484 | return Standard_False; |
485 | } |
486 | |
487 | // Make vectors. |
488 | gp_Vec2d v1(p12, p11); |
489 | gp_Vec2d v2(p12, p22); |
490 | v1.Normalize(); |
491 | v2.Normalize(); |
492 | |
493 | // Make bisectrissa. |
494 | gp_Vec2d bisec = 0.5 * (v1 + v2); |
495 | |
496 | // Check bisectrissa. |
497 | if (bisec.SquareMagnitude() < Precision::SquareConfusion()) |
498 | return Standard_False; |
499 | |
500 | // Normalize the bisectrissa. |
501 | bisec.Normalize(); |
502 | |
503 | // Angle at bisectrissa. |
504 | Standard_Real beta = v1.Angle(bisec); |
505 | |
506 | // Length along the bisectrissa till the center of fillet. |
507 | Standard_Real L = radius / sin(fabs(beta)); |
508 | |
509 | // Center point of fillet. |
510 | gp_Pnt2d pc = p12.Translated(L * bisec); |
511 | pc.Coord(xc, yc); |
512 | |
513 | // Shrinking length along segments. |
514 | start = sqrt(L * L - radius * radius); |
515 | end = start; |
516 | |
517 | // Orientation of fillet. |
518 | cw = beta > 0.0; |
519 | return Standard_True; |
520 | } |
521 | |
522 | // A function constructs a fillet between a segment and an arc. |
523 | Standard_Boolean ChFi2d_AnaFilletAlgo::SegmentFilletArc(const Standard_Real radius, |
524 | Standard_Real& xc, Standard_Real& yc, |
525 | Standard_Boolean& cw, |
526 | Standard_Real& start, Standard_Real& end, |
527 | Standard_Real& xend, Standard_Real& yend) |
528 | { |
529 | // Make a line parallel to the segment at the side of center point of fillet. |
530 | // This side may be defined through making a bisectrissa for vectors at p12 (or p21). |
531 | |
532 | // Make 2D points. |
533 | gp_Pnt2d p12(x12, y12); |
534 | gp_Pnt2d p11(x11, y11); |
535 | gp_Pnt2d pc2(xc2, yc2); |
536 | |
537 | // Check length of segment. |
538 | if (p11.SquareDistance(p12) < gp::Resolution()) |
539 | return Standard_False; |
540 | |
541 | // Make 2D vectors. |
542 | gp_Vec2d v1(p12, p11); |
543 | gp_Vec2d v2(p12, pc2); |
544 | |
545 | // Rotate the arc vector to become tangential at p21. |
546 | if (cw2) |
547 | v2.Rotate(+M_PI_2); |
548 | else |
549 | v2.Rotate(-M_PI_2); |
550 | |
551 | // If vectors coincide (segment and arc are tangent), |
552 | // the algorithm doesn't work... |
553 | Standard_Real angle = v1.Angle(v2); |
554 | if (fabs(angle) < Precision::Angular()) |
555 | return Standard_False; |
556 | |
557 | // Make a bissectrisa of vectors at p12. |
558 | v2.Normalize(); |
559 | v1.Normalize(); |
560 | gp_Vec2d bisec = 0.5 * (v1 + v2); |
561 | |
562 | // If segment and arc look in opposite direction, |
563 | // no fillet is possible. |
564 | if (bisec.SquareMagnitude() < gp::Resolution()) |
565 | return Standard_False; |
566 | |
567 | // Define an appropriate point to choose center of fillet. |
568 | bisec.Normalize(); |
569 | gp_Pnt2d nearp = p12.Translated(radius * bisec); |
570 | gp_Lin2d nearl(p12, bisec); |
571 | |
572 | // Make a line parallel to segment and |
573 | // passing near the "near" point. |
574 | gp_Vec2d d1(v1); |
575 | gp_Lin2d line(p11, -d1); |
576 | d1.Rotate(M_PI_2); |
577 | line.Translate(radius * d1); |
578 | if (line.Distance(nearp) > radius) |
579 | line.Translate(-2.0 * radius * d1); |
580 | |
581 | // Make a circle of radius of the arc +/- fillet radius. |
582 | gp_Ax2d axes(pc2, gp::DX2d()); |
583 | gp_Circ2d circ(axes, radius2 + radius); |
584 | if (radius2 > radius && circ.Distance(nearp) > radius) |
585 | circ.SetRadius(radius2 - radius); |
586 | |
587 | // Calculate intersection of the line and the circle. |
588 | IntAna2d_AnaIntersection intersector(line, circ); |
589 | if (!intersector.IsDone() || !intersector.NbPoints()) |
590 | return Standard_False; |
591 | |
592 | // Find center point of fillet. |
593 | Standard_Integer i; |
594 | Standard_Real minDist = DBL_MAX; |
595 | for (i = 1; i <= intersector.NbPoints(); ++i) |
596 | { |
597 | const IntAna2d_IntPoint& intp = intersector.Point(i); |
598 | const gp_Pnt2d& p = intp.Value(); |
599 | |
600 | Standard_Real d = nearl.Distance(p); |
601 | if (d < minDist) |
602 | { |
603 | minDist = d; |
604 | p.Coord(xc, yc); |
605 | } |
606 | } |
607 | |
608 | // Shrink of segment. |
609 | gp_Pnt2d pc(xc, yc); |
610 | Standard_Real L2 = pc.SquareDistance(p12); |
611 | const Standard_Real Rf2 = radius * radius; |
612 | start = sqrt(L2 - Rf2); |
613 | |
614 | // Shrink of arc. |
615 | gp_Vec2d pcc(pc2, pc); |
616 | end = fabs(gp_Vec2d(pc2, p12).Angle(pcc)); |
617 | |
618 | // Duplicate the information on shrink the arc: |
619 | // calculate a point on the arc coinciding with the end of fillet. |
620 | line.SetLocation(pc2); |
621 | line.SetDirection(pcc); |
622 | circ.SetLocation(pc2); |
623 | circ.SetRadius(radius2); |
624 | intersector.Perform(line, circ); |
625 | if (!intersector.IsDone() || !intersector.NbPoints()) |
626 | return Standard_False; |
627 | |
628 | xend = DBL_MAX; |
629 | yend = DBL_MAX; |
630 | for (i = 1; i <= intersector.NbPoints(); ++i) |
631 | { |
632 | const IntAna2d_IntPoint& intp = intersector.Point(i); |
633 | const gp_Pnt2d& p = intp.Value(); |
634 | |
635 | const Standard_Real d2 = p.SquareDistance(pc); |
636 | if (fabs(d2 - Rf2) < Precision::Confusion()) |
637 | { |
638 | p.Coord(xend, yend); |
639 | break; |
640 | } |
641 | } |
642 | |
643 | // Orientation of the fillet. |
644 | angle = v1.Angle(v2); |
645 | cw = angle > 0.0; |
646 | return Standard_True; |
647 | } |
648 | |
649 | // A function constructs a fillet between an arc and a segment. |
650 | Standard_Boolean ChFi2d_AnaFilletAlgo::ArcFilletSegment(const Standard_Real radius, |
651 | Standard_Real& xc, Standard_Real& yc, |
652 | Standard_Boolean& cw, |
653 | Standard_Real& start, Standard_Real& end, |
654 | Standard_Real& xstart, Standard_Real& ystart) |
655 | { |
656 | // Make a line parallel to the segment at the side of center point of fillet. |
657 | // This side may be defined through making a bisectrissa for vectors at p12 (or p21). |
658 | |
659 | // Make 2D points. |
660 | gp_Pnt2d p12(x12, y12); |
661 | gp_Pnt2d p22(x22, y22); |
662 | gp_Pnt2d pc1(xc1, yc1); |
663 | |
664 | // Check length of segment. |
665 | if (p12.SquareDistance(p22) < gp::Resolution()) |
666 | return Standard_False; |
667 | |
668 | // Make 2D vectors. |
669 | gp_Vec2d v1(p12, pc1); |
670 | gp_Vec2d v2(p12, p22); |
671 | |
672 | // Rotate the arc vector to become tangential at p21. |
673 | if (cw1) |
674 | v1.Rotate(-M_PI_2); |
675 | else |
676 | v1.Rotate(+M_PI_2); |
677 | |
678 | // If vectors coincide (segment and arc are tangent), |
679 | // the algorithm doesn't work... |
680 | Standard_Real angle = v1.Angle(v2); |
681 | if (fabs(angle) < Precision::Angular()) |
682 | return Standard_False; |
683 | |
684 | // Make a bisectrissa of vectors at p12. |
685 | v1.Normalize(); |
686 | v2.Normalize(); |
687 | gp_Vec2d bisec = 0.5 * (v1 + v2); |
688 | |
689 | // If segment and arc look in opposite direction, |
690 | // no fillet is possible. |
691 | if (bisec.SquareMagnitude() < gp::Resolution()) |
692 | return Standard_False; |
693 | |
694 | // Define an appropriate point to choose center of fillet. |
695 | bisec.Normalize(); |
696 | gp_Pnt2d nearPoint = p12.Translated(radius * bisec); |
697 | gp_Lin2d nearLine(p12, bisec); |
698 | |
699 | // Make a line parallel to segment and |
700 | // passing near the "near" point. |
701 | gp_Vec2d d2(v2); |
702 | gp_Lin2d line(p22, -d2); |
703 | d2.Rotate(M_PI_2); |
704 | line.Translate(radius * d2); |
705 | if (line.Distance(nearPoint) > radius) |
706 | line.Translate(-2.0 * radius * d2); |
707 | |
708 | // Make a circle of radius of the arc +/- fillet radius. |
709 | gp_Ax2d axes(pc1, gp::DX2d()); |
710 | gp_Circ2d circ(axes, radius1 + radius); |
711 | if (radius1 > radius && circ.Distance(nearPoint) > radius) |
712 | circ.SetRadius(radius1 - radius); |
713 | |
714 | // Calculate intersection of the line and the big circle. |
715 | IntAna2d_AnaIntersection intersector(line, circ); |
716 | if (!intersector.IsDone() || !intersector.NbPoints()) |
717 | return Standard_False; |
718 | |
719 | // Find center point of fillet. |
720 | Standard_Integer i; |
721 | Standard_Real minDist = DBL_MAX; |
722 | for (i = 1; i <= intersector.NbPoints(); ++i) |
723 | { |
724 | const IntAna2d_IntPoint& intp = intersector.Point(i); |
725 | const gp_Pnt2d& p = intp.Value(); |
726 | |
727 | Standard_Real d = nearLine.Distance(p); |
728 | if (d < minDist) |
729 | { |
730 | minDist = d; |
731 | p.Coord(xc, yc); |
732 | } |
733 | } |
734 | |
735 | // Shrink of segment. |
736 | gp_Pnt2d pc(xc, yc); |
737 | Standard_Real L2 = pc.SquareDistance(p12); |
738 | const Standard_Real Rf2 = radius * radius; |
739 | end = sqrt(L2 - Rf2); |
740 | |
741 | // Shrink of arc. |
742 | gp_Vec2d pcc(pc1, pc); |
743 | start = fabs(gp_Vec2d(pc1, p12).Angle(pcc)); |
744 | |
745 | // Duplicate the information on shrink the arc: |
746 | // calculate a point on the arc coinciding with the start of fillet. |
747 | line.SetLocation(pc1); |
748 | line.SetDirection(pcc); |
749 | circ.SetLocation(pc1); |
750 | circ.SetRadius(radius1); |
751 | intersector.Perform(line, circ); |
752 | if (!intersector.IsDone() || !intersector.NbPoints()) |
753 | return Standard_False; |
754 | |
755 | xstart = DBL_MAX; |
756 | ystart = DBL_MAX; |
757 | for (i = 1; i <= intersector.NbPoints(); ++i) |
758 | { |
759 | const IntAna2d_IntPoint& intp = intersector.Point(i); |
760 | const gp_Pnt2d& p = intp.Value(); |
761 | |
762 | const Standard_Real d2 = p.SquareDistance(pc); |
763 | if (fabs(d2 - Rf2) < Precision::SquareConfusion()) |
764 | { |
765 | p.Coord(xstart, ystart); |
766 | break; |
767 | } |
768 | } |
769 | |
770 | // Orientation of the fillet. |
771 | angle = v2.Angle(v1); |
772 | cw = angle < 0.0; |
773 | return Standard_True; |
774 | } |
775 | |
776 | // WW5 method to compute fillet: arc - arc. |
777 | // It returns a constructed fillet definition: |
778 | // center point (xc, yc) |
779 | // shrinking parameter of the 1st circle (start) |
780 | // shrinking parameter of the 2nd circle (end) |
781 | // if the arc of fillet clockwise (cw = true) or counterclockwise (cw = false). |
782 | Standard_Boolean ChFi2d_AnaFilletAlgo::ArcFilletArc(const Standard_Real radius, |
783 | Standard_Real& xc, Standard_Real& yc, |
784 | Standard_Boolean& cw, |
785 | Standard_Real& start, Standard_Real& end) |
786 | { |
787 | // Make points. |
788 | const gp_Pnt2d pc1(xc1, yc1); |
789 | const gp_Pnt2d pc2(xc2, yc2); |
790 | const gp_Pnt2d p12(x12, y12); |
791 | |
792 | // Make vectors at p12. |
793 | gp_Vec2d v1(pc1, p12); |
794 | gp_Vec2d v2(pc2, p12); |
795 | |
796 | // Rotate the vectors so that they are tangent to circles at p12. |
797 | if (cw1) |
798 | v1.Rotate(+M_PI_2); |
799 | else |
800 | v1.Rotate(-M_PI_2); |
801 | if (cw2) |
802 | v2.Rotate(-M_PI_2); |
803 | else |
804 | v2.Rotate(+M_PI_2); |
805 | |
806 | // Make a "check" point for choosing an offset circle. |
807 | v1.Normalize(); |
808 | v2.Normalize(); |
809 | gp_Vec2d bisec = 0.5 * (v1 + v2); |
810 | if (bisec.SquareMagnitude() < gp::Resolution()) |
811 | return Standard_False; |
812 | |
813 | const gp_Pnt2d checkp = p12.Translated(radius * bisec); |
814 | const gp_Lin2d checkl(p12, bisec); |
815 | |
816 | // Make two circles of radius r1 +/- r and r2 +/- r |
817 | // with center point equal to pc1 and pc2. |
818 | // Arc 1. |
819 | gp_Ax2d axes(pc1, gp::DX2d()); |
820 | gp_Circ2d c1(axes, radius1 + radius); |
821 | if (radius1 > radius && c1.Distance(checkp) > radius) |
822 | c1.SetRadius(radius1 - radius); |
823 | // Arc 2. |
824 | axes.SetLocation(pc2); |
825 | gp_Circ2d c2(axes, radius2 + radius); |
826 | if (radius2 > radius && c2.Distance(checkp) > radius) |
827 | c2.SetRadius(radius2 - radius); |
828 | |
829 | // Calculate an intersection point of these two circles |
830 | // and choose the one closer to the "check" point. |
831 | IntAna2d_AnaIntersection intersector(c1, c2); |
832 | if (!intersector.IsDone() || !intersector.NbPoints()) |
833 | return Standard_False; |
834 | |
835 | // Find center point of fillet. |
836 | gp_Pnt2d pc; |
837 | Standard_Real minDist = DBL_MAX; |
838 | for (int i = 1; i <= intersector.NbPoints(); ++i) |
839 | { |
840 | const IntAna2d_IntPoint& intp = intersector.Point(i); |
841 | const gp_Pnt2d& p = intp.Value(); |
842 | |
843 | Standard_Real d = checkp.SquareDistance(p); |
844 | if (d < minDist) |
845 | { |
846 | minDist = d; |
847 | pc = p; |
848 | } |
849 | } |
850 | pc.Coord(xc, yc); |
851 | |
852 | // Orientation of fillet. |
853 | Standard_Real angle = v1.Angle(v2); |
854 | if (fabs(angle) < Precision::Angular()) |
855 | { |
856 | angle = gp_Vec2d(pc, pc1).Angle(gp_Vec2d(pc, pc2)); |
857 | cw = angle < 0.0; |
858 | } |
859 | else |
860 | { |
861 | cw = angle > 0.0; |
862 | } |
863 | |
864 | // Shrinking of circles. |
865 | start = fabs(gp_Vec2d(pc1, p12).Angle(gp_Vec2d(pc1, pc))); |
866 | end = fabs(gp_Vec2d(pc2, p12).Angle(gp_Vec2d(pc2, pc))); |
867 | return Standard_True; |
868 | } |
869 | |
870 | // Cuts intersecting edges of a contour. |
871 | Standard_Boolean ChFi2d_AnaFilletAlgo::Cut(const gp_Pln& plane, TopoDS_Edge& e1, TopoDS_Edge& e2) |
872 | { |
873 | gp_Pnt p; |
874 | Standard_Boolean found(Standard_False); |
875 | Standard_Real param1 = 0.0, param2 = 0.0; |
876 | Standard_Real f1, l1, f2, l2; |
877 | Handle(Geom_Curve) c1 = BRep_Tool::Curve(e1, f1, l1); |
878 | Handle(Geom_Curve) c2 = BRep_Tool::Curve(e2, f2, l2); |
879 | GeomAPI_ExtremaCurveCurve extrema(c1, c2, f1, l1, f2, l2); |
880 | if (extrema.NbExtrema()) |
881 | { |
882 | Standard_Integer i, nb = extrema.NbExtrema(); |
883 | for (i = 1; i <= nb; ++i) |
884 | { |
885 | const Standard_Real d = extrema.Distance(i); |
886 | if (d < Precision::Confusion()) |
887 | { |
888 | extrema.Parameters(i, param1, param2); |
889 | if (fabs(l1 - param1) > Precision::Confusion() && |
890 | fabs(f2 - param2) > Precision::Confusion()) |
891 | { |
892 | found = Standard_True; |
893 | extrema.Points(i, p, p); |
894 | break; |
895 | } |
896 | } |
897 | } |
898 | } |
899 | |
900 | if (found) |
901 | { |
902 | BRepBuilderAPI_MakeEdge mkEdge1(c1, f1, param1); |
903 | if (mkEdge1.IsDone()) |
904 | { |
905 | e1 = mkEdge1.Edge(); |
906 | |
907 | BRepBuilderAPI_MakeEdge mkEdge2(c2, param2, l2); |
908 | if (mkEdge2.IsDone()) |
909 | { |
910 | e2 = mkEdge2.Edge(); |
911 | |
912 | gp_Pnt2d p2d = ProjLib::Project(plane, p); |
913 | p2d.Coord(x12, y12); |
914 | x21 = x12; |
915 | y21 = y12; |
916 | return Standard_True; |
917 | } |
918 | } |
919 | } |
920 | return Standard_False; |
921 | } |