[occt.git] / src / BRepBlend / BRepBlend_SurfCurvConstRadInv.cxx

// Created on: 1997-02-21
// Created by: Laurent BOURESCHE
// Copyright (c) 1997-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_HCurve2d.hxx>
#include <Adaptor3d_HCurve.hxx>
#include <Adaptor3d_HSurface.hxx>
#include <BRepBlend_SurfCurvConstRadInv.hxx>
#include <math_Matrix.hxx>

//=======================================================================
//function : BRepBlend_SurfCurvConstRadInv
//purpose  : 
//=======================================================================
BRepBlend_SurfCurvConstRadInv::BRepBlend_SurfCurvConstRadInv
(const Handle(Adaptor3d_HSurface)& S,
 const Handle(Adaptor3d_HCurve)& C,
 const Handle(Adaptor3d_HCurve)& Cg) : surf(S),curv(C),guide(Cg)
{
}

//=======================================================================
//function : Set
//purpose  : 
//=======================================================================

void BRepBlend_SurfCurvConstRadInv::Set(const Standard_Real R,
					const Standard_Integer Choix) 
{
  choix = Choix;
  switch (choix) {
  case 1:
  case 2:
    {
      ray = -Abs(R);
    }
    break;
  case 3:
  case 4:
    {
      ray = Abs(R);
    }
    break;
  default:
    {
      ray = -Abs(R);
    }
  }
}

//=======================================================================
//function : NbEquations
//purpose  : 
//=======================================================================

Standard_Integer BRepBlend_SurfCurvConstRadInv::NbEquations() const
{
  return 3;
}

//=======================================================================
//function : Value
//purpose  : 
//=======================================================================

Standard_Boolean BRepBlend_SurfCurvConstRadInv::Value(const math_Vector& X,
						      math_Vector& F) 
{
  gp_Pnt ptgui;
  gp_Vec d1gui;
  guide->D1(X(1),ptgui,d1gui);
  gp_Vec nplan = d1gui.Normalized();
  Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
  gp_Pnt ptcur = curv->Value(X(2));
  F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
  gp_Pnt2d p2drst = rst->Value(X(3));
  gp_Pnt pts;
  gp_Vec du,dv;
  surf->D1(p2drst.X(),p2drst.Y(),pts,du,dv);
  F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
  gp_Vec ns = du.Crossed(dv);
  Standard_Real norm = nplan.Crossed(ns).Magnitude();
  Standard_Real unsurnorm = 1./norm;
  ns.SetLinearForm(nplan.Dot(ns),nplan, -1.,ns);
  ns.Multiply(unsurnorm);
  gp_Vec ref(ptcur,pts);
  ref.SetLinearForm(ray,ns,ref);
  F(3) = ref.SquareMagnitude() - ray*ray;
  return Standard_True;
}

//=======================================================================
//function : Derivatives
//purpose  : 
//=======================================================================

Standard_Boolean BRepBlend_SurfCurvConstRadInv::Derivatives(const math_Vector& X,
							    math_Matrix& D) 
{
  gp_Pnt ptgui;
  gp_Vec d1gui,d2gui;
  guide->D2(X(1),ptgui,d1gui,d2gui);
  Standard_Real normd1gui = d1gui.Magnitude();
  Standard_Real unsurnormd1gui = 1./normd1gui;
  gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
  gp_Vec dnplan;
  dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
  dnplan.Multiply(unsurnormd1gui);
  Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
  gp_Pnt ptcur;
  gp_Vec d1cur;
  curv->D1(X(2),ptcur,d1cur);
  D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
  D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
  D(1,3) = 0.;

  gp_Pnt2d p2drst;
  gp_Vec2d d1rst;
  rst->D1(X(3),p2drst,d1rst);
  gp_Pnt pts;
  gp_Vec d1u,d1v,d2u,d2v,duv;
  surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
  D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
  D(2,2) = 0.;
  gp_Vec dwrstpts;
  dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
  D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());

  gp_Vec nsurf = d1u.Crossed(d1v);
  gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
  gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
  gp_Vec dwrstnsurf;
  dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);

  gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
  gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
  gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);

  Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
  Standard_Real norm       = sqrt(norm2);
  Standard_Real unsurnorm  = 1./norm;
  Standard_Real raysurnorm = ray*unsurnorm;
  Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
  Standard_Real raysurnorm2 = ray*unsurnorm2;
  Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
  Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);

  Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
  Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
  Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);

  gp_Vec temp,dwguitemp,dwrsttemp;
  temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
  dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
  dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);

  gp_Vec corde(ptcur,pts);
  gp_Vec ref,dwguiref,dwrstref;
  ref.SetLinearForm(raysurnorm,temp,corde);
  dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
  dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);

  ref.Add(ref);
  D(3,1) = ref.Dot(dwguiref);
  D(3,2) = -ref.Dot(d1cur);
  D(3,3) = ref.Dot(dwrstref);

  return Standard_True;
}

