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   }
 212   if (norm2 < Eps)  {
 213     norm2 = 1; // Unsatisfactory, but it is not necessary to stop
 214   }
 215
 216   gp_Vec resul;
 217   ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1);
 218   ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2,ns2);
 219   resul.SetLinearForm(ray1,ns1,-1.,pts2.XYZ(),-ray2,ns2,pts1.XYZ());
 220   F(2) = resul.X();
 221   F(3) = resul.Y();
 222   F(4) = resul.Z();
 223
 224   return Standard_True;
 225 }
 226
 227
 228 Standard_Boolean BlendFunc_ConstRadInv::Derivatives(const math_Vector& X,
 229                                                     math_Matrix& D)
 230 {
 231   gp_Vec d1u1,d1v1,d1u2,d1v2;
 232   gp_Vec d2u1,d2v1,d2uv1,d2u2,d2v2,d2uv2;
 233   gp_Vec d1cur,d2cur;
 234   gp_Vec ns1,ns2,nplan,dnplan,ncrossns1,ncrossns2,resul1,resul2,temp;
 235   gp_Pnt pts1,pts2,ptcur;
 236   gp_Pnt2d p2d;
 237   gp_Vec2d v2d;
 238   Standard_Real norm1,norm2,ndotns1,ndotns2,normtgcur;
 239   Standard_Real grosterme,theD;
 240
 241   curv->D2(X(2),ptcur,d1cur,d2cur);
 242   normtgcur = d1cur.Magnitude();
 243   nplan = d1cur.Normalized();
 244   theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
 245
 246   dnplan.SetLinearForm(theD, nplan, d2cur);
 247   dnplan /= normtgcur;
 248
 249   csurf->D1(X(1),p2d,v2d);
 250   if (first)
 251   {
 252     surf1->D2(p2d.X(),p2d.Y(),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 253     surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 254     temp.SetLinearForm(v2d.X(),d1u1,v2d.Y(),d1v1);
 255     D(1,1) = nplan.Dot(temp)/2.;
 256     temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ()) - ptcur.XYZ());
 257     D(1,2) = dnplan.Dot(temp) - normtgcur;
 258     D(1,3) = nplan.Dot(d1u2)/2.;
 259     D(1,4) = nplan.Dot(d1v2)/2.;
 260   }
 261   else
 262   {
 263     surf1->D2(X(3),X(4),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 264     surf2->D2(p2d.X(),p2d.Y(),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 265     temp.SetLinearForm(v2d.X(),d1u2,v2d.Y(),d1v2);
 266     D(1,1) = nplan.Dot(temp)/2.;
 267     temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ()) - ptcur.XYZ());
 268     D(1,2) = dnplan.Dot(temp) - normtgcur;
 269     D(1,3) = nplan.Dot(d1u1)/2.;
 270     D(1,4) = nplan.Dot(d1v1)/2.;
 271   }
 272
 273   ns1 = d1u1.Crossed(d1v1);
 274   if (ns1.Magnitude() < Eps) {
 275    if (first) BlendFunc::ComputeNormal(surf1, p2d, ns1);
 276     else {
 277       gp_Pnt2d P(X(3), X(4));
 278       BlendFunc::ComputeNormal(surf1, P, ns1);
 279     }
 280   }
 281
 282   ns2 = d1u2.Crossed(d1v2);
 283   if (ns2.Magnitude() < Eps) {
 284    if (!first) BlendFunc::ComputeNormal(surf2, p2d, ns2);
 285     else {
 286       gp_Pnt2d P(X(3), X(4));
 287       BlendFunc::ComputeNormal(surf2, P, ns2);
 288     }
 289   }
 290
 291   ncrossns1 = nplan.Crossed(ns1);
 292   ncrossns2 = nplan.Crossed(ns2);
 293   norm1 = ncrossns1.Magnitude();
 294   norm2 = ncrossns2.Magnitude();
 295   if (norm1 < Eps)  {
 296     norm1 = 1; // Unsatisfactory, but it is not necessary to stop
 297 #ifdef OCCT_DEBUG
 298     cout << " ConstRadInv : Surface singuliere " << endl;
 299 #endif
 300   }
 301   if (norm2 < Eps)  {
 302     norm2 = 1; // Unsatisfactory, but it is not necessary to stop
 303 #ifdef OCCT_DEBUG
 304     cout << " ConstRadInv : Surface singuliere " << endl;
 305 #endif
 306   }
 307
 308   ndotns1 = nplan.Dot(ns1);
 309   ndotns2 = nplan.Dot(ns2);
 310
 311   // Derived compared to u1
 312
 313   temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
 314   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 315   resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
 316                        ray1*grosterme/norm1,ns1,
 317                        -ray1/norm1,temp,
 318                        d1u1);
 319
 320
 321   // Derived compared to v1
 322
 323   temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
 324   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 325   resul2.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
 326                        ray1*grosterme/norm1,ns1,
 327                        -ray1/norm1,temp,
 328                        d1v1);
 329
 330   if (first) {
 331     D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
 332     D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
 333     D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
 334   }
 335   else {
 336     D(2,3) = resul1.X();
 337     D(3,3) = resul1.Y();
 338     D(4,3) = resul1.Z();
 339
 340     D(2,4) = resul2.X();
 341     D(3,4) = resul2.Y();
 342     D(4,4) = resul2.Z();
 343   }
 344
 345
 346   // derived compared to w (parameter on guideline)
 347   // It is assumed that the radius is constant
 348
 349   grosterme = ncrossns1.Dot(dnplan.Crossed(ns1))/norm1/norm1;
 350   resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-dnplan.Dot(ns1)),nplan,
 351                        ray1*ndotns1/norm1,dnplan,
 352                        ray1*grosterme/norm1,ns1);
 353
 354
 355   grosterme = ncrossns2.Dot(dnplan.Crossed(ns2))/norm2/norm2;
 356   resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-dnplan.Dot(ns2)),nplan,
 357                        -ray2*ndotns2/norm2,dnplan,
 358                        -ray2*grosterme/norm2,ns2);
 359
 360
 361   D(2,2) = resul1.X() + resul2.X();
 362   D(3,2) = resul1.Y() + resul2.Y();
 363   D(4,2) = resul1.Z() + resul2.Z();
 364
 365
 366
 367   // Derived compared to u2
 368   temp = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
 369   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 370   resul1.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
 371                        -ray2*grosterme/norm2,ns2,
 372                        ray2/norm2,temp);
 373   resul1.Subtract(d1u2);
 374
 375   // Derived compared to v2
 376   temp = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
 377   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 378   resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
 379                        -ray2*grosterme/norm2,ns2,
 380                        ray2/norm2,temp);
 381   resul2.Subtract(d1v2);
 382
 383   if (!first) {
 384     D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
 385     D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
 386     D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
 387   }
 388   else {
 389     D(2,3) = resul1.X();
 390     D(3,3) = resul1.Y();
 391     D(4,3) = resul1.Z();
 392
 393     D(2,4) = resul2.X();
 394     D(3,4) = resul2.Y();
 395     D(4,4) = resul2.Z();
 396   }
 397
 398
 399   return Standard_True;
 400 }
 401
 402 Standard_Boolean BlendFunc_ConstRadInv::Values(const math_Vector& X,
 403                                                math_Vector& F,
 404                                                math_Matrix& D)
 405 {
 406   gp_Vec d1u1,d1v1,d1u2,d1v2,d1cur;
 407   gp_Vec d2u1,d2v1,d2uv1,d2u2,d2v2,d2uv2,d2cur;
 408   gp_Vec ns1,ns2,nplan,dnplan,ncrossns1,ncrossns2,resul1,resul2,temp;
 409   gp_Pnt ptcur,pts1,pts2;
 410   gp_Pnt2d p2d;
 411   gp_Vec2d v2d;
 412   Standard_Real norm1,norm2,ndotns1,ndotns2,normtgcur;
 413   Standard_Real grosterme,theD;
 414
 415   curv->D2(X(2),ptcur,d1cur,d2cur);
 416   normtgcur = d1cur.Magnitude();
 417   nplan = d1cur.Normalized();
 418   theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
 419   dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur);
 420   dnplan /= normtgcur;
 421
 422   csurf->D1(X(1),p2d,v2d);
 423
 424   if (first)
 425   {
 426     surf1->D2(p2d.X(),p2d.Y(),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 427     surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 428
 429     temp.SetLinearForm(v2d.X(),d1u1,v2d.Y(),d1v1);
 430     D(1,1) = nplan.Dot(temp)/2.;
 431     temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ())-ptcur.XYZ());
 432     D(1,2) = dnplan.Dot(temp) - normtgcur;
 433     D(1,3) = nplan.Dot(d1u2)/2.;
 434     D(1,4) = nplan.Dot(d1v2)/2.;
 435   }
 436   else
 437   {
 438     surf1->D2(X(3),X(4),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
 439     surf2->D2(p2d.X(),p2d.Y(),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
 440
 441     temp.SetLinearForm(v2d.X(),d1u2,v2d.Y(),d1v2);
 442     D(1,1) = nplan.Dot(temp)/2.;
 443     temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ())-ptcur.XYZ());
 444     D(1,2) = dnplan.Dot(temp) - normtgcur;
 445     D(1,3) = nplan.Dot(d1u1)/2.;
 446     D(1,4) = nplan.Dot(d1v1)/2.;
 447   }
 448
 449   F(1) = (nplan.X()* (pts1.X() + pts2.X()) +
 450           nplan.Y()* (pts1.Y() + pts2.Y()) +
 451           nplan.Z()* (pts1.Z() + pts2.Z())) /2. + theD;
 452
 453
 454   ns1 = d1u1.Crossed(d1v1);
 455   if (ns1.Magnitude() < Eps) {
 456     if (first) BlendFunc::ComputeNormal(surf1, p2d, ns1);
 457     else {
 458       gp_Pnt2d P(X(3), X(4));
 459       BlendFunc::ComputeNormal(surf1, P, ns1);
 460     }
 461   }
 462
 463   ns2 = d1u2.Crossed(d1v2);
 464   if (ns2.Magnitude() < Eps) {
 465     if (!first) BlendFunc::ComputeNormal(surf2, p2d, ns2);
 466     else {
 467       gp_Pnt2d P(X(3), X(4));
 468       BlendFunc::ComputeNormal(surf2, P, ns2);
 469     }
 470   }
 471
 472   ncrossns1 = nplan.Crossed(ns1);
 473   ncrossns2 = nplan.Crossed(ns2);
 474   norm1 = ncrossns1.Magnitude();
 475   norm2 = ncrossns2.Magnitude();
 476   if (norm1 < Eps)  {
 477     norm1 = 1; // Unsatisfactory, but it is not necessary to stop
 478 #ifdef OCCT_DEBUG
 479     cout << " ConstRadInv : Surface singuliere " << endl;
 480 #endif
 481   }
 482   if (norm2 < Eps)  {
 483     norm2 = 1; // Unsatisfactory, but it is not necessary to stop
 484 #ifdef OCCT_DEBUG
 485     cout << " ConstRadInv : Surface singuliere " << endl;
 486 #endif
 487   }
 488
 489   ndotns1 = nplan.Dot(ns1);
 490   ndotns2 = nplan.Dot(ns2);
 491
 492   temp.SetLinearForm(ndotns1/norm1,nplan, -1./norm1,ns1);
 493   resul1.SetLinearForm(ray1,temp,gp_Vec(pts2,pts1));
 494   temp.SetLinearForm(ndotns2/norm2,nplan,-1./norm2,ns2);
 495   resul1.Subtract(ray2*temp);
 496
 497   F(2) = resul1.X();
 498   F(3) = resul1.Y();
 499   F(4) = resul1.Z();
 500
 501   // Derived compared to u1
 502
 503   temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
 504   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 505   resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
 506                        ray1*grosterme/norm1,ns1,
 507                        -ray1/norm1,temp,
 508                        d1u1);
 509
 510
 511   // Derived compared to v1
 512
 513   temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
 514   grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
 515   resul2.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
 516                        ray1*grosterme/norm1,ns1,
 517                        -ray1/norm1,temp,
 518                        d1v1);
 519
 520   if (first) {
 521     D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
 522     D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
 523     D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
 524   }
 525   else {
 526     D(2,3) = resul1.X();
 527     D(3,3) = resul1.Y();
 528     D(4,3) = resul1.Z();
 529
 530     D(2,4) = resul2.X();
 531     D(3,4) = resul2.Y();
 532     D(4,4) = resul2.Z();
 533   }
 534
 535   // derived compared to w (parameter on guideline)
 536   // It is assumed that the raduis is constant
 537
 538   grosterme = ncrossns1.Dot(dnplan.Crossed(ns1))/norm1/norm1;
 539   resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-dnplan.Dot(ns1)),nplan,
 540                        ray1*ndotns1/norm1,dnplan,
 541                        ray1*grosterme/norm1,ns1);
 542
 543
 544   grosterme = ncrossns2.Dot(dnplan.Crossed(ns2))/norm2/norm2;
 545   resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-dnplan.Dot(ns2)),nplan,
 546                        -ray2*ndotns2/norm2,dnplan,
 547                        -ray2*grosterme/norm2,ns2);
 548
 549
 550   D(2,2) = resul1.X() + resul2.X();
 551   D(3,2) = resul1.Y() + resul2.Y();
 552   D(4,2) = resul1.Z() + resul2.Z();
 553
 554
 555
 556   // Derived compared to u2
 557   temp = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
 558   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 559   resul1.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
 560                        -ray2*grosterme/norm2,ns2,
 561                        ray2/norm2,temp);
 562   resul1.Subtract(d1u2);
 563
 564   // Derived compared to v2
 565   temp = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
 566   grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
 567   resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
 568                        -ray2*grosterme/norm2,ns2,
 569                        ray2/norm2,temp);
 570   resul2.Subtract(d1v2);
 571
 572   if (!first) {
 573     D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
 574     D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
 575     D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
 576   }
 577   else {
 578     D(2,3) = resul1.X();
 579     D(3,3) = resul1.Y();
 580     D(4,3) = resul1.Z();
 581
 582     D(2,4) = resul2.X();
 583     D(3,4) = resul2.Y();
 584     D(4,4) = resul2.Z();
 585   }
 586   return Standard_True;
 587 }