src/BRepBlend/BRepBlend_SurfCurvEvolRadInv.cxx

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