//=======================================================================
//function : Values
//purpose  : 
//=======================================================================

Standard_Boolean BRepBlend_SurfCurvConstRadInv::Values(const math_Vector& X,
						       math_Vector& F,
						       math_Matrix& D) 
{
  gp_Pnt ptgui;
  gp_Vec d1gui(0.,0.,0.),d2gui(0.,0.,0.);
  guide->D2(X(1),ptgui,d1gui,d2gui);
  Standard_Real normd1gui = d1gui.Magnitude();
  Standard_Real unsurnormd1gui = 1./normd1gui;
  gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
  Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
  gp_Vec dnplan;
  dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
  dnplan.Multiply(unsurnormd1gui);
  Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
  gp_Pnt ptcur;
  gp_Vec d1cur;
  curv->D1(X(2),ptcur,d1cur);
  F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
  D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
  D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
  D(1,3) = 0.;

  gp_Pnt2d p2drst;
  gp_Vec2d d1rst;
  rst->D1(X(3),p2drst,d1rst);
  gp_Pnt pts;
  gp_Vec d1u,d1v,d2u,d2v,duv;
  surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
  F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
  D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
  D(2,2) = 0.;
  gp_Vec dwrstpts;
  dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
  D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());

  gp_Vec nsurf = d1u.Crossed(d1v);
  gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
  gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
  gp_Vec dwrstnsurf;
  dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);

  gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
  gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
  gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);

  Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
  Standard_Real norm       = sqrt(norm2);
  Standard_Real unsurnorm  = 1./norm;
  Standard_Real raysurnorm = ray*unsurnorm;
  Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
  Standard_Real raysurnorm2 = ray*unsurnorm2;
  Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
  Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);

  Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
  Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
  Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);

  gp_Vec temp,dwguitemp,dwrsttemp;
  temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
  dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
  dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);

  gp_Vec corde(ptcur,pts);
  gp_Vec ref,dwguiref,dwrstref;
  ref.SetLinearForm(raysurnorm,temp,corde);
  F(3) = ref.SquareMagnitude() - ray*ray;
  dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
  dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);

  ref.Add(ref);
  D(3,1) = ref.Dot(dwguiref);
  D(3,2) = -ref.Dot(d1cur);
  D(3,3) = ref.Dot(dwrstref);
  return Standard_True;
}

//=======================================================================
//function : Set
//purpose  : 
//=======================================================================

void BRepBlend_SurfCurvConstRadInv::Set(const Handle(Adaptor2d_HCurve2d)& Rst) 
{
  rst = Rst;
}

//=======================================================================
//function : GetTolerance
//purpose  : 
//=======================================================================

void BRepBlend_SurfCurvConstRadInv::GetTolerance(math_Vector& Tolerance,
						 const Standard_Real Tol) const
{
  Tolerance(1) = guide->Resolution(Tol);
  Tolerance(2) = curv->Resolution(Tol);
  Standard_Real ru,rv;
  ru = surf->UResolution(Tol);
  rv = surf->VResolution(Tol);
  Tolerance(3) = rst->Resolution(Min(ru,rv)); 
}

