| 1 | // Created on: 1997-02-10 |
| 2 | // Created by: Jacques GOUSSARD |
| 3 | // Copyright (c) 1997-1999 Matra Datavision |
| 4 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
| 5 | // |
| 6 | // This file is part of Open CASCADE Technology software library. |
| 7 | // |
| 8 | // This library is free software; you can redistribute it and / or modify it |
| 9 | // under the terms of the GNU Lesser General Public version 2.1 as published |
| 10 | // by the Free Software Foundation, with special exception defined in the file |
| 11 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
| 12 | // distribution for complete text of the license and disclaimer of any warranty. |
| 13 | // |
| 14 | // Alternatively, this file may be used under the terms of Open CASCADE |
| 15 | // commercial license or contractual agreement. |
| 16 | |
| 17 | #include <BRepBlend_RstRstConstRad.ixx> |
| 18 | #include <math_Gauss.hxx> |
| 19 | #include <math_SVD.hxx> |
| 20 | |
| 21 | #include <ElCLib.hxx> |
| 22 | #include <gp.hxx> |
| 23 | #include <BlendFunc.hxx> |
| 24 | #include <GeomFill.hxx> |
| 25 | #include <Standard_DomainError.hxx> |
| 26 | #include <Standard_NotImplemented.hxx> |
| 27 | #include <Precision.hxx> |
| 28 | |
| 29 | #define Eps 1.e-15 |
| 30 | |
| 31 | static void t3dto2d(Standard_Real& a, |
| 32 | Standard_Real& b, |
| 33 | const gp_Vec& A, |
| 34 | const gp_Vec& B, |
| 35 | const gp_Vec& C) |
| 36 | { |
| 37 | Standard_Real AB = A.Dot(B); |
| 38 | Standard_Real AC = A.Dot(C); |
| 39 | Standard_Real BC = B.Dot(C); |
| 40 | Standard_Real BB = B.Dot(B); |
| 41 | Standard_Real CC = C.Dot(C); |
| 42 | Standard_Real deno = (BB*CC-BC*BC); |
| 43 | a = (AB*CC-AC*BC)/deno; |
| 44 | b = (AC*BB-AB*BC)/deno; |
| 45 | } |
| 46 | |
| 47 | //======================================================================= |
| 48 | //function : BRepBlend_RstRstConstRad |
| 49 | //purpose : |
| 50 | //======================================================================= |
| 51 | |
| 52 | BRepBlend_RstRstConstRad::BRepBlend_RstRstConstRad |
| 53 | (const Handle(Adaptor3d_HSurface)& Surf1, |
| 54 | const Handle(Adaptor2d_HCurve2d)& Rst1, |
| 55 | const Handle(Adaptor3d_HSurface)& Surf2, |
| 56 | const Handle(Adaptor2d_HCurve2d)& Rst2, |
| 57 | const Handle(Adaptor3d_HCurve)& CGuide): |
| 58 | surf1(Surf1), surf2(Surf2), rst1(Rst1), rst2(Rst2), |
| 59 | cons1(Rst1, Surf1), cons2(Rst2, Surf2), |
| 60 | guide(CGuide), tguide(CGuide), |
| 61 | istangent(Standard_True), maxang(RealFirst()), minang(RealLast()), |
| 62 | distmin(RealLast()), |
| 63 | mySShape(BlendFunc_Rational) |
| 64 | {} |
| 65 | |
| 66 | //======================================================================= |
| 67 | //function : NbVariables |
| 68 | //purpose : |
| 69 | //======================================================================= |
| 70 | |
| 71 | Standard_Integer BRepBlend_RstRstConstRad::NbVariables() const |
| 72 | { |
| 73 | return 2; |
| 74 | } |
| 75 | |
| 76 | //======================================================================= |
| 77 | //function : NbEquations |
| 78 | //purpose : |
| 79 | //======================================================================= |
| 80 | |
| 81 | Standard_Integer BRepBlend_RstRstConstRad::NbEquations() const |
| 82 | { |
| 83 | return 2; |
| 84 | } |
| 85 | |
| 86 | //======================================================================= |
| 87 | //function : Value |
| 88 | //purpose : |
| 89 | //======================================================================= |
| 90 | |
| 91 | Standard_Boolean BRepBlend_RstRstConstRad::Value(const math_Vector& X, |
| 92 | math_Vector& F) |
| 93 | { |
| 94 | ptrst1 = cons1.Value(X(1)); |
| 95 | ptrst2 = cons2.Value(X(2)); |
| 96 | |
| 97 | F(1) = nplan.XYZ().Dot(ptrst1.XYZ()) + theD; |
| 98 | F(2) = nplan.XYZ().Dot(ptrst2.XYZ()) + theD; |
| 99 | |
| 100 | return Standard_True; |
| 101 | } |
| 102 | |
| 103 | //======================================================================= |
| 104 | //function : Derivatives |
| 105 | //purpose : |
| 106 | //======================================================================= |
| 107 | |
| 108 | Standard_Boolean BRepBlend_RstRstConstRad::Derivatives(const math_Vector& X, |
| 109 | math_Matrix& D) |
| 110 | { |
| 111 | gp_Vec d11, d21; |
| 112 | |
| 113 | cons1.D1(X(1), ptrst1, d11); |
| 114 | cons2.D1(X(2), ptrst2, d21); |
| 115 | |
| 116 | D(1,1) = nplan.Dot(d11); |
| 117 | D(1,2) = 0.; |
| 118 | |
| 119 | D(2,1) = 0.; |
| 120 | D(2,2) = nplan.Dot(d21); |
| 121 | |
| 122 | return Standard_True; |
| 123 | } |
| 124 | |
| 125 | //======================================================================= |
| 126 | //function : Values |
| 127 | //purpose : |
| 128 | //======================================================================= |
| 129 | |
| 130 | Standard_Boolean BRepBlend_RstRstConstRad::Values(const math_Vector& X, |
| 131 | math_Vector& F, |
| 132 | math_Matrix& D) |
| 133 | { |
| 134 | Standard_Boolean Error; |
| 135 | |
| 136 | Error = Value(X, F); |
| 137 | Error = Derivatives(X, D); |
| 138 | |
| 139 | return Standard_True; |
| 140 | } |
| 141 | |
| 142 | //======================================================================= |
| 143 | //function : Set |
| 144 | //purpose : |
| 145 | //======================================================================= |
| 146 | |
| 147 | void BRepBlend_RstRstConstRad::Set(const Handle(Adaptor3d_HSurface)& SurfRef1, |
| 148 | const Handle(Adaptor2d_HCurve2d)& RstRef1, |
| 149 | const Handle(Adaptor3d_HSurface)& SurfRef2, |
| 150 | const Handle(Adaptor2d_HCurve2d)& RstRef2) |
| 151 | { |
| 152 | surfref1 = SurfRef1; |
| 153 | surfref2 = SurfRef2; |
| 154 | rstref1 = RstRef1; |
| 155 | rstref2 = RstRef2; |
| 156 | } |
| 157 | |
| 158 | //======================================================================= |
| 159 | //function : Set |
| 160 | //purpose : |
| 161 | //======================================================================= |
| 162 | |
| 163 | void BRepBlend_RstRstConstRad::Set(const Standard_Real Param) |
| 164 | { |
| 165 | tguide->D2(Param, ptgui, d1gui, d2gui); |
| 166 | normtg = d1gui.Magnitude(); |
| 167 | nplan = d1gui.Normalized(); |
| 168 | theD = - (nplan.XYZ().Dot(ptgui.XYZ())); |
| 169 | } |
| 170 | |
| 171 | //======================================================================= |
| 172 | //function : Set |
| 173 | //purpose : |
| 174 | //======================================================================= |
| 175 | |
| 176 | void BRepBlend_RstRstConstRad::Set(const Standard_Real First, |
| 177 | const Standard_Real Last) |
| 178 | { |
| 179 | tguide = guide->Trim(First, Last, 1.e-12); |
| 180 | } |
| 181 | |
| 182 | //======================================================================= |
| 183 | //function : GetTolerance |
| 184 | //purpose : |
| 185 | //======================================================================= |
| 186 | |
| 187 | void BRepBlend_RstRstConstRad::GetTolerance(math_Vector& Tolerance, |
| 188 | const Standard_Real Tol) const |
| 189 | { |
| 190 | Tolerance(1) = cons1.Resolution(Tol); |
| 191 | Tolerance(2) = cons2.Resolution(Tol); |
| 192 | } |
| 193 | |
| 194 | //======================================================================= |
| 195 | //function : GetBounds |
| 196 | //purpose : |
| 197 | //======================================================================= |
| 198 | |
| 199 | void BRepBlend_RstRstConstRad::GetBounds(math_Vector& InfBound, |
| 200 | math_Vector& SupBound) const |
| 201 | { |
| 202 | InfBound(1) = cons1.FirstParameter(); |
| 203 | InfBound(2) = cons2.FirstParameter(); |
| 204 | SupBound(1) = cons1.LastParameter(); |
| 205 | SupBound(2) = cons2.LastParameter(); |
| 206 | |
| 207 | } |
| 208 | |
| 209 | //======================================================================= |
| 210 | //function : IsSolution |
| 211 | //purpose : |
| 212 | //======================================================================= |
| 213 | |
| 214 | Standard_Boolean BRepBlend_RstRstConstRad::IsSolution(const math_Vector& Sol, |
| 215 | const Standard_Real Tol) |
| 216 | |
| 217 | |
| 218 | { |
| 219 | math_Vector valsol(1, 2), secmember(1, 2); |
| 220 | math_Matrix gradsol(1, 2, 1, 2); |
| 221 | |
| 222 | gp_Vec dnplan, d1urst1, d1vrst1, d1urst2, d1vrst2, d11, d21, temp; |
| 223 | gp_Pnt bid; |
| 224 | |
| 225 | Standard_Real Cosa, Sina, Angle; |
| 226 | |
| 227 | Values(Sol, valsol, gradsol); |
| 228 | |
| 229 | if (Abs(valsol(1)) <= Tol && |
| 230 | Abs(valsol(2)) <= Tol ) { |
| 231 | |
| 232 | // Calculation of tangents |
| 233 | prmrst1 = Sol(1); |
| 234 | pt2drst1 = rst1->Value(prmrst1); |
| 235 | prmrst2 = Sol(2); |
| 236 | pt2drst2 = rst2->Value(prmrst2); |
| 237 | |
| 238 | cons1.D1(Sol(1), ptrst1, d11); |
| 239 | cons2.D1(Sol(2), ptrst2, d21); |
| 240 | |
| 241 | dnplan.SetLinearForm(1./normtg, d2gui, |
| 242 | -1./normtg * (nplan.Dot(d2gui)), nplan); |
| 243 | |
| 244 | temp.SetXYZ(ptrst1.XYZ() - ptgui.XYZ()); |
| 245 | secmember(1) = normtg - dnplan.Dot(temp); |
| 246 | |
| 247 | temp.SetXYZ(ptrst2.XYZ() - ptgui.XYZ()); |
| 248 | secmember(2) = normtg - dnplan.Dot(temp); |
| 249 | |
| 250 | math_Gauss Resol(gradsol); |
| 251 | |
| 252 | if (Resol.IsDone()) { |
| 253 | Resol.Solve(secmember); |
| 254 | istangent = Standard_False; |
| 255 | } |
| 256 | else { |
| 257 | math_SVD SingRS (gradsol); |
| 258 | if (SingRS.IsDone()) { |
| 259 | math_Vector DEDT(1,3); |
| 260 | DEDT = secmember; |
| 261 | SingRS.Solve(DEDT, secmember, 1.e-6); |
| 262 | istangent = Standard_False; |
| 263 | } |
| 264 | else istangent = Standard_True; |
| 265 | } |
| 266 | |
| 267 | |
| 268 | if (!istangent) { |
| 269 | tgrst1 = secmember(1) * d11; |
| 270 | tgrst2 = secmember(2) * d21; |
| 271 | |
| 272 | Standard_Real a, b; |
| 273 | surf1->D1(pt2drst1.X(), pt2drst1.Y(), bid, d1urst1, d1vrst1); |
| 274 | t3dto2d(a, b, tgrst1, d1urst1, d1vrst1); |
| 275 | tg2drst1.SetCoord(a, b); |
| 276 | surf2->D1(pt2drst2.X(), pt2drst2.Y(), bid, d1urst2, d1vrst2); |
| 277 | t3dto2d(a, b, tgrst1, d1urst2, d1vrst2); |
| 278 | tg2drst2.SetCoord(a, b); |
| 279 | } |
| 280 | |
| 281 | gp_Pnt Center; |
| 282 | gp_Vec NotUsed; |
| 283 | Standard_Boolean IsCenter; |
| 284 | |
| 285 | IsCenter = CenterCircleRst1Rst2(ptrst1, ptrst2, nplan, Center, NotUsed); |
| 286 | |
| 287 | if (!IsCenter) return Standard_False; |
| 288 | |
| 289 | gp_Vec n1(Center, ptrst1) , n2(Center, ptrst2); |
| 290 | |
| 291 | n1.Normalize(); |
| 292 | n2.Normalize(); |
| 293 | |
| 294 | Cosa = n1.Dot(n2); |
| 295 | Sina = nplan.Dot(n1.Crossed(n2)); |
| 296 | |
| 297 | if (choix%2 != 0) { |
| 298 | Sina = -Sina; //nplan is changed into -nplan |
| 299 | } |
| 300 | |
| 301 | Angle = ACos(Cosa); |
| 302 | if (Sina < 0.) { |
| 303 | Angle = 2.*M_PI - Angle; |
| 304 | } |
| 305 | |
| 306 | if (Angle > maxang) {maxang = Angle;} |
| 307 | if (Angle < minang) {minang = Angle;} |
| 308 | distmin = Min( distmin, ptrst1.Distance(ptrst2)); |
| 309 | |
| 310 | return Standard_True; |
| 311 | } |
| 312 | istangent = Standard_True; |
| 313 | return Standard_False; |
| 314 | } |
| 315 | |
| 316 | //======================================================================= |
| 317 | //function : GetMinimalDistance |
| 318 | //purpose : |
| 319 | //======================================================================= |
| 320 | |
| 321 | Standard_Real BRepBlend_RstRstConstRad::GetMinimalDistance() const |
| 322 | { |
| 323 | return distmin; |
| 324 | } |
| 325 | |
| 326 | //======================================================================= |
| 327 | //function : PointOnRst1 |
| 328 | //purpose : |
| 329 | //======================================================================= |
| 330 | |
| 331 | const gp_Pnt& BRepBlend_RstRstConstRad::PointOnRst1() const |
| 332 | { |
| 333 | return ptrst1; |
| 334 | } |
| 335 | |
| 336 | //======================================================================= |
| 337 | //function : PointOnRst2 |
| 338 | //purpose : |
| 339 | //======================================================================= |
| 340 | |
| 341 | const gp_Pnt& BRepBlend_RstRstConstRad::PointOnRst2() const |
| 342 | { |
| 343 | return ptrst2; |
| 344 | } |
| 345 | |
| 346 | //======================================================================= |
| 347 | //function : Pnt2dOnRst1 |
| 348 | //purpose : |
| 349 | //======================================================================= |
| 350 | |
| 351 | const gp_Pnt2d& BRepBlend_RstRstConstRad::Pnt2dOnRst1() const |
| 352 | { |
| 353 | return pt2drst1; |
| 354 | } |
| 355 | |
| 356 | //======================================================================= |
| 357 | //function : Pnt2dOnRst2 |
| 358 | //purpose : |
| 359 | //======================================================================= |
| 360 | |
| 361 | const gp_Pnt2d& BRepBlend_RstRstConstRad::Pnt2dOnRst2() const |
| 362 | { |
| 363 | return pt2drst2; |
| 364 | } |
| 365 | |
| 366 | //======================================================================= |
| 367 | //function : ParameterOnRst1 |
| 368 | //purpose : |
| 369 | //======================================================================= |
| 370 | |
| 371 | Standard_Real BRepBlend_RstRstConstRad::ParameterOnRst1() const |
| 372 | { |
| 373 | return prmrst1; |
| 374 | } |
| 375 | |
| 376 | //======================================================================= |
| 377 | //function : ParameterOnRst2 |
| 378 | //purpose : |
| 379 | //======================================================================= |
| 380 | |
| 381 | Standard_Real BRepBlend_RstRstConstRad::ParameterOnRst2() const |
| 382 | { |
| 383 | return prmrst2; |
| 384 | } |
| 385 | //======================================================================= |
| 386 | //function : IsTangencyPoint |
| 387 | //purpose : |
| 388 | //======================================================================= |
| 389 | |
| 390 | Standard_Boolean BRepBlend_RstRstConstRad::IsTangencyPoint() const |
| 391 | { |
| 392 | return istangent; |
| 393 | } |
| 394 | |
| 395 | //======================================================================= |
| 396 | //function : TangentOnRst1 |
| 397 | //purpose : |
| 398 | //======================================================================= |
| 399 | |
| 400 | const gp_Vec& BRepBlend_RstRstConstRad::TangentOnRst1() const |
| 401 | { |
| 402 | if (istangent) {Standard_DomainError::Raise();} |
| 403 | return tgrst1; |
| 404 | } |
| 405 | |
| 406 | //======================================================================= |
| 407 | //function : Tangent2dOnRst1 |
| 408 | //purpose : |
| 409 | //======================================================================= |
| 410 | |
| 411 | const gp_Vec2d& BRepBlend_RstRstConstRad::Tangent2dOnRst1() const |
| 412 | { |
| 413 | if (istangent) {Standard_DomainError::Raise();} |
| 414 | return tg2drst1; |
| 415 | } |
| 416 | |
| 417 | //======================================================================= |
| 418 | //function : TangentOnRst2 |
| 419 | //purpose : |
| 420 | //======================================================================= |
| 421 | |
| 422 | const gp_Vec& BRepBlend_RstRstConstRad::TangentOnRst2() const |
| 423 | { |
| 424 | if (istangent) {Standard_DomainError::Raise();} |
| 425 | return tgrst2; |
| 426 | } |
| 427 | |
| 428 | //======================================================================= |
| 429 | //function : Tangent2dOnRst2 |
| 430 | //purpose : |
| 431 | //======================================================================= |
| 432 | |
| 433 | const gp_Vec2d& BRepBlend_RstRstConstRad::Tangent2dOnRst2() const |
| 434 | { |
| 435 | if (istangent) {Standard_DomainError::Raise();} |
| 436 | return tg2drst2; |
| 437 | } |
| 438 | |
| 439 | //======================================================================= |
| 440 | //function : Decroch |
| 441 | //purpose : |
| 442 | //======================================================================= |
| 443 | |
| 444 | Blend_DecrochStatus BRepBlend_RstRstConstRad::Decroch(const math_Vector& Sol, |
| 445 | gp_Vec& NRst1, |
| 446 | gp_Vec& TgRst1, |
| 447 | gp_Vec& NRst2, |
| 448 | gp_Vec& TgRst2)const |
| 449 | { |
| 450 | gp_Vec NRst1InPlane, NRst2InPlane; |
| 451 | gp_Pnt PtTmp1, PtTmp2, Center; |
| 452 | gp_Vec d1u, d1v, centptrst, NotUsed; |
| 453 | Standard_Real norm, unsurnorm; |
| 454 | Standard_Real u,v; |
| 455 | |
| 456 | rstref1->Value(Sol(1)).Coord(u, v); |
| 457 | surfref1->D1(u, v,PtTmp1,d1u,d1v); |
| 458 | // Normal to the reference surface 1 |
| 459 | NRst1 = d1u.Crossed(d1v); |
| 460 | rstref2->Value(Sol(2)).Coord(u, v); |
| 461 | surfref2->D1(u, v, PtTmp2, d1u, d1v); |
| 462 | // Normal to the reference surface 2 |
| 463 | NRst2 = d1u.Crossed(d1v); |
| 464 | |
| 465 | Standard_Boolean IsCenter; |
| 466 | |
| 467 | IsCenter = CenterCircleRst1Rst2(PtTmp1, PtTmp2, nplan, Center, NotUsed); |
| 468 | |
| 469 | norm = nplan.Crossed(NRst1).Magnitude(); |
| 470 | unsurnorm = 1. / norm; |
| 471 | |
| 472 | NRst1InPlane.SetLinearForm(nplan.Dot(NRst1) * unsurnorm, nplan, -unsurnorm, NRst1); |
| 473 | |
| 474 | centptrst.SetXYZ(PtTmp1.XYZ() - Center.XYZ()); |
| 475 | |
| 476 | if (centptrst.Dot(NRst1InPlane) < 0.) NRst1InPlane.Reverse(); |
| 477 | |
| 478 | TgRst1 = nplan.Crossed(centptrst); |
| 479 | |
| 480 | norm = nplan.Crossed(NRst2).Magnitude(); |
| 481 | unsurnorm = 1./ norm; |
| 482 | NRst2InPlane.SetLinearForm(nplan.Dot(NRst2) * unsurnorm, nplan, -unsurnorm, NRst2); |
| 483 | centptrst.SetXYZ(PtTmp2.XYZ() - Center.XYZ()); |
| 484 | |
| 485 | |
| 486 | if (centptrst.Dot(NRst2InPlane) < 0.) NRst2InPlane.Reverse(); |
| 487 | |
| 488 | TgRst2 = nplan.Crossed(centptrst); |
| 489 | |
| 490 | if (choix %2 != 0) { |
| 491 | TgRst1.Reverse(); |
| 492 | TgRst2.Reverse(); |
| 493 | } |
| 494 | |
| 495 | // The vectors are returned |
| 496 | if (NRst1InPlane.Dot(TgRst1) > -1.e-10) { |
| 497 | if (NRst2InPlane.Dot(TgRst2) < 1.e-10) { |
| 498 | return Blend_DecrochBoth; |
| 499 | } |
| 500 | else { |
| 501 | return Blend_DecrochRst1; |
| 502 | } |
| 503 | } |
| 504 | else { |
| 505 | if (NRst2InPlane.Dot(TgRst2) < 1.e-10) { |
| 506 | return Blend_DecrochRst2; |
| 507 | } |
| 508 | else { |
| 509 | return Blend_NoDecroch; |
| 510 | } |
| 511 | } |
| 512 | |
| 513 | } |
| 514 | |
| 515 | //======================================================================= |
| 516 | //function : Set |
| 517 | //purpose : |
| 518 | //======================================================================= |
| 519 | |
| 520 | void BRepBlend_RstRstConstRad::Set(const Standard_Real Radius, |
| 521 | const Standard_Integer Choix) |
| 522 | { |
| 523 | choix = Choix; |
| 524 | ray = Abs(Radius); |
| 525 | |
| 526 | } |
| 527 | |
| 528 | //======================================================================= |
| 529 | //function : Set |
| 530 | //purpose : |
| 531 | //======================================================================= |
| 532 | |
| 533 | void BRepBlend_RstRstConstRad::Set(const BlendFunc_SectionShape TypeSection) |
| 534 | { |
| 535 | mySShape = TypeSection; |
| 536 | } |
| 537 | |
| 538 | |
| 539 | |
| 540 | //======================================================================= |
| 541 | //function : CenterCircleRst1Rst2 |
| 542 | //purpose : Calculate the center of the circle passing by two points of restrictions |
| 543 | //======================================================================= |
| 544 | Standard_Boolean BRepBlend_RstRstConstRad::CenterCircleRst1Rst2(const gp_Pnt& PtRst1, |
| 545 | const gp_Pnt& PtRst2, |
| 546 | const gp_Vec& np, |
| 547 | gp_Pnt& Center, |
| 548 | gp_Vec& VdMed) const |
| 549 | { |
| 550 | |
| 551 | gp_Vec rst1rst2(PtRst1, PtRst2); |
| 552 | gp_Vec vdmedNor; //,NRst1; vdmedNor directong vector of the perpendicular bisector |
| 553 | Standard_Real norm2; |
| 554 | Standard_Real Dist;// distance between the middle of PtRst1,PtRst2 and Center |
| 555 | |
| 556 | // Calculate the center of the circle |
| 557 | VdMed = rst1rst2.Crossed(np); |
| 558 | norm2 = rst1rst2.SquareMagnitude(); |
| 559 | Dist = ray * ray - 0.25 * norm2; |
| 560 | |
| 561 | if (choix > 2) { |
| 562 | VdMed.Reverse(); |
| 563 | } |
| 564 | |
| 565 | if (Dist < - 1.E-07) return Standard_False; |
| 566 | |
| 567 | if (Dist > 1.E-07) { |
| 568 | Dist = sqrt(Dist); |
| 569 | vdmedNor = VdMed.Normalized(); |
| 570 | Center.SetXYZ(0.5 * rst1rst2.XYZ() + PtRst1.XYZ() + Dist * vdmedNor.XYZ()); |
| 571 | } |
| 572 | else |
| 573 | { |
| 574 | Center.SetXYZ(0.5 * rst1rst2.XYZ() + PtRst1.XYZ()); |
| 575 | } |
| 576 | |
| 577 | return Standard_True; |
| 578 | |
| 579 | } |
| 580 | |
| 581 | |
| 582 | |
| 583 | |
| 584 | |
| 585 | |
| 586 | //======================================================================= |
| 587 | //function : Section |
| 588 | //purpose : |
| 589 | //======================================================================= |
| 590 | |
| 591 | void BRepBlend_RstRstConstRad::Section(const Standard_Real Param, |
| 592 | const Standard_Real U, |
| 593 | const Standard_Real V, |
| 594 | Standard_Real& Pdeb, |
| 595 | Standard_Real& Pfin, |
| 596 | gp_Circ& C) |
| 597 | { |
| 598 | gp_Vec ns, np, NotUsed; |
| 599 | gp_Pnt Center; |
| 600 | |
| 601 | tguide->D1(Param, ptgui, d1gui); |
| 602 | np = d1gui.Normalized(); |
| 603 | ptrst1 = cons1.Value(U); |
| 604 | ptrst2 = cons2.Value(V); |
| 605 | |
| 606 | Standard_Boolean IsCenter; |
| 607 | |
| 608 | IsCenter = CenterCircleRst1Rst2(ptrst1, ptrst2, np, Center, NotUsed); |
| 609 | |
| 610 | C.SetRadius(Abs(ray)); |
| 611 | ns = gp_Vec(Center, ptrst1).Normalized(); |
| 612 | |
| 613 | if (choix%2 != 0) { |
| 614 | np.Reverse(); |
| 615 | } |
| 616 | |
| 617 | C.SetPosition(gp_Ax2(Center, np, ns)); |
| 618 | Pdeb = 0; //ElCLib::Parameter(C, pts); |
| 619 | Pfin = ElCLib::Parameter(C, ptrst2); |
| 620 | |
| 621 | // Test of angles negative and almost null : Special Case |
| 622 | if (Pfin > 1.5 * M_PI) { |
| 623 | np.Reverse(); |
| 624 | C.SetPosition(gp_Ax2(Center, np, ns)); |
| 625 | Pfin = ElCLib::Parameter(C, ptrst2); |
| 626 | } |
| 627 | if (Pfin < Precision::PConfusion()) Pfin += Precision::PConfusion(); |
| 628 | } |
| 629 | |
| 630 | //======================================================================= |
| 631 | //function : IsRational |
| 632 | //purpose : |
| 633 | //======================================================================= |
| 634 | |
| 635 | Standard_Boolean BRepBlend_RstRstConstRad::IsRational () const |
| 636 | { |
| 637 | return (mySShape==BlendFunc_Rational || mySShape==BlendFunc_QuasiAngular); |
| 638 | } |
| 639 | |
| 640 | //======================================================================= |
| 641 | //function : GetSectionSize |
| 642 | //purpose : |
| 643 | //======================================================================= |
| 644 | |
| 645 | Standard_Real BRepBlend_RstRstConstRad::GetSectionSize() const |
| 646 | { |
| 647 | return maxang * Abs(ray); |
| 648 | } |
| 649 | |
| 650 | //======================================================================= |
| 651 | //function : GetMinimalWeight |
| 652 | //purpose : |
| 653 | //======================================================================= |
| 654 | |
| 655 | void BRepBlend_RstRstConstRad::GetMinimalWeight(TColStd_Array1OfReal& Weights) const |
| 656 | { |
| 657 | BlendFunc::GetMinimalWeights(mySShape, myTConv, minang, maxang, Weights ); |
| 658 | // It is supposed that it does not depend on the Radius! |
| 659 | } |
| 660 | |
| 661 | //======================================================================= |
| 662 | //function : NbIntervals |
| 663 | //purpose : |
| 664 | //======================================================================= |
| 665 | |
| 666 | Standard_Integer BRepBlend_RstRstConstRad::NbIntervals (const GeomAbs_Shape S) const |
| 667 | { |
| 668 | return guide->NbIntervals(BlendFunc::NextShape(S)); |
| 669 | } |
| 670 | |
| 671 | //======================================================================= |
| 672 | //function : Intervals |
| 673 | //purpose : |
| 674 | //======================================================================= |
| 675 | |
| 676 | void BRepBlend_RstRstConstRad::Intervals (TColStd_Array1OfReal& T, |
| 677 | const GeomAbs_Shape S) const |
| 678 | { |
| 679 | guide->Intervals(T, BlendFunc::NextShape(S)); |
| 680 | } |
| 681 | |
| 682 | //======================================================================= |
| 683 | //function : GetShape |
| 684 | //purpose : |
| 685 | //======================================================================= |
| 686 | |
| 687 | void BRepBlend_RstRstConstRad::GetShape (Standard_Integer& NbPoles, |
| 688 | Standard_Integer& NbKnots, |
| 689 | Standard_Integer& Degree, |
| 690 | Standard_Integer& NbPoles2d) |
| 691 | { |
| 692 | NbPoles2d = 2; |
| 693 | BlendFunc::GetShape(mySShape, maxang, NbPoles, NbKnots, Degree, myTConv); |
| 694 | } |
| 695 | |
| 696 | //======================================================================= |
| 697 | //function : GetTolerance |
| 698 | //purpose : Determine Tolerances to be used in approximations. |
| 699 | //======================================================================= |
| 700 | |
| 701 | void BRepBlend_RstRstConstRad::GetTolerance(const Standard_Real BoundTol, |
| 702 | const Standard_Real SurfTol, |
| 703 | const Standard_Real AngleTol, |
| 704 | math_Vector& Tol3d, |
| 705 | math_Vector& Tol1d) const |
| 706 | { |
| 707 | Standard_Integer low = Tol3d.Lower(), up = Tol3d.Upper(); |
| 708 | Standard_Real Tol; |
| 709 | Tol= GeomFill::GetTolerance(myTConv, minang, Abs(ray), |
| 710 | AngleTol, SurfTol); |
| 711 | Tol1d.Init(SurfTol); |
| 712 | Tol3d.Init(SurfTol); |
| 713 | Tol3d(low+1) = Tol3d(up-1) = Min(Tol, SurfTol); |
| 714 | Tol3d(low) = Tol3d(up) = Min(Tol, BoundTol); |
| 715 | } |
| 716 | |
| 717 | //======================================================================= |
| 718 | //function : Knots |
| 719 | //purpose : |
| 720 | //======================================================================= |
| 721 | |
| 722 | void BRepBlend_RstRstConstRad::Knots(TColStd_Array1OfReal& TKnots) |
| 723 | { |
| 724 | GeomFill::Knots(myTConv, TKnots); |
| 725 | } |
| 726 | |
| 727 | //======================================================================= |
| 728 | //function : Mults |
| 729 | //purpose : |
| 730 | //======================================================================= |
| 731 | |
| 732 | void BRepBlend_RstRstConstRad::Mults(TColStd_Array1OfInteger& TMults) |
| 733 | { |
| 734 | GeomFill::Mults(myTConv, TMults); |
| 735 | } |
| 736 | |
| 737 | //======================================================================= |
| 738 | //function : Section |
| 739 | //purpose : |
| 740 | //======================================================================= |
| 741 | |
| 742 | void BRepBlend_RstRstConstRad::Section(const Blend_Point& P, |
| 743 | TColgp_Array1OfPnt& Poles, |
| 744 | TColgp_Array1OfPnt2d& Poles2d, |
| 745 | TColStd_Array1OfReal& Weights) |
| 746 | { |
| 747 | gp_Vec ns, ns2, NotUsed; |
| 748 | gp_Pnt Center; |
| 749 | Standard_Real u, v; |
| 750 | |
| 751 | Standard_Real prm = P.Parameter(); |
| 752 | Standard_Integer low = Poles.Lower(); |
| 753 | Standard_Integer upp = Poles.Upper(); |
| 754 | |
| 755 | tguide->D1(prm,ptgui, d1gui); |
| 756 | nplan = d1gui.Normalized(); |
| 757 | |
| 758 | u = P.ParameterOnC1(); |
| 759 | v = P.ParameterOnC2(); |
| 760 | |
| 761 | gp_Pnt2d pt2d1 = rst1->Value(u); |
| 762 | gp_Pnt2d pt2d2 = rst2->Value(v); |
| 763 | |
| 764 | ptrst1 = cons1.Value(u); |
| 765 | ptrst2 = cons2.Value(v); |
| 766 | distmin = Min (distmin, ptrst1.Distance(ptrst2)); |
| 767 | |
| 768 | Poles2d(Poles2d.Lower()).SetCoord(pt2d1.X(),pt2d1.Y()); |
| 769 | Poles2d(Poles2d.Upper()).SetCoord(pt2d2.X(),pt2d2.Y()); |
| 770 | |
| 771 | // Linear case |
| 772 | if (mySShape == BlendFunc_Linear) { |
| 773 | Poles(low) = ptrst1; |
| 774 | Poles(upp) = ptrst2; |
| 775 | Weights(low) = 1.0; |
| 776 | Weights(upp) = 1.0; |
| 777 | return; |
| 778 | } |
| 779 | |
| 780 | // Calculate the center of the circle |
| 781 | Standard_Boolean IsCenter; |
| 782 | IsCenter = CenterCircleRst1Rst2(ptrst1, ptrst2, nplan, Center, NotUsed); |
| 783 | |
| 784 | // normals to the section with points |
| 785 | ns = gp_Vec(Center, ptrst1).Normalized(); |
| 786 | ns2 = gp_Vec(Center, ptrst2).Normalized(); |
| 787 | |
| 788 | if (choix%2 != 0) { |
| 789 | nplan.Reverse(); |
| 790 | } |
| 791 | |
| 792 | GeomFill::GetCircle(myTConv, |
| 793 | ns, ns2, |
| 794 | nplan, ptrst1, ptrst2, |
| 795 | Abs(ray), Center, |
| 796 | Poles, Weights); |
| 797 | } |
| 798 | |
| 799 | //======================================================================= |
| 800 | //function : Section |
| 801 | //purpose : |
| 802 | //======================================================================= |
| 803 | |
| 804 | Standard_Boolean BRepBlend_RstRstConstRad::Section(const Blend_Point& P, |
| 805 | TColgp_Array1OfPnt& Poles, |
| 806 | TColgp_Array1OfVec& DPoles, |
| 807 | TColgp_Array1OfPnt2d& Poles2d, |
| 808 | TColgp_Array1OfVec2d& DPoles2d, |
| 809 | TColStd_Array1OfReal& Weights, |
| 810 | TColStd_Array1OfReal& DWeights) |
| 811 | { |
| 812 | |
| 813 | gp_Vec d11, d21; |
| 814 | gp_Vec dnplan, d1n1, d1n2;//,np2, dnp2; |
| 815 | gp_Vec temp, tgct; |
| 816 | gp_Vec d1urst, d1vrst; |
| 817 | gp_Pnt Center, NotUsed; |
| 818 | |
| 819 | Standard_Real norm2, normmed, Dist; |
| 820 | |
| 821 | math_Vector sol(1, 2), valsol(1, 2), secmember(1, 2); |
| 822 | math_Matrix gradsol(1, 2, 1, 2); |
| 823 | |
| 824 | Standard_Real prm = P.Parameter(); |
| 825 | Standard_Integer low = Poles.Lower(); |
| 826 | Standard_Integer upp = Poles.Upper(); |
| 827 | Standard_Boolean istgt; |
| 828 | |
| 829 | tguide->D2(prm, ptgui, d1gui, d2gui); |
| 830 | normtg = d1gui.Magnitude(); |
| 831 | nplan = d1gui.Normalized(); |
| 832 | dnplan.SetLinearForm(1./normtg, d2gui, |
| 833 | -1./normtg * (nplan.Dot(d2gui)), nplan); |
| 834 | |
| 835 | sol(1) = prmrst1 = P.ParameterOnC1(); |
| 836 | sol(2) = prmrst2 = P.ParameterOnC2(); |
| 837 | pt2drst1 = rst1->Value(prmrst1); |
| 838 | pt2drst2 = rst2->Value(prmrst2); |
| 839 | |
| 840 | Values(sol, valsol, gradsol); |
| 841 | |
| 842 | cons1.D1(sol(1), ptrst1, d11); |
| 843 | cons2.D1(sol(2), ptrst2, d21); |
| 844 | |
| 845 | temp.SetXYZ(ptrst1.XYZ() - ptgui.XYZ()); |
| 846 | secmember(1) = normtg - dnplan.Dot(temp); |
| 847 | |
| 848 | temp.SetXYZ(ptrst2.XYZ() - ptgui.XYZ()); |
| 849 | secmember(2) = normtg - dnplan.Dot(temp); |
| 850 | |
| 851 | math_Gauss Resol(gradsol, 1.e-9); |
| 852 | |
| 853 | if (Resol.IsDone()) { |
| 854 | istgt = Standard_False; |
| 855 | Resol.Solve(secmember); |
| 856 | } |
| 857 | else { |
| 858 | math_SVD SingRS (gradsol); |
| 859 | if (SingRS.IsDone()) { |
| 860 | math_Vector DEDT(1,2); |
| 861 | DEDT = secmember; |
| 862 | SingRS.Solve(DEDT, secmember, 1.e-6); |
| 863 | istgt = Standard_False; |
| 864 | } |
| 865 | else istgt = Standard_True; |
| 866 | } |
| 867 | |
| 868 | gp_Vec med; |
| 869 | gp_Vec rst1rst2(ptrst1, ptrst2); |
| 870 | Standard_Boolean IsCenter; |
| 871 | |
| 872 | IsCenter = CenterCircleRst1Rst2(ptrst1, ptrst2, nplan, Center, med); |
| 873 | if (!IsCenter) return Standard_False; |
| 874 | |
| 875 | normmed = med.Magnitude(); |
| 876 | med.Normalize(); |
| 877 | gp_Vec n1(Center, ptrst1), n2(Center, ptrst2); |
| 878 | n1.Normalize(); |
| 879 | n2.Normalize(); |
| 880 | |
| 881 | if (!istgt) { |
| 882 | // secmember contains derivatives of parameters on curves |
| 883 | // compared to t |
| 884 | tgrst1 = secmember(1) * d11; |
| 885 | tgrst2 = secmember(2) * d21; |
| 886 | |
| 887 | gp_Vec d1rst1rst2; |
| 888 | |
| 889 | norm2 = rst1rst2.SquareMagnitude(); |
| 890 | d1rst1rst2 = tgrst2 - tgrst1; |
| 891 | Dist = ray * ray - 0.25 * norm2; |
| 892 | |
| 893 | if (Dist > 1.E-07) { |
| 894 | gp_Vec d1P1P2CrosNp, dmed; |
| 895 | d1P1P2CrosNp = d1rst1rst2.Crossed(nplan) + rst1rst2.Crossed(dnplan); |
| 896 | // derivative of the perpendicular bisector |
| 897 | dmed = d1P1P2CrosNp - med.Dot(d1P1P2CrosNp) * med; |
| 898 | dmed /= normmed; |
| 899 | Dist = sqrt(Dist); |
| 900 | Standard_Real d1Dist = - (0.25 / Dist) * rst1rst2.Dot(d1rst1rst2); |
| 901 | |
| 902 | if (choix > 2) { |
| 903 | dmed.Reverse(); |
| 904 | } |
| 905 | |
| 906 | // the derivative of coefficient Dist is located in dmed |
| 907 | dmed.SetLinearForm(Dist, dmed, d1Dist, med); |
| 908 | d1rst1rst2 *= 0.5; |
| 909 | // derivative of the Normal to the curve in P1 |
| 910 | d1n1 = - (dmed + d1rst1rst2) / ray; |
| 911 | |
| 912 | // derivative of the Normal to the curve in P2 |
| 913 | d1n2 = (d1rst1rst2 - dmed) / ray; |
| 914 | } |
| 915 | else { |
| 916 | d1rst1rst2 *= 0.5; |
| 917 | // Normal to the curve in P1 |
| 918 | d1n1 = - d1rst1rst2 / ray; |
| 919 | |
| 920 | // Normal to the curve in P2 |
| 921 | d1n2 = d1rst1rst2 / ray; |
| 922 | } |
| 923 | } |
| 924 | |
| 925 | // Tops 2d |
| 926 | |
| 927 | Poles2d(Poles2d.Lower()).SetCoord(pt2drst1.X(), pt2drst1.Y()); |
| 928 | Poles2d(Poles2d.Upper()).SetCoord(pt2drst2.X(), pt2drst2.Y()); |
| 929 | if (!istgt) { |
| 930 | Standard_Real a, b; |
| 931 | surf1->D1(pt2drst1.X(), pt2drst1.Y(), NotUsed, d1urst, d1vrst); |
| 932 | t3dto2d(a,b,tgrst1, d1urst, d1vrst); |
| 933 | DPoles2d(Poles2d.Lower()).SetCoord(a, b); |
| 934 | |
| 935 | surf2->D1(pt2drst2.X(), pt2drst2.Y(), NotUsed, d1urst, d1vrst); |
| 936 | t3dto2d(a, b, tgrst2, d1urst, d1vrst); |
| 937 | DPoles2d(Poles2d.Upper()).SetCoord(a, b); |
| 938 | } |
| 939 | |
| 940 | // Linear case |
| 941 | if (mySShape == BlendFunc_Linear) { |
| 942 | Poles(low) = ptrst1; |
| 943 | Poles(upp) = ptrst2; |
| 944 | Weights(low) = 1.0; |
| 945 | Weights(upp) = 1.0; |
| 946 | if (!istgt) { |
| 947 | DPoles(low) = tgrst1; |
| 948 | DPoles(upp) = tgrst2; |
| 949 | DWeights(low) = 0.0; |
| 950 | DWeights(upp) = 0.0; |
| 951 | } |
| 952 | return (!istgt); |
| 953 | } |
| 954 | |
| 955 | // Case of the circle |
| 956 | // tangent to the center of the circle |
| 957 | if (!istgt) { |
| 958 | tgct.SetLinearForm(-ray, d1n1, tgrst1); |
| 959 | } |
| 960 | |
| 961 | |
| 962 | if (choix%2 != 0) { |
| 963 | nplan.Reverse(); |
| 964 | dnplan.Reverse(); |
| 965 | } |
| 966 | |
| 967 | if (!istgt) { |
| 968 | return GeomFill::GetCircle(myTConv, |
| 969 | n1, n2, |
| 970 | d1n1, d1n2, |
| 971 | nplan, dnplan, |
| 972 | ptrst1, ptrst2, |
| 973 | tgrst1, tgrst2, |
| 974 | Abs(ray), 0, |
| 975 | Center, tgct, |
| 976 | Poles, |
| 977 | DPoles, |
| 978 | Weights, |
| 979 | DWeights); |
| 980 | } |
| 981 | else { |
| 982 | GeomFill::GetCircle(myTConv, |
| 983 | n1, n2, |
| 984 | nplan, ptrst1, ptrst2, |
| 985 | Abs(ray), Center, |
| 986 | Poles, Weights); |
| 987 | return Standard_False; |
| 988 | } |
| 989 | } |
| 990 | |
| 991 | //======================================================================= |
| 992 | //function : Section |
| 993 | //purpose : |
| 994 | //======================================================================= |
| 995 | |
| 996 | Standard_Boolean BRepBlend_RstRstConstRad::Section |
| 997 | (const Blend_Point&, |
| 998 | TColgp_Array1OfPnt&, |
| 999 | TColgp_Array1OfVec&, |
| 1000 | TColgp_Array1OfVec&, |
| 1001 | TColgp_Array1OfPnt2d&, |
| 1002 | TColgp_Array1OfVec2d&, |
| 1003 | TColgp_Array1OfVec2d&, |
| 1004 | TColStd_Array1OfReal&, |
| 1005 | TColStd_Array1OfReal&, |
| 1006 | TColStd_Array1OfReal&) |
| 1007 | { |
| 1008 | return Standard_False; |
| 1009 | } |
| 1010 | |
| 1011 | |
| 1012 | void BRepBlend_RstRstConstRad::Resolution(const Standard_Integer IC2d, |
| 1013 | const Standard_Real Tol, |
| 1014 | Standard_Real& TolU, |
| 1015 | Standard_Real& TolV) const |
| 1016 | { |
| 1017 | if(IC2d == 1){ |
| 1018 | TolU = surf1->UResolution(Tol); |
| 1019 | TolV = surf1->VResolution(Tol); |
| 1020 | } |
| 1021 | else { |
| 1022 | TolU = surf2->UResolution(Tol); |
| 1023 | TolV = surf2->VResolution(Tol); |
| 1024 | } |
| 1025 | } |
| 1026 | |