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

// Created on: 1997-02-12
// 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 <Adaptor3d_HCurve.hxx>
#include <Adaptor3d_HSurface.hxx>
#include <BRepBlend_SurfPointConstRadInv.hxx>
#include <gp_Pnt.hxx>
#include <math_Matrix.hxx>

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

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

void BRepBlend_SurfPointConstRadInv::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_SurfPointConstRadInv::NbEquations() const
{
  return 3;
}

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

Standard_Boolean BRepBlend_SurfPointConstRadInv::Value(const math_Vector& X,
						       math_Vector& F) 
{
  Standard_Real theD,norm,unsurnorm;
  gp_Pnt ptcur,pts;
  gp_Vec d1cur(0.,0.,0.),d1u(0.,0.,0.),d1v(0.,0.,0.);
  gp_XYZ nplan(0.,0.,0.),ns(0.,0.,0.),ref(0.,0.,0.);
  curv->D1(X(1),ptcur,d1cur);
  nplan = d1cur.Normalized().XYZ();
//  theD = -(nplan.Dot(ptcur.XYZ()));
  gp_XYZ ptcurXYZ(ptcur.XYZ());
  theD =  nplan.Dot(ptcurXYZ)  ;
  theD = theD  * (-1.) ;

  surf->D1(X(2),X(3),pts,d1u,d1v);
  F(1) = nplan.Dot(point.XYZ()) + theD;
  F(2) = nplan.Dot(pts.XYZ()) + theD;
  ns = d1u.Crossed(d1v).XYZ();
  norm = nplan.Crossed(ns).Modulus();
  unsurnorm = 1./norm;
  ns.SetLinearForm(nplan.Dot(ns),nplan, -1.,ns);
  ns.Multiply(unsurnorm);
  ref = pts.XYZ() - point.XYZ();
  ref.SetLinearForm(ray,ns,ref);
  F(3) = ref.SquareModulus() - ray*ray;
  return Standard_True;
}

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

Standard_Boolean BRepBlend_SurfPointConstRadInv::Derivatives(const math_Vector& X,
							     math_Matrix& D) 
{
  gp_Pnt ptcur,pts;
  gp_Vec d1cur,d2cur,nplan,dnplan,d1u,d1v,d2u,d2v,duv;
  Standard_Real theD, dtheD, normd1cur, unsurnormd1cur;

  curv->D2(X(1),ptcur,d1cur,d2cur);
  normd1cur = d1cur.Magnitude();
  unsurnormd1cur = 1./normd1cur;
  nplan = unsurnormd1cur * d1cur;
//  theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
  gp_XYZ nplanXYZ(nplan.XYZ());
  gp_XYZ ptcurXYZ(ptcur.XYZ());
  theD =  nplanXYZ.Dot(ptcurXYZ)  ;
  theD = theD  * (-1.) ;

  dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur);
  dnplan.Multiply(unsurnormd1cur);
  dtheD = - nplan.XYZ().Dot(d1cur.XYZ()) - dnplan.XYZ().Dot(ptcur.XYZ());
  D(1,1) = dnplan.XYZ().Dot(point.XYZ()) + dtheD;
  D(1,2) = D(1,3) = 0.;
  surf->D2(X(2),X(3),pts,d1u,d1v,d2u,d2v,duv);
  D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
  D(2,2) = nplan.Dot(d1u);
  D(2,3) = nplan.Dot(d1v);
  
  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 nplancrosnsurf = nplan.Crossed(nsurf);
  gp_Vec dwnplancrosnsurf = dnplan.Crossed(nsurf);
  gp_Vec dunplancrosnsurf = nplan.Crossed(dunsurf);
  gp_Vec dvnplancrosnsurf = nplan.Crossed(dvnsurf);

  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 dwnorm = unsurnorm*nplancrosnsurf.Dot(dwnplancrosnsurf);
  Standard_Real dunorm = unsurnorm*nplancrosnsurf.Dot(dunplancrosnsurf);
  Standard_Real dvnorm = unsurnorm*nplancrosnsurf.Dot(dvnplancrosnsurf);

  Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
  Standard_Real dwnplandotnsurf = dnplan.Dot(nsurf);
  Standard_Real dunplandotnsurf = nplan.Dot(dunsurf);
  Standard_Real dvnplandotnsurf = nplan.Dot(dvnsurf);
  
  gp_Vec temp,dwtemp,dutemp,dvtemp;
  temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
  dwtemp.SetLinearForm(nplandotnsurf,dnplan,dwnplandotnsurf,nplan);
  dutemp.SetLinearForm(dunplandotnsurf,nplan,-1.,dunsurf);
  dvtemp.SetLinearForm(dvnplandotnsurf,nplan,-1.,dvnsurf);

  gp_Vec ref,dwref,duref,dvref,corde(point,pts);
  ref.SetLinearForm(raysurnorm,temp,corde);
  dwref.SetLinearForm(raysurnorm,dwtemp,-raysurnorm2*dwnorm,temp);
  duref.SetLinearForm(raysurnorm,dutemp,-raysurnorm2*dunorm,temp,d1u);
  dvref.SetLinearForm(raysurnorm,dvtemp,-raysurnorm2*dvnorm,temp,d1v);
  
  ref.Add(ref);
  D(3,1) = ref.Dot(dwref);
  D(3,2) = ref.Dot(duref);
  D(3,3) = ref.Dot(dvref);

  return Standard_True;
}

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

