src/BlendFunc/BlendFunc_RuledInv.cxx

   1 // File:      BlendFunc_RuledInv.cxx
   2 // Created:   Thu Dec  2 10:28:44 1993
   3 // Author:    Jacques GOUSSARD
   4 // Copyright: OPEN CASCADE 1993
   5
   6 #include <BlendFunc_RuledInv.ixx>
   7
   8 #include <Precision.hxx>
   9
  10 BlendFunc_RuledInv::BlendFunc_RuledInv(const Handle(Adaptor3d_HSurface)& S1,
  11                                        const Handle(Adaptor3d_HSurface)& S2,
  12                                        const Handle(Adaptor3d_HCurve)& C) :
  13        surf1(S1),surf2(S2),curv(C)
  14 {}
  15
  16 void BlendFunc_RuledInv::Set(const Standard_Boolean OnFirst,
  17                              const Handle(Adaptor2d_HCurve2d)& C)
  18 {
  19   first = OnFirst;
  20   csurf = C;
  21 }
  22
  23 Standard_Integer BlendFunc_RuledInv::NbEquations () const
  24 {
  25   return 4;
  26 }
  27
  28 void BlendFunc_RuledInv::GetTolerance(math_Vector& Tolerance,
  29                                       const Standard_Real Tol) const
  30 {
  31   Tolerance(1) = csurf->Resolution(Tol);
  32   Tolerance(2) = curv->Resolution(Tol);
  33   if (first) {
  34     Tolerance(3) = surf2->UResolution(Tol);
  35     Tolerance(4) = surf2->VResolution(Tol);
  36   }
  37   else {
  38     Tolerance(3) = surf1->UResolution(Tol);
  39     Tolerance(4) = surf1->VResolution(Tol);
  40   }
  41 }
  42
  43 void BlendFunc_RuledInv::GetBounds(math_Vector& InfBound,
  44                                    math_Vector& SupBound) const
  45 {
  46   InfBound(1) = csurf->FirstParameter();
  47   InfBound(2) = curv->FirstParameter();
  48   SupBound(1) = csurf->LastParameter();
  49   SupBound(2) = curv->LastParameter();
  50
  51   if (first) {
  52     InfBound(3) = surf2->FirstUParameter();
  53     InfBound(4) = surf2->FirstVParameter();
  54     SupBound(3) = surf2->LastUParameter();
  55     SupBound(4) = surf2->LastVParameter();
  56     if(!Precision::IsInfinite(InfBound(3)) &&
  57        !Precision::IsInfinite(SupBound(3))) {
  58       const Standard_Real range = (SupBound(3) - InfBound(3));
  59       InfBound(3) -= range;
  60       SupBound(3) += range;
  61     }
  62     if(!Precision::IsInfinite(InfBound(4)) &&
  63        !Precision::IsInfinite(SupBound(4))) {
  64       const Standard_Real range = (SupBound(4) - InfBound(4));
  65       InfBound(4) -= range;
  66       SupBound(4) += range;
  67     }
  68   }
  69   else {
  70     InfBound(3) = surf1->FirstUParameter();
  71     InfBound(4) = surf1->FirstVParameter();
  72     SupBound(3) = surf1->LastUParameter();
  73     SupBound(4) = surf1->LastVParameter();
  74     if(!Precision::IsInfinite(InfBound(3)) &&
  75        !Precision::IsInfinite(SupBound(3))) {
  76       const Standard_Real range = (SupBound(3) - InfBound(3));
  77       InfBound(3) -= range;
  78       SupBound(3) += range;
  79     }
  80     if(!Precision::IsInfinite(InfBound(4)) &&
  81        !Precision::IsInfinite(SupBound(4))) {
  82       const Standard_Real range = (SupBound(4) - InfBound(4));
  83       InfBound(4) -= range;
  84       SupBound(4) += range;
  85     }
  86   }
  87 }
  88
  89 Standard_Boolean BlendFunc_RuledInv::IsSolution(const math_Vector& Sol,
  90                                                 const Standard_Real Tol)
  91 {
  92   math_Vector valsol(1,4);
  93   Value(Sol,valsol);
  94   if (Abs(valsol(1)) <= Tol &&
  95       Abs(valsol(2)) <= Tol &&
  96       Abs(valsol(3)) <= Tol &&
  97       Abs(valsol(4)) <= Tol)
  98     return Standard_True;
  99
 100   return Standard_False;
 101 }
 102
 103
 104 Standard_Boolean BlendFunc_RuledInv::Value(const math_Vector& X,
 105                                            math_Vector& F)
 106 {
 107   gp_Pnt ptcur;
 108   gp_Vec d1cur;
 109   curv->D1(X(2),ptcur,d1cur);
 110
 111   const gp_XYZ nplan = d1cur.Normalized().XYZ();
 112   const Standard_Real theD = -(nplan.Dot(ptcur.XYZ()));
 113   const gp_Pnt2d pt2d(csurf->Value(X(1)));
 114
 115   gp_Pnt pts1,pts2;
 116   gp_Vec d1u1,d1v1,d1u2,d1v2;
 117   if (first)
 118   {
 119     surf1->D1(pt2d.X(),pt2d.Y(),pts1,d1u1,d1v1);
 120     surf2->D1(X(3),X(4),pts2,d1u2,d1v2);
 121   }
 122   else
 123   {
 124     surf1->D1(X(3),X(4),pts1,d1u1,d1v1);
 125     surf2->D1(pt2d.X(),pt2d.Y(),pts2,d1u2,d1v2);
 126   }
 127
 128   const gp_XYZ temp(pts2.XYZ()-pts1.XYZ());
 129
 130   gp_XYZ ns1 = d1u1.Crossed(d1v1).XYZ();
 131   gp_XYZ ns2 = d1u2.Crossed(d1v2).XYZ();
 132   const Standard_Real norm1 = nplan.Crossed(ns1).Modulus();
 133   const Standard_Real norm2 = nplan.Crossed(ns2).Modulus();
 134   ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1);
 135   ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2,ns2);
 136
 137   F(1) = (nplan.Dot(pts1.XYZ())) + theD;
 138   F(2) = (nplan.Dot(pts2.XYZ())) + theD;
 139
 140   F(3) = temp.Dot(ns1);
 141   F(4) = temp.Dot(ns2);
 142
 143   return Standard_True;
 144 }
 145
 146 Standard_Boolean BlendFunc_RuledInv::Derivatives(const math_Vector& X,
 147                                                  math_Matrix& D)
 148 {
 149   gp_Pnt ptcur;
 150   gp_Vec d1cur,d2cur;
 151   curv->D2(X(2),ptcur,d1cur,d2cur);
 152
 153   const Standard_Real normtgcur = d1cur.Magnitude();
 154   const gp_Vec nplan = d1cur.Normalized();
 155   const Standard_Real theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
 156
 157   gp_Vec dnplan;
 158   dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur);
 159   dnplan /= normtgcur;
 160
 161   gp_Pnt2d p2d;
 162   gp_Vec2d v2d;
 163   csurf->D1(X(1),p2d,v2d);
 164
 165   gp_Pnt pts1,pts2;
 166   gp_Vec d1u1,d1v1,d1u2,d1v2;
 167   gp_Vec d2u1,d2v1,d2u2,d2v2,d2uv1,d2uv2;
 168   gp_Vec dpdt, p1p2;
 169   if (first)
 170   {
 171     surf1->D2(p2d.X(),p2d.Y(),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 172     surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 173     dpdt.SetLinearForm(v2d.X(),d1u1,v2d.Y(),d1v1);
 174     p1p2 = gp_Vec(pts1,pts2);
 175     D(1,1) = dpdt.Dot(nplan);
 176     D(1,2) = dnplan.XYZ().Dot(pts1.XYZ()-ptcur.XYZ()) - normtgcur;
 177     D(1,3) = 0.;
 178     D(1,4) = 0.;
 179
 180     D(2,1) = 0.;
 181     D(2,2) = dnplan.XYZ().Dot(pts2.XYZ()-ptcur.XYZ()) - normtgcur;
 182     D(2,3) = d1u2.Dot(nplan);
 183     D(2,4) = d1v2.Dot(nplan);
 184   }
 185   else
 186   {
 187     surf1->D2(X(3),X(4),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 188     surf2->D2(p2d.X(),p2d.Y(),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 189     dpdt.SetLinearForm(v2d.X(),d1u2,v2d.Y(),d1v2);
 190     p1p2 = gp_Vec(pts1,pts2);
 191     D(1,1) = 0.;
 192     D(1,2) = dnplan.XYZ().Dot(pts1.XYZ()-ptcur.XYZ()) - normtgcur;
 193     D(1,3) = d1u1.Dot(nplan);
 194     D(1,4) = d1v1.Dot(nplan);
 195
 196     D(2,1) = dpdt.Dot(nplan);
 197     D(2,2) = dnplan.XYZ().Dot(pts2.XYZ()-ptcur.XYZ()) - normtgcur;
 198     D(2,3) = 0.;
 199     D(2,4) = 0.;
 200   }
 201
 202   const gp_Vec ns1 = d1u1.Crossed(d1v1);
 203   const gp_Vec ns2 = d1u2.Crossed(d1v2);
 204   const gp_Vec ncrossns1 = nplan.Crossed(ns1);
 205   const gp_Vec ncrossns2 = nplan.Crossed(ns2);
 206   const Standard_Real norm1 = ncrossns1.Magnitude();
 207   const Standard_Real norm2 = ncrossns2.Magnitude();
 208
 209   const Standard_Real ndotns1 = nplan.Dot(ns1);
 210   const Standard_Real ndotns2 = nplan.Dot(ns2);
 211
 212   gp_Vec nor1,nor2;
 213   nor1.SetLinearForm(ndotns1/norm1,nplan,-1./norm1,ns1);
 214   nor2.SetLinearForm(ndotns2/norm2,nplan,-1./norm2,ns2);
 215
 216   if (first) {
 217     D(3,3) = d1u2.Dot(nor1);
 218     D(3,4) = d1v2.Dot(nor1);
 219
 220     D(4,1) = -(dpdt.Dot(nor2));
 221   }
 222   else {
 223     D(3,1) = dpdt.Dot(nor1);
 224
 225     D(4,3) = -(d1u1.Dot(nor2));
 226     D(4,4) = -(d1v1.Dot(nor2));
 227   }
 228
 229   gp_Vec resul1,resul2,temp;
 230   Standard_Real grosterme;
 231
 232   // Derivee de nor1 par rapport a u1
 233
 234   temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
 235   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 236   resul1.SetLinearForm(-(grosterme*ndotns1-nplan.Dot(temp))/norm1,nplan,
 237                        grosterme/norm1,ns1,
 238                        -1./norm1,temp);
 239
 240   // Derivee par rapport a v1
 241
 242   temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
 243   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 244   resul2.SetLinearForm(-(grosterme*ndotns1-nplan.Dot(temp))/norm1,nplan,
 245                        grosterme/norm1,ns1,
 246                        -1./norm1,temp);
 247
 248   if (first) {
 249     resul1.SetLinearForm(v2d.X(),resul1,v2d.Y(),resul2);
 250     D(3,1) = p1p2.Dot(resul1) - (dpdt.Dot(nor1));
 251   }
 252   else {
 253     D(3,3) = -(d1u1.Dot(nor1)) + p1p2.Dot(resul1);
 254     D(3,4) = -(d1v1.Dot(nor1)) + p1p2.Dot(resul2);
 255   }
 256
 257   // Derivee de nor2 par rapport a u2
 258   temp = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
 259   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 260   resul1.SetLinearForm(-(grosterme*ndotns2-nplan.Dot(temp))/norm2,nplan,
 261                        grosterme/norm2,ns2,
 262                        -1./norm2,temp);
 263
 264   // Derivee par rapport a v2
 265   temp = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
 266   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 267   resul2.SetLinearForm(-(grosterme*ndotns2-nplan.Dot(temp))/norm2,nplan,
 268                        grosterme/norm2,ns2,
 269                        -1./norm2,temp);
 270
 271   if (first) {
 272     D(4,3) = d1u2.Dot(nor2) + p1p2.Dot(resul1);
 273     D(4,4) = d1v2.Dot(nor2) + p1p2.Dot(resul2);
 274   }
 275   else {
 276     resul1.SetLinearForm(v2d.X(),resul1,v2d.Y(),resul2);
 277     D(4,1) = p1p2.Dot(resul1) + dpdt.Dot(nor2) ;
 278   }
 279
 280
 281   // derivee par rapport a w (parametre sur ligne guide)
 282
 283   grosterme = ncrossns1.Dot(dnplan.Crossed(ns1))/norm1/norm1;
 284   resul1.SetLinearForm(-(grosterme*ndotns1-dnplan.Dot(ns1))/norm1,nplan,
 285                        ndotns1/norm1,dnplan,
 286                        grosterme/norm1,ns1);
 287
 288
 289   grosterme = ncrossns2.Dot(dnplan.Crossed(ns2))/norm2/norm2;
 290   resul2.SetLinearForm(-(grosterme*ndotns2-dnplan.Dot(ns2))/norm2,nplan,
 291                        ndotns2/norm2,dnplan,
 292                        grosterme/norm2,ns2);
 293
 294   D(3,2) = p1p2.Dot(resul1);
 295   D(4,2) = p1p2.Dot(resul2);
 296
 297   return Standard_True;
 298 }
 299
 300
 301 Standard_Boolean BlendFunc_RuledInv::Values(const math_Vector& X,
 302                                             math_Vector& F,
 303                                             math_Matrix& D)
 304 {
 305   gp_Pnt ptcur;
 306   gp_Vec d1cur,d2cur;
 307   curv->D2(X(2),ptcur,d1cur,d2cur);
 308
 309   const Standard_Real normtgcur = d1cur.Magnitude();
 310   const gp_Vec nplan = d1cur.Normalized();
 311   const Standard_Real theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
 312
 313   gp_Vec dnplan;
 314   dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur);
 315   dnplan /= normtgcur;
 316
 317   gp_Pnt2d p2d;
 318   gp_Vec2d v2d;
 319   csurf->D1(X(1),p2d,v2d);
 320
 321   gp_Pnt pts1,pts2;
 322   gp_Vec d1u1,d1v1,d1u2,d1v2;
 323   gp_Vec d2u1,d2v1,d2u2,d2v2,d2uv1,d2uv2;
 324   gp_Vec dpdt,p1p2;
 325   if (first)
 326   {
 327     surf1->D2(p2d.X(),p2d.Y(),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 328     surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 329     dpdt.SetLinearForm(v2d.X(),d1u1,v2d.Y(),d1v1);
 330     p1p2 = gp_Vec(pts1,pts2);
 331     D(1,1) = dpdt.Dot(nplan);
 332     D(1,2) = dnplan.XYZ().Dot(pts1.XYZ()-ptcur.XYZ()) - normtgcur;
 333     D(1,3) = 0.;
 334     D(1,4) = 0.;
 335
 336     D(2,1) = 0.;
 337     D(2,2) = dnplan.XYZ().Dot(pts2.XYZ()-ptcur.XYZ()) - normtgcur;
 338     D(2,3) = d1u2.Dot(nplan);
 339     D(2,4) = d1v2.Dot(nplan);
 340   }
 341   else
 342   {
 343     surf1->D2(X(3),X(4),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 344     surf2->D2(p2d.X(),p2d.Y(),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 345     dpdt.SetLinearForm(v2d.X(),d1u2,v2d.Y(),d1v2);
 346     p1p2 = gp_Vec(pts1,pts2);
 347     D(1,1) = 0.;
 348     D(1,2) = dnplan.XYZ().Dot(pts1.XYZ()-ptcur.XYZ()) - normtgcur;
 349     D(1,3) = d1u1.Dot(nplan);
 350     D(1,4) = d1v1.Dot(nplan);
 351
 352     D(2,1) = dpdt.Dot(nplan);
 353     D(2,2) = dnplan.XYZ().Dot(pts2.XYZ()-ptcur.XYZ()) - normtgcur;
 354     D(2,3) = 0.;
 355     D(2,4) = 0.;
 356   }
 357
 358   const gp_Vec ns1 = d1u1.Crossed(d1v1);
 359   const gp_Vec ns2 = d1u2.Crossed(d1v2);
 360   const gp_Vec ncrossns1 = nplan.Crossed(ns1);
 361   const gp_Vec ncrossns2 = nplan.Crossed(ns2);
 362   const Standard_Real norm1 = ncrossns1.Magnitude();
 363   const Standard_Real norm2 = ncrossns2.Magnitude();
 364
 365   const Standard_Real ndotns1 = nplan.Dot(ns1);
 366   const Standard_Real ndotns2 = nplan.Dot(ns2);
 367
 368   gp_Vec nor1,nor2;
 369   nor1.SetLinearForm(ndotns1/norm1,nplan,-1./norm1,ns1);
 370   nor2.SetLinearForm(ndotns2/norm2,nplan,-1./norm2,ns2);
 371
 372   F(1) = (nplan.Dot(pts1.XYZ())) + theD;
 373   F(2) = (nplan.Dot(pts2.XYZ())) + theD;
 374
 375   F(3) = p1p2.Dot(nor1);
 376   F(4) = p1p2.Dot(nor2);
 377
 378   if (first) {
 379     D(3,3) = d1u2.Dot(nor1);
 380     D(3,4) = d1v2.Dot(nor1);
 381
 382     D(4,1) = -(dpdt.Dot(nor2));
 383   }
 384   else {
 385     D(3,1) = dpdt.Dot(nor1);
 386
 387     D(4,3) = -(d1u1.Dot(nor2));
 388     D(4,4) = -(d1v1.Dot(nor2));
 389   }
 390
 391   gp_Vec resul1,resul2,temp;
 392   Standard_Real grosterme;
 393
 394   // Derivee de nor1 par rapport a u1
 395
 396   temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
 397   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 398   resul1.SetLinearForm(-(grosterme*ndotns1-nplan.Dot(temp))/norm1,nplan,
 399                        grosterme/norm1,ns1,
 400                        -1./norm1,temp);
 401
 402   // Derivee par rapport a v1
 403
 404   temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
 405   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 406   resul2.SetLinearForm(-(grosterme*ndotns1-nplan.Dot(temp))/norm1,nplan,
 407                        grosterme/norm1,ns1,
 408                        -1./norm1,temp);
 409
 410   if (first) {
 411     resul1.SetLinearForm(v2d.X(),resul1,v2d.Y(),resul2);
 412     D(3,1) = p1p2.Dot(resul1) - (dpdt.Dot(nor1));
 413   }
 414   else {
 415     D(3,3) = -(d1u1.Dot(nor1)) + p1p2.Dot(resul1);
 416     D(3,4) = -(d1v1.Dot(nor1)) + p1p2.Dot(resul2);
 417   }
 418
 419   // Derivee de nor2 par rapport a u2
 420   temp = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
 421   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 422   resul1.SetLinearForm(-(grosterme*ndotns2-nplan.Dot(temp))/norm2,nplan,
 423                        grosterme/norm2,ns2,
 424                        -1./norm2,temp);
 425
 426   // Derivee par rapport a v2
 427   temp = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
 428   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 429   resul2.SetLinearForm(-(grosterme*ndotns2-nplan.Dot(temp))/norm2,nplan,
 430                        grosterme/norm2,ns2,
 431                        -1./norm2,temp);
 432
 433   if (first) {
 434     D(4,3) = d1u2.Dot(nor2) + p1p2.Dot(resul1);
 435     D(4,4) = d1v2.Dot(nor2) + p1p2.Dot(resul2);
 436   }
 437   else {
 438     resul1.SetLinearForm(v2d.X(),resul1,v2d.Y(),resul2);
 439     D(4,1) = p1p2.Dot(resul1) + dpdt.Dot(nor2) ;
 440   }
 441
 442
 443   // derivee par rapport a w (parametre sur ligne guide)
 444
 445   grosterme = ncrossns1.Dot(dnplan.Crossed(ns1))/norm1/norm1;
 446   resul1.SetLinearForm(-(grosterme*ndotns1-dnplan.Dot(ns1))/norm1,nplan,
 447                        ndotns1/norm1,dnplan,
 448                        grosterme/norm1,ns1);
 449
 450
 451   grosterme = ncrossns2.Dot(dnplan.Crossed(ns2))/norm2/norm2;
 452   resul2.SetLinearForm(-(grosterme*ndotns2-dnplan.Dot(ns2))/norm2,nplan,
 453                        ndotns2/norm2,dnplan,
 454                        grosterme/norm2,ns2);
 455
 456   D(3,2) = p1p2.Dot(resul1);
 457   D(4,2) = p1p2.Dot(resul2);
 458
 459   return Standard_True;
 460 }