//=======================================================================
//function : GetBounds
//purpose  : 
//=======================================================================

void BRepBlend_SurfCurvConstRadInv::GetBounds(math_Vector& InfBound,
					      math_Vector& SupBound) const
{
  InfBound(1) = guide->FirstParameter();
  SupBound(1) = guide->LastParameter();
  InfBound(2) = curv->FirstParameter();
  SupBound(2) = curv->LastParameter();
  InfBound(3) = rst->FirstParameter();
  SupBound(3) = rst->LastParameter();
}

//=======================================================================
//function : IsSolution
//purpose  : 
//=======================================================================

Standard_Boolean BRepBlend_SurfCurvConstRadInv::IsSolution(const math_Vector& Sol,
							   const Standard_Real Tol) 
{
  math_Vector valsol(1,3);
  Value(Sol,valsol);
  if (Abs(valsol(1)) <= Tol && 
      Abs(valsol(2)) <= Tol &&
      Abs(valsol(3)) <= 2*Tol*Abs(ray) ) {
    return Standard_True;
  }
  return Standard_False;
}
Commit	Line	Data
b311480e	1	// Created on: 1997-02-21
	2	// Created by: Laurent BOURESCHE
	3	// Copyright (c) 1997-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
42cf5bc1	17
	18	#include <Adaptor2d_HCurve2d.hxx>
	19	#include <Adaptor3d_HCurve.hxx>
	20	#include <Adaptor3d_HSurface.hxx>
	21	#include <BRepBlend_SurfCurvConstRadInv.hxx>
	22	#include <math_Matrix.hxx>
7fd59977	23
	24	//=======================================================================
	25	//function : BRepBlend_SurfCurvConstRadInv
	26	//purpose :
	27	//=======================================================================
