src/BlendFunc/BlendFunc_ConstRadInv.cxx

   1 // Created on: 1993-12-02
   2 // Created by: Jacques GOUSSARD
   3 // Copyright (c) 1993-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 under
   9 // the terms of the GNU Lesser General Public License 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 <BlendFunc_ConstRadInv.ixx>
  18
  19 #include <Precision.hxx>
  20 #include <BlendFunc.hxx>
  21
  22 #define Eps 1.e-15
  23
  24
  25 BlendFunc_ConstRadInv::BlendFunc_ConstRadInv(const Handle(Adaptor3d_HSurface)& S1,
  26                                              const Handle(Adaptor3d_HSurface)& S2,
  27                                              const Handle(Adaptor3d_HCurve)& C):
  28        surf1(S1),surf2(S2),curv(C)
  29 {}
  30
  31 void BlendFunc_ConstRadInv::Set(const Standard_Real R,
  32                                 const Standard_Integer Choix)
  33
  34 {
  35   choix = Choix;
  36   switch (choix) {
  37   case 1:
  38   case 2:
  39     {
  40       ray1 = -R;
  41       ray2 = -R;
  42     }
  43     break;
  44   case 3:
  45   case 4:
  46     {
  47       ray1 = R;
  48       ray2 = -R;
  49     }
  50     break;
  51   case 5:
  52   case 6:
  53     {
  54       ray1 = R;
  55       ray2 = R;
  56     }
  57     break;
  58   case 7:
  59   case 8:
  60     {
  61       ray1 = -R;
  62       ray2 = R;
  63     }
  64     break;
  65   default:
  66     ray1 = ray2 = -R;
  67   }
  68 }
  69
  70 void BlendFunc_ConstRadInv::Set(const Standard_Boolean OnFirst,
  71                                 const Handle(Adaptor2d_HCurve2d)& C)
  72 {
  73   first = OnFirst;
  74   csurf = C;
  75 }
  76
  77 Standard_Integer BlendFunc_ConstRadInv::NbEquations () const
  78 {
  79   return 4;
  80 }
  81
  82
  83 void BlendFunc_ConstRadInv::GetTolerance(math_Vector& Tolerance,
  84                                          const Standard_Real Tol) const
  85 {
  86   Tolerance(1) = csurf->Resolution(Tol);
  87   Tolerance(2) = curv->Resolution(Tol);
  88   if (first) {
  89     Tolerance(3) = surf2->UResolution(Tol);
  90     Tolerance(4) = surf2->VResolution(Tol);
  91   }
  92   else {
  93     Tolerance(3) = surf1->UResolution(Tol);
  94     Tolerance(4) = surf1->VResolution(Tol);
  95   }
  96 }
  97
  98
  99 void BlendFunc_ConstRadInv::GetBounds(math_Vector& InfBound,
 100                                       math_Vector& SupBound) const
 101 {
 102   InfBound(1) = csurf->FirstParameter();
 103   InfBound(2) = curv->FirstParameter();
 104   SupBound(1) = csurf->LastParameter();
 105   SupBound(2) = curv->LastParameter();
 106
 107   if (first) {
 108     InfBound(3) = surf2->FirstUParameter();
 109     InfBound(4) = surf2->FirstVParameter();
 110     SupBound(3) = surf2->LastUParameter();
 111     SupBound(4) = surf2->LastVParameter();
 112     if(!Precision::IsInfinite(InfBound(3)) &&
 113        !Precision::IsInfinite(SupBound(3))) {
 114       Standard_Real range = (SupBound(3) - InfBound(3));
 115       InfBound(3) -= range;
 116       SupBound(3) += range;
 117     }
 118     if(!Precision::IsInfinite(InfBound(4)) &&
 119        !Precision::IsInfinite(SupBound(4))) {
 120       Standard_Real range = (SupBound(4) - InfBound(4));
 121       InfBound(4) -= range;
 122       SupBound(4) += range;
 123     }
 124   }
 125   else {
 126     InfBound(3) = surf1->FirstUParameter();
 127     InfBound(4) = surf1->FirstVParameter();
 128     SupBound(3) = surf1->LastUParameter();
 129     SupBound(4) = surf1->LastVParameter();
 130     if(!Precision::IsInfinite(InfBound(3)) &&
 131        !Precision::IsInfinite(SupBound(3))) {
 132       Standard_Real range = (SupBound(3) - InfBound(3));
 133       InfBound(3) -= range;
 134       SupBound(3) += range;
 135     }
 136     if(!Precision::IsInfinite(InfBound(4)) &&
 137        !Precision::IsInfinite(SupBound(4))) {
 138       Standard_Real range = (SupBound(4) - InfBound(4));
 139       InfBound(4) -= range;
 140       SupBound(4) += range;
 141     }
 142   }
 143 }
 144
 145 Standard_Boolean BlendFunc_ConstRadInv::IsSolution(const math_Vector& Sol,
 146                                                    const Standard_Real Tol)
 147 {
 148   math_Vector valsol(1,4);
 149   Value(Sol,valsol);
 150   if (Abs(valsol(1)) <= Tol &&
 151       valsol(2)*valsol(2) + valsol(3)*valsol(3) +
 152       valsol(4)*valsol(4) <= Tol*Tol) {
 153     return Standard_True;
 154   }
 155   return Standard_False;
 156
 157 }
 158
 159
 160 Standard_Boolean BlendFunc_ConstRadInv::Value(const math_Vector& X,
 161                                               math_Vector& F)
 162 {
 163   gp_Pnt ptcur;
 164   gp_Vec d1cur;
 165   curv->D1(X(2),ptcur,d1cur);
 166
 167   const gp_Vec nplan = d1cur.Normalized();
 168   const Standard_Real theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
 169
 170   const gp_Pnt2d pt2d(csurf->Value(X(1)));
 171
 172   gp_Pnt pts1,pts2;
 173   gp_Vec d1u1,d1v1,d1u2,d1v2;
 174   if (first)
 175   {
 176     surf1->D1(pt2d.X(),pt2d.Y(),pts1,d1u1,d1v1);
 177     surf2->D1(X(3),X(4),pts2,d1u2,d1v2);
 178   }
 179   else
 180   {
 181     surf1->D1(X(3),X(4),pts1,d1u1,d1v1);
 182     surf2->D1(pt2d.X(),pt2d.Y(),pts2,d1u2,d1v2);
 183   }
 184
 185   F(1) = (nplan.X() * (pts1.X() + pts2.X()) +
 186           nplan.Y() * (pts1.Y() + pts2.Y()) +
 187           nplan.Z() * (pts1.Z() + pts2.Z())) /2. + theD;
 188
 189   gp_Vec ns1 = d1u1.Crossed(d1v1);
 190   if (ns1.Magnitude() < Eps) {
 191     if (first) BlendFunc::ComputeNormal(surf1, pt2d, ns1);
 192     else {
 193       gp_Pnt2d P(X(3), X(4));
 194       BlendFunc::ComputeNormal(surf1, P, ns1);
 195     }
 196   }
 197
 198   gp_Vec ns2 = d1u2.Crossed(d1v2);
 199   if (ns2.Magnitude() < Eps) {
 200     if (!first) BlendFunc::ComputeNormal(surf2, pt2d, ns2);
 201     else {
 202       gp_Pnt2d P(X(3), X(4));
 203       BlendFunc::ComputeNormal(surf2, P, ns2);
 204     }
 205   }
 206
 207   Standard_Real norm1 = nplan.Crossed(ns1).Magnitude();
 208   Standard_Real norm2 = nplan.Crossed(ns2).Magnitude();
 209   if (norm1 < Eps)  {
 210     norm1 = 1;
 211 //#if DEB
 212 //    cout << " ConstRadInv : Surface singuliere " << endl;
 213 //#endif
 214   }
 215   if (norm2 < Eps)  {
 216     norm2 = 1; // Unsatisfactory, but it is not necessary to stop
 217 //#if DEB
 218 //    cout << " ConstRadInv : Surface singuliere " << endl;
 219 //#endif
 220   }
 221
 222   gp_Vec resul;
 223   ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1);
 224   ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2,ns2);
 225   resul.SetLinearForm(ray1,ns1,-1.,pts2.XYZ(),-ray2,ns2,pts1.XYZ());
 226   F(2) = resul.X();
 227   F(3) = resul.Y();
 228   F(4) = resul.Z();
 229
 230   return Standard_True;
 231 }
 232
 233
 234 Standard_Boolean BlendFunc_ConstRadInv::Derivatives(const math_Vector& X,
 235                                                     math_Matrix& D)
 236 {
 237   gp_Vec d1u1,d1v1,d1u2,d1v2;
 238   gp_Vec d2u1,d2v1,d2uv1,d2u2,d2v2,d2uv2;
 239   gp_Vec d1cur,d2cur;
 240   gp_Vec ns1,ns2,nplan,dnplan,ncrossns1,ncrossns2,resul1,resul2,temp;
 241   gp_Pnt pts1,pts2,ptcur;
 242   gp_Pnt2d p2d;
 243   gp_Vec2d v2d;
 244   Standard_Real norm1,norm2,ndotns1,ndotns2,normtgcur;
 245   Standard_Real grosterme,theD;
 246
 247   curv->D2(X(2),ptcur,d1cur,d2cur);
 248   normtgcur = d1cur.Magnitude();
 249   nplan = d1cur.Normalized();
 250   theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
 251
 252   dnplan.SetLinearForm(theD, nplan, d2cur);
 253   dnplan /= normtgcur;
 254
 255   csurf->D1(X(1),p2d,v2d);
 256   if (first)
 257   {
 258     surf1->D2(p2d.X(),p2d.Y(),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 259     surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 260     temp.SetLinearForm(v2d.X(),d1u1,v2d.Y(),d1v1);
 261     D(1,1) = nplan.Dot(temp)/2.;
 262     temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ()) - ptcur.XYZ());
 263     D(1,2) = dnplan.Dot(temp) - normtgcur;
 264     D(1,3) = nplan.Dot(d1u2)/2.;
 265     D(1,4) = nplan.Dot(d1v2)/2.;
 266   }
 267   else
 268   {
 269     surf1->D2(X(3),X(4),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 270     surf2->D2(p2d.X(),p2d.Y(),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 271     temp.SetLinearForm(v2d.X(),d1u2,v2d.Y(),d1v2);
 272     D(1,1) = nplan.Dot(temp)/2.;
 273     temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ()) - ptcur.XYZ());
 274     D(1,2) = dnplan.Dot(temp) - normtgcur;
 275     D(1,3) = nplan.Dot(d1u1)/2.;
 276     D(1,4) = nplan.Dot(d1v1)/2.;
 277   }
 278
 279   ns1 = d1u1.Crossed(d1v1);
 280   if (ns1.Magnitude() < Eps) {
 281    if (first) BlendFunc::ComputeNormal(surf1, p2d, ns1);
 282     else {
 283       gp_Pnt2d P(X(3), X(4));
 284       BlendFunc::ComputeNormal(surf1, P, ns1);
 285     }
 286   }
 287
 288   ns2 = d1u2.Crossed(d1v2);
 289   if (ns2.Magnitude() < Eps) {
 290    if (!first) BlendFunc::ComputeNormal(surf2, p2d, ns2);
 291     else {
 292       gp_Pnt2d P(X(3), X(4));
 293       BlendFunc::ComputeNormal(surf2, P, ns2);
 294     }
 295   }
 296
 297   ncrossns1 = nplan.Crossed(ns1);
 298   ncrossns2 = nplan.Crossed(ns2);
 299   norm1 = ncrossns1.Magnitude();
 300   norm2 = ncrossns2.Magnitude();
 301   if (norm1 < Eps)  {
 302     norm1 = 1; // Unsatisfactory, but it is not necessary to stop
 303 #if DEB
 304     cout << " ConstRadInv : Surface singuliere " << endl;
 305 #endif
 306   }
 307   if (norm2 < Eps)  {
 308     norm2 = 1; // Unsatisfactory, but it is not necessary to stop
 309 #if DEB
 310     cout << " ConstRadInv : Surface singuliere " << endl;
 311 #endif
 312   }
 313
 314   ndotns1 = nplan.Dot(ns1);
 315   ndotns2 = nplan.Dot(ns2);
 316
 317   // Derived compared to u1
 318
 319   temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
 320   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 321   resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
 322                        ray1*grosterme/norm1,ns1,
 323                        -ray1/norm1,temp,
 324                        d1u1);
 325
 326
 327   // Derived compared to v1
 328
 329   temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
 330   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 331   resul2.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
 332                        ray1*grosterme/norm1,ns1,
 333                        -ray1/norm1,temp,
 334                        d1v1);
 335
 336   if (first) {
 337     D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
 338     D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
 339     D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
 340   }
 341   else {
 342     D(2,3) = resul1.X();
 343     D(3,3) = resul1.Y();
 344     D(4,3) = resul1.Z();
 345
 346     D(2,4) = resul2.X();
 347     D(3,4) = resul2.Y();
 348     D(4,4) = resul2.Z();
 349   }
 350
 351
 352   // derived compared to w (parameter on guideline)
 353   // It is assumed that the radius is constant
 354
 355   grosterme = ncrossns1.Dot(dnplan.Crossed(ns1))/norm1/norm1;
 356   resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-dnplan.Dot(ns1)),nplan,
 357                        ray1*ndotns1/norm1,dnplan,
 358                        ray1*grosterme/norm1,ns1);
 359
 360
 361   grosterme = ncrossns2.Dot(dnplan.Crossed(ns2))/norm2/norm2;
 362   resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-dnplan.Dot(ns2)),nplan,
 363                        -ray2*ndotns2/norm2,dnplan,
 364                        -ray2*grosterme/norm2,ns2);
 365
 366
 367   D(2,2) = resul1.X() + resul2.X();
 368   D(3,2) = resul1.Y() + resul2.Y();
 369   D(4,2) = resul1.Z() + resul2.Z();
 370
 371
 372
 373   // Derived compared to u2
 374   temp = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
 375   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 376   resul1.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
 377                        -ray2*grosterme/norm2,ns2,
 378                        ray2/norm2,temp);
 379   resul1.Subtract(d1u2);
 380
 381   // Derived compared to v2
 382   temp = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
 383   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 384   resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
 385                        -ray2*grosterme/norm2,ns2,
 386                        ray2/norm2,temp);
 387   resul2.Subtract(d1v2);
 388
 389   if (!first) {
 390     D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
 391     D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
 392     D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
 393   }
 394   else {
 395     D(2,3) = resul1.X();
 396     D(3,3) = resul1.Y();
 397     D(4,3) = resul1.Z();
 398
 399     D(2,4) = resul2.X();
 400     D(3,4) = resul2.Y();
 401     D(4,4) = resul2.Z();
 402   }
 403
 404
 405   return Standard_True;
 406 }
 407
 408 Standard_Boolean BlendFunc_ConstRadInv::Values(const math_Vector& X,
 409                                                math_Vector& F,
 410                                                math_Matrix& D)
 411 {
 412   gp_Vec d1u1,d1v1,d1u2,d1v2,d1cur;
 413   gp_Vec d2u1,d2v1,d2uv1,d2u2,d2v2,d2uv2,d2cur;
 414   gp_Vec ns1,ns2,nplan,dnplan,ncrossns1,ncrossns2,resul1,resul2,temp;
 415   gp_Pnt ptcur,pts1,pts2;
 416   gp_Pnt2d p2d;
 417   gp_Vec2d v2d;
 418   Standard_Real norm1,norm2,ndotns1,ndotns2,normtgcur;
 419   Standard_Real grosterme,theD;
 420
 421   curv->D2(X(2),ptcur,d1cur,d2cur);
 422   normtgcur = d1cur.Magnitude();
 423   nplan = d1cur.Normalized();
 424   theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
 425   dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur);
 426   dnplan /= normtgcur;
 427
 428   csurf->D1(X(1),p2d,v2d);
 429
 430   if (first)
 431   {
 432     surf1->D2(p2d.X(),p2d.Y(),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 433     surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 434
 435     temp.SetLinearForm(v2d.X(),d1u1,v2d.Y(),d1v1);
 436     D(1,1) = nplan.Dot(temp)/2.;
 437     temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ())-ptcur.XYZ());
 438     D(1,2) = dnplan.Dot(temp) - normtgcur;
 439     D(1,3) = nplan.Dot(d1u2)/2.;
 440     D(1,4) = nplan.Dot(d1v2)/2.;
 441   }
 442   else
 443   {
 444     surf1->D2(X(3),X(4),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 445     surf2->D2(p2d.X(),p2d.Y(),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 446
 447     temp.SetLinearForm(v2d.X(),d1u2,v2d.Y(),d1v2);
 448     D(1,1) = nplan.Dot(temp)/2.;
 449     temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ())-ptcur.XYZ());
 450     D(1,2) = dnplan.Dot(temp) - normtgcur;
 451     D(1,3) = nplan.Dot(d1u1)/2.;
 452     D(1,4) = nplan.Dot(d1v1)/2.;
 453   }
 454
 455   F(1) = (nplan.X()* (pts1.X() + pts2.X()) +
 456           nplan.Y()* (pts1.Y() + pts2.Y()) +
 457           nplan.Z()* (pts1.Z() + pts2.Z())) /2. + theD;
 458
 459
 460   ns1 = d1u1.Crossed(d1v1);
 461   if (ns1.Magnitude() < Eps) {
 462     if (first) BlendFunc::ComputeNormal(surf1, p2d, ns1);
 463     else {
 464       gp_Pnt2d P(X(3), X(4));
 465       BlendFunc::ComputeNormal(surf1, P, ns1);
 466     }
 467   }
 468
 469   ns2 = d1u2.Crossed(d1v2);
 470   if (ns2.Magnitude() < Eps) {
 471     if (!first) BlendFunc::ComputeNormal(surf2, p2d, ns2);
 472     else {
 473       gp_Pnt2d P(X(3), X(4));
 474       BlendFunc::ComputeNormal(surf2, P, ns2);
 475     }
 476   }
 477
 478   ncrossns1 = nplan.Crossed(ns1);
 479   ncrossns2 = nplan.Crossed(ns2);
 480   norm1 = ncrossns1.Magnitude();
 481   norm2 = ncrossns2.Magnitude();
 482   if (norm1 < Eps)  {
 483     norm1 = 1; // Unsatisfactory, but it is not necessary to stop
 484 #if DEB
 485     cout << " ConstRadInv : Surface singuliere " << endl;
 486 #endif
 487   }
 488   if (norm2 < Eps)  {
 489     norm2 = 1; // Unsatisfactory, but it is not necessary to stop
 490 #if DEB
 491     cout << " ConstRadInv : Surface singuliere " << endl;
 492 #endif
 493   }
 494
 495   ndotns1 = nplan.Dot(ns1);
 496   ndotns2 = nplan.Dot(ns2);
 497
 498   temp.SetLinearForm(ndotns1/norm1,nplan, -1./norm1,ns1);
 499   resul1.SetLinearForm(ray1,temp,gp_Vec(pts2,pts1));
 500   temp.SetLinearForm(ndotns2/norm2,nplan,-1./norm2,ns2);
 501   resul1.Subtract(ray2*temp);
 502
 503   F(2) = resul1.X();
 504   F(3) = resul1.Y();
 505   F(4) = resul1.Z();
 506
 507   // Derived compared to u1
 508
 509   temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
 510   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 511   resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
 512                        ray1*grosterme/norm1,ns1,
 513                        -ray1/norm1,temp,
 514                        d1u1);
 515
 516
 517   // Derived compared to v1
 518
 519   temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
 520   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 521   resul2.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
 522                        ray1*grosterme/norm1,ns1,
 523                        -ray1/norm1,temp,
 524                        d1v1);
 525
 526   if (first) {
 527     D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
 528     D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
 529     D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
 530   }
 531   else {
 532     D(2,3) = resul1.X();
 533     D(3,3) = resul1.Y();
 534     D(4,3) = resul1.Z();
 535
 536     D(2,4) = resul2.X();
 537     D(3,4) = resul2.Y();
 538     D(4,4) = resul2.Z();
 539   }
 540
 541   // derived compared to w (parameter on guideline)
 542   // It is assumed that the raduis is constant
 543
 544   grosterme = ncrossns1.Dot(dnplan.Crossed(ns1))/norm1/norm1;
 545   resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-dnplan.Dot(ns1)),nplan,
 546                        ray1*ndotns1/norm1,dnplan,
 547                        ray1*grosterme/norm1,ns1);
 548
 549
 550   grosterme = ncrossns2.Dot(dnplan.Crossed(ns2))/norm2/norm2;
 551   resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-dnplan.Dot(ns2)),nplan,
 552                        -ray2*ndotns2/norm2,dnplan,
 553                        -ray2*grosterme/norm2,ns2);
 554
 555
 556   D(2,2) = resul1.X() + resul2.X();
 557   D(3,2) = resul1.Y() + resul2.Y();
 558   D(4,2) = resul1.Z() + resul2.Z();
 559
 560
 561
 562   // Derived compared to u2
 563   temp = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
 564   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 565   resul1.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
 566                        -ray2*grosterme/norm2,ns2,
 567                        ray2/norm2,temp);
 568   resul1.Subtract(d1u2);
 569
 570   // Derived compared to v2
 571   temp = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
 572   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 573   resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
 574                        -ray2*grosterme/norm2,ns2,
 575                        ray2/norm2,temp);
 576   resul2.Subtract(d1v2);
 577
 578   if (!first) {
 579     D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
 580     D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
 581     D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
 582   }
 583   else {
 584     D(2,3) = resul1.X();
 585     D(3,3) = resul1.Y();
 586     D(4,3) = resul1.Z();
 587
 588     D(2,4) = resul2.X();
 589     D(3,4) = resul2.Y();
 590     D(4,4) = resul2.Z();
 591   }
 592   return Standard_True;
 593 }