src/BRepBlend/BRepBlend_SurfCurvEvolRadInv.cxx

   1 // Created on: 1997-07-29
   2 // Created by: Jerome LEMONIER
   3 // Copyright (c) 1997-1999 Matra Datavision
   4 // Copyright (c) 1999-2014 OPEN CASCADE SAS
   5 //
   6 // This file is part of Open CASCADE Technology software library.
   7 //
   8 // This library is free software; you can redistribute it and/or modify it under
   9 // the terms of the GNU Lesser General Public License version 2.1 as published
  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.
  13 //
  14 // Alternatively, this file may be used under the terms of Open CASCADE
  15 // commercial license or contractual agreement.
  16
  17
  18 #include <Adaptor2d_HCurve2d.hxx>
  19 #include <Adaptor3d_HCurve.hxx>
  20 #include <Adaptor3d_HSurface.hxx>
  21 #include <BRepBlend_SurfCurvEvolRadInv.hxx>
  22 #include <Law_Function.hxx>
  23 #include <math_Matrix.hxx>
  24
  25 //=======================================================================
  26 //function :
  27 //purpose  :
  28 //=======================================================================
  29 BRepBlend_SurfCurvEvolRadInv::BRepBlend_SurfCurvEvolRadInv
  30 (const Handle(Adaptor3d_HSurface)& S,
  31 const Handle(Adaptor3d_HCurve)& C,
  32 const Handle(Adaptor3d_HCurve)& Cg,
  33  const Handle(Law_Function)& Evol): surf(S),curv(C),guide(Cg)
  34 { tevol=Evol;
  35 }
  36
  37 //=======================================================================
  38 //function :
  39 //purpose  :
  40 //=======================================================================
  41  void BRepBlend_SurfCurvEvolRadInv::Set(const Standard_Integer Choix)
  42 { choix = Choix;
  43   switch (choix) {
  44   case 1 :
  45   case 2 :
  46     sg1 = -1;
  47     break;
  48   case 3 :
  49   case 4 :
  50     sg1 = 1;
  51     break;
  52   default :
  53     sg1 = -1;
  54     break;
  55   }
  56
  57 }
  58
  59 //=======================================================================
  60 //function :
  61 //purpose  :
  62 //=======================================================================
  63  Standard_Integer BRepBlend_SurfCurvEvolRadInv::NbEquations() const
  64 {
  65   return 3;
  66 }
  67
  68 //=======================================================================
  69 //function :
  70 //purpose  :
  71 //=======================================================================
  72  Standard_Boolean BRepBlend_SurfCurvEvolRadInv::Value(const math_Vector& X,math_Vector& F)
  73 {
  74   gp_Pnt ptgui;
  75   gp_Vec d1gui(0.,0.,0.);
  76   guide->D1(X(1),ptgui,d1gui);
  77   ray = sg1*tevol->Value(X(1));
  78   gp_Vec nplan = d1gui.Normalized();
  79 //  Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
  80   gp_XYZ nplanXYZ(nplan.XYZ());
  81   gp_XYZ ptguiXYZ(ptgui.XYZ());
  82   Standard_Real theD =  nplanXYZ.Dot(ptguiXYZ)  ;
  83   theD = theD  * (-1.) ;
  84
  85   gp_Pnt ptcur = curv->Value(X(2));
  86   F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
  87   gp_Pnt2d p2drst = rst->Value(X(3));
  88   gp_Pnt pts;
  89   gp_Vec du,dv;
  90   surf->D1(p2drst.X(),p2drst.Y(),pts,du,dv);
  91   F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
  92   gp_Vec ns = du.Crossed(dv);
  93   Standard_Real norm = nplan.Crossed(ns).Magnitude();
  94   Standard_Real unsurnorm = 1./norm;
  95   ns.SetLinearForm(nplan.Dot(ns),nplan, -1.,ns);
  96   ns.Multiply(unsurnorm);
  97   gp_Vec ref(ptcur,pts);
  98
  99   ref.SetLinearForm(ray,ns,ref);
 100   F(3) = ref.SquareMagnitude() - ray*ray;
 101   return Standard_True;
 102 }
 103
 104 //=======================================================================
 105 //function :
 106 //purpose  :
 107 //=======================================================================
 108  Standard_Boolean BRepBlend_SurfCurvEvolRadInv::Derivatives(const math_Vector& X,math_Matrix& D)
 109 {
 110   gp_Pnt ptgui;
 111   gp_Vec d1gui,d2gui;
 112   guide->D2(X(1),ptgui,d1gui,d2gui);
 113   Standard_Real normd1gui = d1gui.Magnitude(),dray;
 114   Standard_Real unsurnormd1gui = 1./normd1gui;
 115   tevol->D1(X(1),ray,dray);
 116   ray=sg1*ray;
 117   dray=sg1*dray;
 118   gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
 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   dwguiref.SetLinearForm(1.,dwguiref,dray*unsurnorm,temp);
 175   dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
 176
 177   ref.Add(ref);
 178   D(3,1) = ref.Dot(dwguiref) - 2.*dray*ray;
 179   D(3,2) = -ref.Dot(d1cur);
 180   D(3,3) = ref.Dot(dwrstref);
 181
 182   return Standard_True;
 183 }
 184
 185 //=======================================================================
 186 //function :
 187 //purpose  :
 188 //=======================================================================
 189  Standard_Boolean BRepBlend_SurfCurvEvolRadInv::Values(const math_Vector& X,math_Vector& F,math_Matrix& D)
 190 {
 191   gp_Pnt ptgui;
 192   gp_Vec d1gui(0.,0.,0.),d2gui(0.,0.,0.);
 193   guide->D2(X(1),ptgui,d1gui,d2gui);
 194   Standard_Real dray;
 195   tevol->D1(X(1),ray,dray);
 196   ray=sg1*ray;
 197   dray=sg1*dray;
 198   Standard_Real normd1gui = d1gui.Magnitude();
 199   Standard_Real unsurnormd1gui = 1./normd1gui;
 200   gp_Vec nplan = d1gui.Multiplied(unsurnormd1gui);
 201 //  Standard_Real theD = -(nplan.XYZ().Dot(ptgui.XYZ()));
 202   gp_XYZ nplanXYZ(nplan.XYZ());
 203   gp_XYZ ptcurXYZ(ptgui.XYZ());
 204   Standard_Real theD =  nplanXYZ.Dot(ptcurXYZ)  ;
 205   theD = theD  * (-1.) ;
 206
 207   gp_Vec dnplan;
 208   dnplan.SetLinearForm(-nplan.Dot(d2gui),nplan,d2gui);
 209   dnplan.Multiply(unsurnormd1gui);
 210   Standard_Real dtheD = - nplan.XYZ().Dot(d1gui.XYZ()) - dnplan.XYZ().Dot(ptgui.XYZ());
 211   gp_Pnt ptcur;
 212   gp_Vec d1cur;
 213   curv->D1(X(2),ptcur,d1cur);
 214   F(1) = nplan.XYZ().Dot(ptcur.XYZ()) + theD;
 215   D(1,1) = dnplan.XYZ().Dot(ptcur.XYZ()) + dtheD;
 216   D(1,2) = nplan.XYZ().Dot(d1cur.XYZ());
 217   D(1,3) = 0.;
 218
 219   gp_Pnt2d p2drst;
 220   gp_Vec2d d1rst;
 221   rst->D1(X(3),p2drst,d1rst);
 222   gp_Pnt pts;
 223   gp_Vec d1u,d1v,d2u,d2v,duv;
 224   surf->D2(p2drst.X(),p2drst.Y(),pts,d1u,d1v,d2u,d2v,duv);
 225   F(2) = nplan.XYZ().Dot(pts.XYZ()) + theD;
 226   D(2,1) = dnplan.XYZ().Dot(pts.XYZ()) + dtheD;
 227   D(2,2) = 0.;
 228   gp_Vec dwrstpts;
 229   dwrstpts.SetLinearForm(d1rst.X(),d1u,d1rst.Y(),d1v);
 230   D(2,3) = nplan.XYZ().Dot(dwrstpts.XYZ());
 231
 232   gp_Vec nsurf = d1u.Crossed(d1v);
 233   gp_Vec dunsurf = d2u.Crossed(d1v).Added(d1u.Crossed(duv));
 234   gp_Vec dvnsurf = d1u.Crossed(d2v).Added(duv.Crossed(d1v));
 235   gp_Vec dwrstnsurf;
 236   dwrstnsurf.SetLinearForm(d1rst.X(),dunsurf,d1rst.Y(),dvnsurf);
 237
 238   gp_Vec nplancrosnsurf = nplan.Crossed(nsurf);
 239   gp_Vec dwguinplancrosnsurf = dnplan.Crossed(nsurf);
 240   gp_Vec dwrstnplancrosnsurf = nplan.Crossed(dwrstnsurf);
 241
 242   Standard_Real norm2      = nplancrosnsurf.SquareMagnitude();
 243   Standard_Real norm       = sqrt(norm2);
 244   Standard_Real unsurnorm  = 1./norm;
 245   Standard_Real raysurnorm = ray*unsurnorm;
 246   Standard_Real unsurnorm2 = unsurnorm * unsurnorm;
 247   Standard_Real raysurnorm2 = ray*unsurnorm2;
 248   Standard_Real dwguinorm = unsurnorm*nplancrosnsurf.Dot(dwguinplancrosnsurf);
 249   Standard_Real dwrstnorm = unsurnorm*nplancrosnsurf.Dot(dwrstnplancrosnsurf);
 250
 251   Standard_Real nplandotnsurf   = nplan.Dot(nsurf);
 252   Standard_Real dwguinplandotnsurf = dnplan.Dot(nsurf);
 253   Standard_Real dwrstnplandotnsurf = nplan.Dot(dwrstnsurf);
 254
 255   gp_Vec temp,dwguitemp,dwrsttemp;
 256   temp.SetLinearForm(nplandotnsurf,nplan,-1.,nsurf);
 257   dwguitemp.SetLinearForm(nplandotnsurf,dnplan,dwguinplandotnsurf,nplan);
 258   dwrsttemp.SetLinearForm(dwrstnplandotnsurf,nplan,-1.,dwrstnsurf);
 259
 260   gp_Vec corde(ptcur,pts);
 261   gp_Vec ref,dwguiref,dwrstref;
 262   ref.SetLinearForm(raysurnorm,temp,corde);
 263   F(3) = ref.SquareMagnitude() - ray*ray;
 264   dwguiref.SetLinearForm(raysurnorm,dwguitemp,-raysurnorm2*dwguinorm,temp);
 265   dwguiref.SetLinearForm(1.,dwguiref,dray*unsurnorm,temp);
 266   dwrstref.SetLinearForm(raysurnorm,dwrsttemp,-raysurnorm2*dwrstnorm,temp,dwrstpts);
 267
 268   ref.Add(ref);
 269   D(3,1) = ref.Dot(dwguiref) - 2.*dray*ray;
 270   D(3,2) = -ref.Dot(d1cur);
 271   D(3,3) = ref.Dot(dwrstref);
 272   return Standard_True;
 273 }
 274
 275 //=======================================================================
 276 //function :
 277 //purpose  :
 278 //=======================================================================
 279  void BRepBlend_SurfCurvEvolRadInv::Set(const Handle(Adaptor2d_HCurve2d)& Rst)
 280 {
 281   rst = Rst;
 282 }
 283
 284 //=======================================================================
 285 //function :
 286 //purpose  :
 287 //=======================================================================
 288  void BRepBlend_SurfCurvEvolRadInv::GetTolerance(math_Vector& Tolerance,const Standard_Real Tol) const
 289 {
 290   Tolerance(1) = guide->Resolution(Tol);
 291   Tolerance(2) = curv->Resolution(Tol);
 292   Standard_Real ru,rv;
 293   ru = surf->UResolution(Tol);
 294   rv = surf->VResolution(Tol);
 295   Tolerance(3) = rst->Resolution(Min(ru,rv));
 296 }
 297
 298 //=======================================================================
 299 //function :
 300 //purpose  :
 301 //=======================================================================
 302  void BRepBlend_SurfCurvEvolRadInv::GetBounds(math_Vector& InfBound,math_Vector& SupBound) const
 303 {
 304   InfBound(1) = guide->FirstParameter();
 305   SupBound(1) = guide->LastParameter();
 306   InfBound(2) = curv->FirstParameter();
 307   SupBound(2) = curv->LastParameter();
 308   InfBound(3) = rst->FirstParameter();
 309   SupBound(3) = rst->LastParameter();
 310 }
 311
 312 //=======================================================================
 313 //function :
 314 //purpose  :
 315 //=======================================================================
 316  Standard_Boolean BRepBlend_SurfCurvEvolRadInv::IsSolution(const math_Vector& Sol,const Standard_Real Tol)
 317 {
 318   math_Vector valsol(1,3);
 319   Value(Sol,valsol);
 320   if (Abs(valsol(1)) <= Tol &&
 321       Abs(valsol(2)) <= Tol &&
 322       Abs(valsol(3)) <= 2*Tol*Abs(ray)) {
 323     return Standard_True;
 324   }
 325   return Standard_False;
 326 }
 327