src/BRepBlend/BRepBlend_SurfCurvConstRadInv.cxx

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