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