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 #ifdef DEB
 106   Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
 107 #endif
 108   gp_Vec dnplan;
 109   dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
 110   dnplan.Multiply(unsurnormd1gui);
 111   Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
 112   gp_Pnt ptcur;
 113   gp_Vec d1cur;
 114   curv->D1(X(2),ptcur,d1cur);
 115   D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
 116   D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
 117   D(1,3) = 0.;
 118
 119   gp_Pnt2d p2drst;
 120   gp_Vec2d d1rst;
 121   rst->D1(X(3),p2drst,d1rst);
 122   gp_Pnt pts;
 123   gp_Vec d1u,d1v,d2u,d2v,duv;
 124   surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
 125   D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
 126   D(2,2) = 0.;
 127   gp_Vec dwrstpts;
 128   dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
 129   D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());
 130
 131   gp_Vec nsurf = d1u.Crossed(d1v);
 132   gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
 133   gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
 134   gp_Vec dwrstnsurf;
 135   dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);
 136
 137   gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
 138   gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
 139   gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);
 140
 141   Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
 142   Standard_Real norm       = sqrt(norm2);
 143   Standard_Real unsurnorm  = 1./norm;
 144   Standard_Real raysurnorm = ray*unsurnorm;
 145   Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
 146   Standard_Real raysurnorm2 = ray*unsurnorm2;
 147   Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
 148   Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);
 149
 150   Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
 151   Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
 152   Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);
 153
 154   gp_Vec temp,dwguitemp,dwrsttemp;
 155   temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
 156   dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
 157   dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);
 158
 159   gp_Vec corde(ptcur,pts);
 160   gp_Vec ref,dwguiref,dwrstref;
 161   ref.SetLinearForm(raysurnorm,temp,corde);
 162   dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
 163   dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
 164
 165   ref.Add(ref);
 166   D(3,1) = ref.Dot(dwguiref);
 167   D(3,2) = -ref.Dot(d1cur);
 168   D(3,3) = ref.Dot(dwrstref);
 169
 170   return Standard_True;
 171 }
 172
 173 //=======================================================================
 174 //function : Values
 175 //purpose  :
 176 //=======================================================================
 177
 178 Standard_Boolean BRepBlend_SurfCurvConstRadInv::Values(const math_Vector& X,
 179                                                        math_Vector& F,
 180                                                        math_Matrix& D)
 181 {
 182   gp_Pnt ptgui;
 183   gp_Vec d1gui(0.,0.,0.),d2gui(0.,0.,0.);
 184   guide->D2(X(1),ptgui,d1gui,d2gui);
 185   Standard_Real normd1gui = d1gui.Magnitude();
 186   Standard_Real unsurnormd1gui = 1./normd1gui;
 187   gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
 188   Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
 189   gp_Vec dnplan;
 190   dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
 191   dnplan.Multiply(unsurnormd1gui);
 192   Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
 193   gp_Pnt ptcur;
 194   gp_Vec d1cur;
 195   curv->D1(X(2),ptcur,d1cur);
 196   F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
 197   D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
 198   D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
 199   D(1,3) = 0.;
 200
 201   gp_Pnt2d p2drst;
 202   gp_Vec2d d1rst;
 203   rst->D1(X(3),p2drst,d1rst);
 204   gp_Pnt pts;
 205   gp_Vec d1u,d1v,d2u,d2v,duv;
 206   surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
 207   F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
 208   D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
 209   D(2,2) = 0.;
 210   gp_Vec dwrstpts;
 211   dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
 212   D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());
 213
 214   gp_Vec nsurf = d1u.Crossed(d1v);
 215   gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
 216   gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
 217   gp_Vec dwrstnsurf;
 218   dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);
 219
 220   gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
 221   gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
 222   gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);
 223
 224   Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
 225   Standard_Real norm       = sqrt(norm2);
 226   Standard_Real unsurnorm  = 1./norm;
 227   Standard_Real raysurnorm = ray*unsurnorm;
 228   Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
 229   Standard_Real raysurnorm2 = ray*unsurnorm2;
 230   Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
 231   Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);
 232
 233   Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
 234   Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
 235   Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);
 236
 237   gp_Vec temp,dwguitemp,dwrsttemp;
 238   temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
 239   dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
 240   dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);
 241
 242   gp_Vec corde(ptcur,pts);
 243   gp_Vec ref,dwguiref,dwrstref;
 244   ref.SetLinearForm(raysurnorm,temp,corde);
 245   F(3) = ref.SquareMagnitude() - ray*ray;
 246   dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
 247   dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
 248
 249   ref.Add(ref);
 250   D(3,1) = ref.Dot(dwguiref);
 251   D(3,2) = -ref.Dot(d1cur);
 252   D(3,3) = ref.Dot(dwrstref);
 253   return Standard_True;
 254 }
 255
 256 //=======================================================================
 257 //function : Set
 258 //purpose  :
 259 //=======================================================================
 260
 261 void BRepBlend_SurfCurvConstRadInv::Set(const Handle(Adaptor2d_HCurve2d)& Rst)
 262 {
 263   rst = Rst;
 264 }
 265
 266 //=======================================================================
 267 //function : GetTolerance
 268 //purpose  :
 269 //=======================================================================
 270
 271 void BRepBlend_SurfCurvConstRadInv::GetTolerance(math_Vector& Tolerance,
 272                                                  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 : GetBounds
 284 //purpose  :
 285 //=======================================================================
 286
 287 void BRepBlend_SurfCurvConstRadInv::GetBounds(math_Vector& InfBound,
 288                                               math_Vector& SupBound) const
 289 {
 290   InfBound(1) = guide->FirstParameter();
 291   SupBound(1) = guide->LastParameter();
 292   InfBound(2) = curv->FirstParameter();
 293   SupBound(2) = curv->LastParameter();
 294   InfBound(3) = rst->FirstParameter();
 295   SupBound(3) = rst->LastParameter();
 296 }
 297
 298 //=======================================================================
 299 //function : IsSolution
 300 //purpose  :
 301 //=======================================================================
 302
 303 Standard_Boolean BRepBlend_SurfCurvConstRadInv::IsSolution(const math_Vector& Sol,
 304                                                            const Standard_Real Tol)
 305 {
 306   math_Vector valsol(1,3);
 307   Value(Sol,valsol);
 308   if (Abs(valsol(1)) <= Tol &&
 309       Abs(valsol(2)) <= Tol &&
 310       Abs(valsol(3)) <= 2*Tol*Abs(ray) ) {
 311     return Standard_True;
 312   }
 313   return Standard_False;
 314 }
 315
 316