7fd59977	28	BRepBlend_SurfCurvConstRadInv::BRepBlend_SurfCurvConstRadInv
	29	(const Handle(Adaptor3d_HSurface)& S,
	30	const Handle(Adaptor3d_HCurve)& C,
	31	const Handle(Adaptor3d_HCurve)& Cg) : surf(S),curv(C),guide(Cg)
	32	{
	33	}
	34
	35	//=======================================================================
	36	//function : Set
	37	//purpose :
	38	//=======================================================================
	39
	40	void BRepBlend_SurfCurvConstRadInv::Set(const Standard_Real R,
	41	const Standard_Integer Choix)
	42	{
	43	choix = Choix;
	44	switch (choix) {
	45	case 1:
	46	case 2:
	47	{
	48	ray = -Abs(R);
	49	}
	50	break;
	51	case 3:
	52	case 4:
	53	{
	54	ray = Abs(R);
	55	}
	56	break;
	57	default:
	58	{
	59	ray = -Abs(R);
	60	}
	61	}
	62	}
	63
	64	//=======================================================================
	65	//function : NbEquations
	66	//purpose :
	67	//=======================================================================
	68
	69	Standard_Integer BRepBlend_SurfCurvConstRadInv::NbEquations() const
	70	{
	71	return 3;
	72	}
	73
	74	//=======================================================================
	75	//function : Value
	76	//purpose :
	77	//=======================================================================
	78
	79	Standard_Boolean BRepBlend_SurfCurvConstRadInv::Value(const math_Vector& X,
	80	math_Vector& F)
	81	{
	82	gp_Pnt ptgui;
	83	gp_Vec d1gui;
	84	guide->D1(X(1),ptgui,d1gui);
	85	gp_Vec nplan = d1gui.Normalized();
	86	Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
	87	gp_Pnt ptcur = curv->Value(X(2));
	88	F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
	89	gp_Pnt2d p2drst = rst->Value(X(3));
	90	gp_Pnt pts;
	91	gp_Vec du,dv;
92	surf->D1(p2drst.X(),p2drst.Y(),pts,du,dv);
93	F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
94	gp_Vec ns = du.Crossed(dv);
95	Standard_Real norm = nplan.Crossed(ns).Magnitude();
96	Standard_Real unsurnorm = 1./norm;
97	ns.SetLinearForm(nplan.Dot(ns),nplan, -1.,ns);
98	ns.Multiply(unsurnorm);
99	gp_Vec ref(ptcur,pts);
100	ref.SetLinearForm(ray,ns,ref);
101	F(3) = ref.SquareMagnitude() - ray*ray;
102	return Standard_True;
103	}
104
105	//=======================================================================
106	//function : Derivatives
107	//purpose :
108	//=======================================================================
109
110	Standard_Boolean BRepBlend_SurfCurvConstRadInv::Derivatives(const math_Vector& X,
111	math_Matrix& D)
112	{
113	gp_Pnt ptgui;
114	gp_Vec d1gui,d2gui;
115	guide->D2(X(1),ptgui,d1gui,d2gui);
116	Standard_Real normd1gui = d1gui.Magnitude();
117	Standard_Real unsurnormd1gui = 1./normd1gui;
118	gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
7fd59977	119	gp_Vec dnplan;
	120	dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
	121	dnplan.Multiply(unsurnormd1gui);
	122	Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
	123	gp_Pnt ptcur;
	124	gp_Vec d1cur;
	125	curv->D1(X(2),ptcur,d1cur);
	126	D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
	127	D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
	128	D(1,3) = 0.;
	129
	130	gp_Pnt2d p2drst;
	131	gp_Vec2d d1rst;
	132	rst->D1(X(3),p2drst,d1rst);
	133	gp_Pnt pts;
	134	gp_Vec d1u,d1v,d2u,d2v,duv;
	135	surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
	136	D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
	137	D(2,2) = 0.;
	138	gp_Vec dwrstpts;
	139	dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
	140	D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());
	141
	142	gp_Vec nsurf = d1u.Crossed(d1v);
	143	gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
	144	gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
	145	gp_Vec dwrstnsurf;
	146	dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);
	147
	148	gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
	149	gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
	150	gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);
	151
	152	Standard_Real norm2 = nplancrosnsurf.SquareMagnitude();
	153	Standard_Real norm = sqrt(norm2);
	154	Standard_Real unsurnorm = 1./norm;
	155	Standard_Real raysurnorm = ray*unsurnorm;
	156	Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
	157	Standard_Real raysurnorm2 = ray*unsurnorm2;
	158	Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
	159	Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);
	160
	161	Standard_Real nplandotnsurf = nplan.Dot(nsurf);
	162	Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
	163	Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);
	164
	165	gp_Vec temp,dwguitemp,dwrsttemp;
	166	temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
	167	dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
	168	dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);
	169
	170	gp_Vec corde(ptcur,pts);
	171	gp_Vec ref,dwguiref,dwrstref;
	172	ref.SetLinearForm(raysurnorm,temp,corde);
	173	dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
	174	dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
	175
	176	ref.Add(ref);
	177	D(3,1) = ref.Dot(dwguiref);
	178	D(3,2) = -ref.Dot(d1cur);
	179	D(3,3) = ref.Dot(dwrstref);
	180
	181	return Standard_True;
	182	}
