[occt.git] / src / BlendFunc / BlendFunc_ConstRadInv.cxx

// Created on: 1993-12-02
// Created by: Jacques GOUSSARD
// Copyright (c) 1993-1999 Matra Datavision
// Copyright (c) 1999-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.


#include <Adaptor2d_Curve2d.hxx>
#include <Adaptor3d_Curve.hxx>
#include <Adaptor3d_Surface.hxx>
#include <BlendFunc.hxx>
#include <BlendFunc_ConstRadInv.hxx>
#include <math_Matrix.hxx>
#include <Precision.hxx>

#define Eps 1.e-15


BlendFunc_ConstRadInv::BlendFunc_ConstRadInv(const Handle(Adaptor3d_Surface)& S1,
					     const Handle(Adaptor3d_Surface)& S2,
					     const Handle(Adaptor3d_Curve)& C)
: surf1(S1),
  surf2(S2),
  curv(C),
  ray1(0.0),
  ray2(0.0),
  choix(0),
  first(Standard_False)
{
}

void BlendFunc_ConstRadInv::Set(const Standard_Real R,
				const Standard_Integer Choix)

{
  choix = Choix;
  switch (choix) {
  case 1:
  case 2:
    {
      ray1 = -R;
      ray2 = -R;
    }
    break;
  case 3:
  case 4:
    {
      ray1 = R;
      ray2 = -R;
    }
    break;
  case 5:
  case 6:
    {
      ray1 = R;
      ray2 = R;
    }
    break;
  case 7:
  case 8:
    {
      ray1 = -R;
      ray2 = R;
    }
    break;
  default:
    ray1 = ray2 = -R;
  }
}

void BlendFunc_ConstRadInv::Set(const Standard_Boolean OnFirst,
				const Handle(Adaptor2d_Curve2d)& C)
{
  first = OnFirst;
  csurf = C;
}

Standard_Integer BlendFunc_ConstRadInv::NbEquations () const
{
  return 4;
}


void BlendFunc_ConstRadInv::GetTolerance(math_Vector& Tolerance,
					 const Standard_Real Tol) const
{
  Tolerance(1) = csurf->Resolution(Tol);
  Tolerance(2) = curv->Resolution(Tol);
  if (first) {
    Tolerance(3) = surf2->UResolution(Tol);
    Tolerance(4) = surf2->VResolution(Tol);
  }
  else {
    Tolerance(3) = surf1->UResolution(Tol);
    Tolerance(4) = surf1->VResolution(Tol);
  }
}


void BlendFunc_ConstRadInv::GetBounds(math_Vector& InfBound,
				      math_Vector& SupBound) const
{
  InfBound(1) = csurf->FirstParameter();
  InfBound(2) = curv->FirstParameter();
  SupBound(1) = csurf->LastParameter();
  SupBound(2) = curv->LastParameter();

  if (first) {
    InfBound(3) = surf2->FirstUParameter();
    InfBound(4) = surf2->FirstVParameter();
    SupBound(3) = surf2->LastUParameter();
    SupBound(4) = surf2->LastVParameter();
    if(!Precision::IsInfinite(InfBound(3)) &&
       !Precision::IsInfinite(SupBound(3))) {
      Standard_Real range = (SupBound(3) - InfBound(3));
      InfBound(3) -= range;
      SupBound(3) += range;
    }
    if(!Precision::IsInfinite(InfBound(4)) &&
       !Precision::IsInfinite(SupBound(4))) {
      Standard_Real range = (SupBound(4) - InfBound(4));
      InfBound(4) -= range;
      SupBound(4) += range;
    }
  }
  else {
    InfBound(3) = surf1->FirstUParameter();
    InfBound(4) = surf1->FirstVParameter();
    SupBound(3) = surf1->LastUParameter();
    SupBound(4) = surf1->LastVParameter();
    if(!Precision::IsInfinite(InfBound(3)) &&
       !Precision::IsInfinite(SupBound(3))) {
      Standard_Real range = (SupBound(3) - InfBound(3));
      InfBound(3) -= range;
      SupBound(3) += range;
    }
    if(!Precision::IsInfinite(InfBound(4)) &&
       !Precision::IsInfinite(SupBound(4))) {
      Standard_Real range = (SupBound(4) - InfBound(4));
      InfBound(4) -= range;
      SupBound(4) += range;
    }
  }    
}