Standard_Boolean BRepBlend_SurfPointConstRadInv::Values(const math_Vector& X,
							math_Vector& F,
							math_Matrix& D) 
{
  gp_Pnt ptcur,pts;
  gp_Vec d1cur,d2cur,nplan,dnplan,d1u,d1v,d2u,d2v,duv;
  Standard_Real theD, dtheD, normd1cur, unsurnormd1cur;

  curv->D2(X(1),ptcur,d1cur,d2cur);
  surf->D2(X(2),X(3),pts,d1u,d1v,d2u,d2v,duv);
  normd1cur = d1cur.Magnitude();
  unsurnormd1cur = 1./normd1cur;
  nplan = unsurnormd1cur * d1cur;
//  theD = -(nplan.XYZ().Dot(ptcur.XYZ()));
  gp_XYZ nplanXYZ(nplan.XYZ());
  gp_XYZ ptcurXYZ(ptcur.XYZ());
  theD =  nplanXYZ.Dot(ptcurXYZ)  ;
  theD = theD  * (-1.) ;

  F(1) = nplan.XYZ().Dot(point.XYZ()) + theD;
  F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;

  dnplan.SetLinearForm(-nplan.Dot(d2cur),nplan,d2cur);
  dnplan.Multiply(unsurnormd1cur);
  dtheD = - nplan.XYZ().Dot(d1cur.XYZ()) - dnplan.XYZ().Dot(ptcur.XYZ());
  D(1,1) = dnplan.XYZ().Dot(point.XYZ()) + dtheD;
  D(1,2) = D(1,3) = 0.;
  D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
  D(2,2) = nplan.Dot(d1u);
  D(2,3) = nplan.Dot(d1v);
  
  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 nplancrosnsurf = nplan.Crossed(nsurf);
  gp_Vec dwnplancrosnsurf = dnplan.Crossed(nsurf);
  gp_Vec dunplancrosnsurf = nplan.Crossed(dunsurf);
  gp_Vec dvnplancrosnsurf = nplan.Crossed(dvnsurf);

  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 dwnorm = unsurnorm*nplancrosnsurf.Dot(dwnplancrosnsurf);
  Standard_Real dunorm = unsurnorm*nplancrosnsurf.Dot(dunplancrosnsurf);
  Standard_Real dvnorm = unsurnorm*nplancrosnsurf.Dot(dvnplancrosnsurf);

  Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
  Standard_Real dwnplandotnsurf = dnplan.Dot(nsurf);
  Standard_Real dunplandotnsurf = nplan.Dot(dunsurf);
  Standard_Real dvnplandotnsurf = nplan.Dot(dvnsurf);
  
  gp_Vec temp,dwtemp,dutemp,dvtemp;
  temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
  dwtemp.SetLinearForm(nplandotnsurf,dnplan,dwnplandotnsurf,nplan);
  dutemp.SetLinearForm(dunplandotnsurf,nplan,-1.,dunsurf);
  dvtemp.SetLinearForm(dvnplandotnsurf,nplan,-1.,dvnsurf);

  gp_Vec ref,dwref,duref,dvref,corde(point,pts);
  ref.SetLinearForm(raysurnorm,temp,corde);
  F(3) = ref.SquareMagnitude() - ray*ray;
  dwref.SetLinearForm(raysurnorm,dwtemp,-raysurnorm2*dwnorm,temp);
  duref.SetLinearForm(raysurnorm,dutemp,-raysurnorm2*dunorm,temp,d1u);
  dvref.SetLinearForm(raysurnorm,dvtemp,-raysurnorm2*dvnorm,temp,d1v);
  
  ref.Add(ref);
  D(3,1) = ref.Dot(dwref);
  D(3,2) = ref.Dot(duref);
  D(3,3) = ref.Dot(dvref);

  return Standard_True;
}

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

void BRepBlend_SurfPointConstRadInv::Set(const gp_Pnt& P) 
{
  point = P;
}

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

void BRepBlend_SurfPointConstRadInv::GetTolerance(math_Vector& Tolerance,
						  const Standard_Real Tol) const
{
  Tolerance(1) = curv->Resolution(Tol);
  Tolerance(2) = surf->UResolution(Tol);
  Tolerance(3) = surf->VResolution(Tol);
}

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

void BRepBlend_SurfPointConstRadInv::GetBounds(math_Vector& InfBound,
					       math_Vector& SupBound) const
{
  InfBound(1) = curv->FirstParameter();
  SupBound(1) = curv->LastParameter();
  InfBound(2) = surf->FirstUParameter();
  SupBound(2) = surf->LastUParameter();
  InfBound(3) = surf->FirstVParameter();
  SupBound(3) = surf->LastVParameter();
}

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

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