183
184	//=======================================================================
185	//function : Values
186	//purpose :
187	//=======================================================================
188
189	Standard_Boolean BRepBlend_SurfCurvConstRadInv::Values(const math_Vector& X,
190	math_Vector& F,
191	math_Matrix& D)
192	{
193	gp_Pnt ptgui;
194	gp_Vec d1gui(0.,0.,0.),d2gui(0.,0.,0.);
195	guide->D2(X(1),ptgui,d1gui,d2gui);
196	Standard_Real normd1gui = d1gui.Magnitude();
197	Standard_Real unsurnormd1gui = 1./normd1gui;
198	gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
199	Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
200	gp_Vec dnplan;
201	dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
202	dnplan.Multiply(unsurnormd1gui);
203	Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
204	gp_Pnt ptcur;
205	gp_Vec d1cur;
206	curv->D1(X(2),ptcur,d1cur);
207	F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
208	D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
209	D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
210	D(1,3) = 0.;
211
212	gp_Pnt2d p2drst;
213	gp_Vec2d d1rst;
214	rst->D1(X(3),p2drst,d1rst);
215	gp_Pnt pts;
216	gp_Vec d1u,d1v,d2u,d2v,duv;
217	surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
218	F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
219	D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
220	D(2,2) = 0.;
221	gp_Vec dwrstpts;
222	dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
223	D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());
224
225	gp_Vec nsurf = d1u.Crossed(d1v);
226	gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
227	gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
228	gp_Vec dwrstnsurf;
229	dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);
230
231	gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
232	gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
233	gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);
234
235	Standard_Real norm2 = nplancrosnsurf.SquareMagnitude();
236	Standard_Real norm = sqrt(norm2);
237	Standard_Real unsurnorm = 1./norm;
238	Standard_Real raysurnorm = ray*unsurnorm;
239	Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
240	Standard_Real raysurnorm2 = ray*unsurnorm2;
241	Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
242	Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);
243
244	Standard_Real nplandotnsurf = nplan.Dot(nsurf);
245	Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
246	Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);
247
248	gp_Vec temp,dwguitemp,dwrsttemp;
249	temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
250	dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
251	dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);
252
253	gp_Vec corde(ptcur,pts);
254	gp_Vec ref,dwguiref,dwrstref;
255	ref.SetLinearForm(raysurnorm,temp,corde);
256	F(3) = ref.SquareMagnitude() - ray*ray;
257	dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
258	dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
259
260	ref.Add(ref);
261	D(3,1) = ref.Dot(dwguiref);
262	D(3,2) = -ref.Dot(d1cur);
263	D(3,3) = ref.Dot(dwrstref);
264	return Standard_True;
265	}
266
267	//=======================================================================
268	//function : Set
269	//purpose :
270	//=======================================================================
271
272	void BRepBlend_SurfCurvConstRadInv::Set(const Handle(Adaptor2d_HCurve2d)& Rst)
273	{
274	rst = Rst;
275	}
276
277	//=======================================================================
278	//function : GetTolerance
279	//purpose :
280	//=======================================================================
281
282	void BRepBlend_SurfCurvConstRadInv::GetTolerance(math_Vector& Tolerance,
283	const Standard_Real Tol) const
284	{
285	Tolerance(1) = guide->Resolution(Tol);
286	Tolerance(2) = curv->Resolution(Tol);
287	Standard_Real ru,rv;
288	ru = surf->UResolution(Tol);
289	rv = surf->VResolution(Tol);
290	Tolerance(3) = rst->Resolution(Min(ru,rv));
291	}
292
293	//=======================================================================
294	//function : GetBounds
295	//purpose :
296	//=======================================================================
297
298	void BRepBlend_SurfCurvConstRadInv::GetBounds(math_Vector& InfBound,
299	math_Vector& SupBound) const
300	{
301	InfBound(1) = guide->FirstParameter();
302	SupBound(1) = guide->LastParameter();
303	InfBound(2) = curv->FirstParameter();
304	SupBound(2) = curv->LastParameter();
305	InfBound(3) = rst->FirstParameter();
306	SupBound(3) = rst->LastParameter();
307	}
308
309	//=======================================================================
310	//function : IsSolution
311	//purpose :
312	//=======================================================================
313
314	Standard_Boolean BRepBlend_SurfCurvConstRadInv::IsSolution(const math_Vector& Sol,
315	const Standard_Real Tol)
316	{
317	math_Vector valsol(1,3);
318	Value(Sol,valsol);
319	if (Abs(valsol(1)) <= Tol &&
320	Abs(valsol(2)) <= Tol &&
321	Abs(valsol(3)) <= 2TolAbs(ray) ) {
322	return Standard_True;
323	}
324	return Standard_False;
325	}
326
327