Standard_Boolean BlendFunc_ConstRadInv::IsSolution(const math_Vector& Sol,
						   const Standard_Real Tol)
{
  math_Vector valsol(1,4);
  Value(Sol,valsol);
  if (Abs(valsol(1)) <= Tol &&  
      valsol(2)*valsol(2) + valsol(3)*valsol(3) +
      valsol(4)*valsol(4) <= Tol*Tol) {
    return Standard_True;
  }
  return Standard_False;

}


Standard_Boolean BlendFunc_ConstRadInv::Value(const math_Vector& X,
                                              math_Vector& F)
{
  gp_Pnt ptcur;
  gp_Vec d1cur;
  curv->D1(X(2),ptcur,d1cur);

  const gp_Vec nplan = d1cur.Normalized();
  const Standard_Real theD = -(nplan.XYZ().Dot(ptcur.XYZ()));

  const gp_Pnt2d pt2d(csurf->Value(X(1)));

  gp_Pnt pts1,pts2;
  gp_Vec d1u1,d1v1,d1u2,d1v2;
  if (first)
  {
    surf1->D1(pt2d.X(),pt2d.Y(),pts1,d1u1,d1v1);
    surf2->D1(X(3),X(4),pts2,d1u2,d1v2);
  }
  else
  {
    surf1->D1(X(3),X(4),pts1,d1u1,d1v1);
    surf2->D1(pt2d.X(),pt2d.Y(),pts2,d1u2,d1v2);
  }

  F(1) = (nplan.X() * (pts1.X() + pts2.X()) + 
	  nplan.Y() * (pts1.Y() + pts2.Y()) + 
	  nplan.Z() * (pts1.Z() + pts2.Z())) /2. + theD;

  gp_Vec ns1 = d1u1.Crossed(d1v1);
  if (ns1.Magnitude() < Eps) {
    if (first) BlendFunc::ComputeNormal(surf1, pt2d, ns1);
    else {
      gp_Pnt2d P(X(3), X(4));
      BlendFunc::ComputeNormal(surf1, P, ns1);
    }
  }

  gp_Vec ns2 = d1u2.Crossed(d1v2);
  if (ns2.Magnitude() < Eps) {
    if (!first) BlendFunc::ComputeNormal(surf2, pt2d, ns2);
    else {
      gp_Pnt2d P(X(3), X(4));
      BlendFunc::ComputeNormal(surf2, P, ns2);
    }
  }

  Standard_Real norm1 = nplan.Crossed(ns1).Magnitude();
  Standard_Real norm2 = nplan.Crossed(ns2).Magnitude();
  if (norm1 < Eps)  {
    norm1 = 1; 
  }
  if (norm2 < Eps)  {
    norm2 = 1; // Unsatisfactory, but it is not necessary to stop
  }  

  gp_Vec resul;
  ns1.SetLinearForm(nplan.Dot(ns1)/norm1,nplan, -1./norm1,ns1);
  ns2.SetLinearForm(nplan.Dot(ns2)/norm2,nplan, -1./norm2,ns2);
  resul.SetLinearForm(ray1,ns1,-1.,pts2.XYZ(),-ray2,ns2,pts1.XYZ());
  F(2) = resul.X();
  F(3) = resul.Y();
  F(4) = resul.Z();

  return Standard_True;
}


Standard_Boolean BlendFunc_ConstRadInv::Derivatives(const math_Vector& X,
                                                    math_Matrix& D)
{
  gp_Vec d1u1,d1v1,d1u2,d1v2;
  gp_Vec d2u1,d2v1,d2uv1,d2u2,d2v2,d2uv2;
  gp_Vec d1cur,d2cur;
  gp_Vec ns1,ns2,nplan,dnplan,ncrossns1,ncrossns2,resul1,resul2,temp;
  gp_Pnt pts1,pts2,ptcur;
  gp_Pnt2d p2d;
  gp_Vec2d v2d;
  Standard_Real norm1,norm2,ndotns1,ndotns2,normtgcur;
  Standard_Real grosterme,theD;

  curv->D2(X(2),ptcur,d1cur,d2cur);
  normtgcur = d1cur.Magnitude();
  nplan = d1cur.Normalized();
  theD = -(nplan.XYZ().Dot(ptcur.XYZ()));

  dnplan.SetLinearForm(theD, nplan, d2cur);
  dnplan /= normtgcur;

  csurf->D1(X(1),p2d,v2d);
  if (first)
  {
    surf1->D2(p2d.X(),p2d.Y(),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
    surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
    temp.SetLinearForm(v2d.X(),d1u1,v2d.Y(),d1v1);
    D(1,1) = nplan.Dot(temp)/2.;
    temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ()) - ptcur.XYZ());
    D(1,2) = dnplan.Dot(temp) - normtgcur;
    D(1,3) = nplan.Dot(d1u2)/2.;
    D(1,4) = nplan.Dot(d1v2)/2.;
  }
  else
  {
    surf1->D2(X(3),X(4),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
    surf2->D2(p2d.X(),p2d.Y(),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);
    temp.SetLinearForm(v2d.X(),d1u2,v2d.Y(),d1v2);
    D(1,1) = nplan.Dot(temp)/2.;
    temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ()) - ptcur.XYZ());
    D(1,2) = dnplan.Dot(temp) - normtgcur;
    D(1,3) = nplan.Dot(d1u1)/2.;
    D(1,4) = nplan.Dot(d1v1)/2.;
  }

  ns1 = d1u1.Crossed(d1v1);
  if (ns1.Magnitude() < Eps) {
   if (first) BlendFunc::ComputeNormal(surf1, p2d, ns1);
    else {
      gp_Pnt2d P(X(3), X(4));
      BlendFunc::ComputeNormal(surf1, P, ns1);
    }
  }

  ns2 = d1u2.Crossed(d1v2);
  if (ns2.Magnitude() < Eps) {
   if (!first) BlendFunc::ComputeNormal(surf2, p2d, ns2);
    else {
      gp_Pnt2d P(X(3), X(4));
      BlendFunc::ComputeNormal(surf2, P, ns2);
    }
  }

  ncrossns1 = nplan.Crossed(ns1);
  ncrossns2 = nplan.Crossed(ns2);
  norm1 = ncrossns1.Magnitude();
  norm2 = ncrossns2.Magnitude();
  if (norm1 < Eps)  {
    norm1 = 1; // Unsatisfactory, but it is not necessary to stop
#ifdef OCCT_DEBUG
    std::cout << " ConstRadInv : Surface singuliere " << std::endl;
#endif
  }
  if (norm2 < Eps)  {
    norm2 = 1; // Unsatisfactory, but it is not necessary to stop
#ifdef OCCT_DEBUG
    std::cout << " ConstRadInv : Surface singuliere " << std::endl;
#endif
  } 

  ndotns1 = nplan.Dot(ns1);
  ndotns2 = nplan.Dot(ns2);

  // Derived compared to u1

  temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
  grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
  resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
		       ray1*grosterme/norm1,ns1,
		       -ray1/norm1,temp,
		       d1u1);


  // Derived compared to v1

  temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
  grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
  resul2.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
		       ray1*grosterme/norm1,ns1,
		       -ray1/norm1,temp,
		       d1v1);

  if (first) {
    D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
    D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
    D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
  }
  else {
    D(2,3) = resul1.X();
    D(3,3) = resul1.Y();
    D(4,3) = resul1.Z();

    D(2,4) = resul2.X();
    D(3,4) = resul2.Y();
    D(4,4) = resul2.Z();
  }


  // derived compared to w (parameter on guideline)
  // It is assumed that the radius is constant

  grosterme = ncrossns1.Dot(dnplan.Crossed(ns1))/norm1/norm1;
  resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-dnplan.Dot(ns1)),nplan,
		       ray1*ndotns1/norm1,dnplan,
		       ray1*grosterme/norm1,ns1);


  grosterme = ncrossns2.Dot(dnplan.Crossed(ns2))/norm2/norm2;
  resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-dnplan.Dot(ns2)),nplan,
		       -ray2*ndotns2/norm2,dnplan,
		       -ray2*grosterme/norm2,ns2);


  D(2,2) = resul1.X() + resul2.X();
  D(3,2) = resul1.Y() + resul2.Y();
  D(4,2) = resul1.Z() + resul2.Z();


  // Derived compared to u2
  temp = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
  grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
  resul1.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
		       -ray2*grosterme/norm2,ns2,
		       ray2/norm2,temp);
  resul1.Subtract(d1u2);

  // Derived compared to v2
  temp = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
  grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
  resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
		       -ray2*grosterme/norm2,ns2,
		       ray2/norm2,temp);
  resul2.Subtract(d1v2);

  if (!first) {
    D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
    D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
    D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
  }
  else {
    D(2,3) = resul1.X();
    D(3,3) = resul1.Y();
    D(4,3) = resul1.Z();

    D(2,4) = resul2.X();
    D(3,4) = resul2.Y();
    D(4,4) = resul2.Z();
  }


  return Standard_True;
}

Standard_Boolean BlendFunc_ConstRadInv::Values(const math_Vector& X,
					       math_Vector& F,
					       math_Matrix& D)
{
  gp_Vec d1u1,d1v1,d1u2,d1v2,d1cur;
  gp_Vec d2u1,d2v1,d2uv1,d2u2,d2v2,d2uv2,d2cur;
  gp_Vec ns1,ns2,nplan,dnplan,ncrossns1,ncrossns2,resul1,resul2,temp;
  gp_Pnt ptcur,pts1,pts2;
  gp_Pnt2d p2d;
  gp_Vec2d v2d;
  Standard_Real norm1,norm2,ndotns1,ndotns2,normtgcur;
  Standard_Real grosterme,theD;

  curv->D2(X(2),ptcur,d1cur,d2cur);
  normtgcur = d1cur.Magnitude();
  nplan = d1cur.Normalized();
  theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
  dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur);
  dnplan /= normtgcur;

  csurf->D1(X(1),p2d,v2d);

  if (first)
  {
    surf1->D2(p2d.X(),p2d.Y(),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
    surf2->D2(X(3),X(4),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);

    temp.SetLinearForm(v2d.X(),d1u1,v2d.Y(),d1v1);
    D(1,1) = nplan.Dot(temp)/2.;
    temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ())-ptcur.XYZ());
    D(1,2) = dnplan.Dot(temp) - normtgcur;
    D(1,3) = nplan.Dot(d1u2)/2.;
    D(1,4) = nplan.Dot(d1v2)/2.;
  }
  else
  {
    surf1->D2(X(3),X(4),pts1,d1u1,d1v1,d2u1,d2v1,d2uv1);
    surf2->D2(p2d.X(),p2d.Y(),pts2,d1u2,d1v2,d2u2,d2v2,d2uv2);

    temp.SetLinearForm(v2d.X(),d1u2,v2d.Y(),d1v2);
    D(1,1) = nplan.Dot(temp)/2.;
    temp.SetXYZ(0.5*(pts1.XYZ()+pts2.XYZ())-ptcur.XYZ());
    D(1,2) = dnplan.Dot(temp) - normtgcur;
    D(1,3) = nplan.Dot(d1u1)/2.;
    D(1,4) = nplan.Dot(d1v1)/2.;
  }

  F(1) = (nplan.X()* (pts1.X() + pts2.X()) + 
	  nplan.Y()* (pts1.Y() + pts2.Y()) + 
	  nplan.Z()* (pts1.Z() + pts2.Z())) /2. + theD;


  ns1 = d1u1.Crossed(d1v1);
  if (ns1.Magnitude() < Eps) {
    if (first) BlendFunc::ComputeNormal(surf1, p2d, ns1);
    else {
      gp_Pnt2d P(X(3), X(4));
      BlendFunc::ComputeNormal(surf1, P, ns1);
    }
  }

  ns2 = d1u2.Crossed(d1v2);
  if (ns2.Magnitude() < Eps) {
    if (!first) BlendFunc::ComputeNormal(surf2, p2d, ns2);
    else {
      gp_Pnt2d P(X(3), X(4));
      BlendFunc::ComputeNormal(surf2, P, ns2);
    }
  }

  ncrossns1 = nplan.Crossed(ns1);
  ncrossns2 = nplan.Crossed(ns2);
  norm1 = ncrossns1.Magnitude();
  norm2 = ncrossns2.Magnitude();
  if (norm1 < Eps)  {
    norm1 = 1; // Unsatisfactory, but it is not necessary to stop
#ifdef OCCT_DEBUG
    std::cout << " ConstRadInv : Surface singuliere " << std::endl;
#endif
  }
  if (norm2 < Eps)  {
    norm2 = 1; // Unsatisfactory, but it is not necessary to stop
#ifdef OCCT_DEBUG
    std::cout << " ConstRadInv : Surface singuliere " << std::endl;
#endif
  } 

  ndotns1 = nplan.Dot(ns1);
  ndotns2 = nplan.Dot(ns2);

  temp.SetLinearForm(ndotns1/norm1,nplan, -1./norm1,ns1);
  resul1.SetLinearForm(ray1,temp,gp_Vec(pts2,pts1));
  temp.SetLinearForm(ndotns2/norm2,nplan,-1./norm2,ns2);
  resul1.Subtract(ray2*temp);

  F(2) = resul1.X();
  F(3) = resul1.Y();
  F(4) = resul1.Z();

  // Derived compared to u1

  temp = d2u1.Crossed(d1v1).Added(d1u1.Crossed(d2uv1));
  grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
  resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
		       ray1*grosterme/norm1,ns1,
		       -ray1/norm1,temp,
		       d1u1);


  // Derived compared to v1

  temp = d2uv1.Crossed(d1v1).Added(d1u1.Crossed(d2v1));
  grosterme = ncrossns1.Dot(nplan.Crossed(temp))/norm1/norm1;
  resul2.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-nplan.Dot(temp)),nplan,
		       ray1*grosterme/norm1,ns1,
		       -ray1/norm1,temp,
		       d1v1);

  if (first) {
    D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
    D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
    D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
  }
  else {
    D(2,3) = resul1.X();
    D(3,3) = resul1.Y();
    D(4,3) = resul1.Z();

    D(2,4) = resul2.X();
    D(3,4) = resul2.Y();
    D(4,4) = resul2.Z();
  }

  // derived compared to w (parameter on guideline)
  // It is assumed that the raduis is constant

  grosterme = ncrossns1.Dot(dnplan.Crossed(ns1))/norm1/norm1;
  resul1.SetLinearForm(-ray1/norm1*(grosterme*ndotns1-dnplan.Dot(ns1)),nplan,
		       ray1*ndotns1/norm1,dnplan,
		       ray1*grosterme/norm1,ns1);


  grosterme = ncrossns2.Dot(dnplan.Crossed(ns2))/norm2/norm2;
  resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-dnplan.Dot(ns2)),nplan,
		       -ray2*ndotns2/norm2,dnplan,
		       -ray2*grosterme/norm2,ns2);


  D(2,2) = resul1.X() + resul2.X();
  D(3,2) = resul1.Y() + resul2.Y();
  D(4,2) = resul1.Z() + resul2.Z();


  // Derived compared to u2
  temp = d2u2.Crossed(d1v2).Added(d1u2.Crossed(d2uv2));
  grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
  resul1.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
		       -ray2*grosterme/norm2,ns2,
		       ray2/norm2,temp);
  resul1.Subtract(d1u2);

  // Derived compared to v2
  temp = d2uv2.Crossed(d1v2).Added(d1u2.Crossed(d2v2));
  grosterme = ncrossns2.Dot(nplan.Crossed(temp))/norm2/norm2;
  resul2.SetLinearForm(ray2/norm2*(grosterme*ndotns2-nplan.Dot(temp)),nplan,
		       -ray2*grosterme/norm2,ns2,
		       ray2/norm2,temp);
  resul2.Subtract(d1v2);

  if (!first) {
    D(2,1) = v2d.X()*resul1.X() + v2d.Y()*resul2.X();
    D(3,1) = v2d.X()*resul1.Y() + v2d.Y()*resul2.Y();
    D(4,1) = v2d.X()*resul1.Z() + v2d.Y()*resul2.Z();
  }
  else {
    D(2,3) = resul1.X();
    D(3,3) = resul1.Y();
    D(4,3) = resul1.Z();

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