src/BlendFunc/BlendFunc_ConstThroatWithPenetrationInv.cxx

   1 // Created by: Julia GERASIMOVA
   2 // Copyright (c) 2015 OPEN CASCADE SAS
   3 //
   4 // This file is part of Open CASCADE Technology software library.
   5 //
   6 // This library is free software; you can redistribute it and/or modify it under
   7 // the terms of the GNU Lesser General Public License version 2.1 as published
   8 // by the Free Software Foundation, with special exception defined in the file
   9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
  10 // distribution for complete text of the license and disclaimer of any warranty.
  11 //
  12 // Alternatively, this file may be used under the terms of Open CASCADE
  13 // commercial license or contractual agreement.
  14
  15
  16 #include <Adaptor2d_HCurve2d.hxx>
  17 #include <Adaptor3d_HCurve.hxx>
  18 #include <Adaptor3d_HSurface.hxx>
  19 #include <BlendFunc.hxx>
  20 #include <BlendFunc_ConstThroatWithPenetrationInv.hxx>
  21 #include <math_Matrix.hxx>
  22 #include <Precision.hxx>
  23
  24 //=======================================================================
  25 //function : BlendFunc_ConstThroatInv
  26 //purpose  :
  27 //=======================================================================
  28
  29 BlendFunc_ConstThroatWithPenetrationInv::
  30 BlendFunc_ConstThroatWithPenetrationInv(const Handle(Adaptor3d_HSurface)& S1,
  31                                         const Handle(Adaptor3d_HSurface)& S2,
  32                                         const Handle(Adaptor3d_HCurve)&   C)
  33   : BlendFunc_ConstThroatInv(S1,S2,C)
  34 {
  35 }
  36
  37
  38 //=======================================================================
  39 //function : IsSolution
  40 //purpose  :
  41 //=======================================================================
  42
  43 Standard_Boolean BlendFunc_ConstThroatWithPenetrationInv::IsSolution(const math_Vector& Sol,
  44                                                                      const Standard_Real Tol)
  45 {
  46   math_Vector valsol(1,4);
  47   Value(Sol, valsol);
  48
  49   if (Abs(valsol(1)) <= Tol &&
  50       Abs(valsol(2)) <= Tol &&
  51       Abs(valsol(3)) <= Tol*Tol &&
  52       Abs(valsol(4)) <= Tol)
  53     return Standard_True;
  54
  55   return Standard_False;;
  56 }
  57
  58 //=======================================================================
  59 //function : Value
  60 //purpose  :
  61 //=======================================================================
  62
  63 Standard_Boolean BlendFunc_ConstThroatWithPenetrationInv::Value(const math_Vector& X,
  64                                                                 math_Vector& F)
  65 {
  66   gp_Pnt2d p2d;
  67   gp_Vec2d v2d;
  68   csurf->D1(X(1),p2d,v2d);
  69   param = X(2);
  70   curv->D2(param,ptgui,d1gui,d2gui);
  71   normtg = d1gui.Magnitude();
  72   nplan  = d1gui.Normalized();
  73   theD = - (nplan.XYZ().Dot(ptgui.XYZ()));
  74
  75   math_Vector XX(1,4);
  76
  77   if(first){
  78     XX(1) = p2d.X(); XX(2) = p2d.Y();
  79     XX(3) = X(3); XX(4) = X(4);
  80   }
  81
  82   else{
  83     XX(1) = X(3); XX(2) = X(4);
  84     XX(3) = p2d.X(); XX(4) = p2d.Y();
  85   }
  86
  87   surf1->D0( XX(1), XX(2), pts1 );
  88   surf2->D0( XX(3), XX(4), pts2 );
  89
  90   F(1) = nplan.XYZ().Dot(pts1.XYZ()) + theD;
  91   F(2) = nplan.XYZ().Dot(pts2.XYZ()) + theD;
  92
  93   const gp_Vec vref(ptgui, pts1);
  94
  95   F(3) = vref.SquareMagnitude() - Throat*Throat;
  96
  97   const gp_Vec vec12(pts1, pts2);
  98
  99   F(4) = vref.Dot(vec12);
 100
 101   return Standard_True;
 102 }
 103
 104 //=======================================================================
 105 //function : Derivatives
 106 //purpose  :
 107 //=======================================================================
 108
 109 Standard_Boolean BlendFunc_ConstThroatWithPenetrationInv::Derivatives(const math_Vector& X,
 110                                                                       math_Matrix& D)
 111 {
 112   //Standard_Integer i, j;
 113   gp_Pnt2d p2d;
 114   gp_Vec2d v2d; //, df1, df2;
 115   //gp_Pnt pts, ptgui;
 116   gp_Vec dnplan, temp, temp1, temp2, temp3; //, d1u, d1v, nplan;
 117   math_Vector XX(1,4); //x1(1,2), x2(1,2);
 118   //math_Matrix d1(1,2,1,2), d2(1,2,1,2);
 119
 120   csurf->D1(X(1), p2d, v2d);
 121   //corde1.SetParam(X(2));
 122   //corde2.SetParam(X(2));
 123   param = X(2);
 124   curv->D2(param,ptgui,d1gui,d2gui);
 125   normtg = d1gui.Magnitude();
 126   nplan  = d1gui.Normalized();
 127   theD = - (nplan.XYZ().Dot(ptgui.XYZ()));
 128
 129   dnplan.SetLinearForm(1./normtg,d2gui,
 130                        -1./normtg*(nplan.Dot(d2gui)),nplan);
 131
 132   temp1.SetXYZ(pts1.XYZ() - ptgui.XYZ());
 133   temp2.SetXYZ(pts2.XYZ() - ptgui.XYZ());
 134   temp3.SetXYZ(pts2.XYZ() - pts1.XYZ());
 135
 136   //x1(1) = p2d.X(); x1(2) = p2d.Y();
 137   //x2(1) = X(3); x2(2) = X(4);
 138   if (first)
 139   {
 140     XX(1) = p2d.X(); XX(2) = p2d.Y();
 141     XX(3) = X(3); XX(4) = X(4);
 142   }
 143   else
 144   {
 145     XX(1) = X(3); XX(2) = X(4);
 146     XX(3) = p2d.X(); XX(4) = p2d.Y();
 147   }
 148
 149   surf1->D1(XX(1), XX(2), pts1, d1u1, d1v1);
 150   surf2->D1(XX(3), XX(4), pts2, d1u2, d1v2);
 151
 152   if( first ){
 153     // p2d = pts est sur surf1
 154     //ptgui = corde1.PointOnGuide();
 155     //nplan = corde1.NPlan();
 156     temp.SetLinearForm(v2d.X(),d1u1, v2d.Y(),d1v1);
 157
 158     D(1,1) = nplan.Dot(temp);
 159     D(2,1) = 0.;
 160     //D(3,1) = 2*gp_Vec(ptgui,pts1).Dot(temp);
 161     D(3,1) = 2*temp1.Dot(temp);
 162     //D(4,1) = temp.Dot(gp_Vec(pts1,pts2)) - temp.Dot(gp_Vec(ptgui,pts1));
 163     D(4,1) = temp.Dot(temp3) - temp.Dot(temp1);
 164
 165     D(1,3) = 0.;
 166     D(1,4) = 0.;
 167     D(2,3) = nplan.Dot(d1u2);
 168     D(2,4) = nplan.Dot(d1v2);
 169     D(3,3) = 0.;
 170     D(3,4) = 0.;
 171     //D(4,3) = gp_Vec(ptgui,pts1).Dot(d1u2);
 172     D(4,3) = temp1.Dot(d1u2);
 173     //D(4,4) = gp_Vec(ptgui,pts1).Dot(d1v2);
 174     D(4,4) = temp1.Dot(d1v2);
 175
 176     //surf1->D1(x1(1),x1(2),pts,d1u,d1v);
 177   }
 178   else{
 179     //  p2d = pts est sur surf2
 180     //ptgui = corde2.PointOnGuide();
 181     //nplan = corde2.NPlan();
 182     temp.SetLinearForm(v2d.X(),d1u2, v2d.Y(),d1v2);
 183
 184     D(1,1) = 0.;
 185     D(2,1) = nplan.Dot(temp);
 186     D(3,1) = 0.;
 187     //D(4,1) = gp_Vec(ptgui,pts1).Dot(temp);
 188     D(4,1) = temp1.Dot(temp);
 189
 190     D(1,3) = nplan.Dot(d1u1);
 191     D(1,4) = nplan.Dot(d1v1);
 192     D(2,3) = 0.;
 193     D(2,4) = 0.;
 194     //D(3,3) = 2.*gp_Vec(ptgui,pts1).Dot(d1u1);
 195     D(3,3) = 2.*temp1.Dot(d1u1);
 196     //D(3,4) = 2.*gp_Vec(ptgui,pts1).Dot(d1v1);
 197     D(3,4) = 2.*temp1.Dot(d1v1);
 198     //D(4,3) = d1u1.Dot(gp_Vec(pts1,pts2)) - d1u1.Dot(gp_Vec(ptgui,pts1));
 199     D(4,3) = d1u1.Dot(temp3) - d1u1.Dot(temp1);
 200     D(4,4) = d1v1.Dot(temp3) - d1v1.Dot(temp1);
 201
 202     //surf2->D1(x1(1),x1(2),pts,d1u,d1v);
 203   }
 204
 205   D(1,2) = dnplan.Dot(temp1) - nplan.Dot(d1gui);
 206   D(2,2) = dnplan.Dot(temp2) - nplan.Dot(d1gui);
 207   //D(3,2) = -2.*gp_Vec(ptgui,pts1).Dot(d1gui);
 208   D(3,2) = -2.*d1gui.Dot(temp1);
 209   //D(4,2) = -(gp_Vec(pts1,pts2).Dot(d1gui));
 210   D(4,2) = -d1gui.Dot(temp3);
 211
 212   return Standard_True;
 213 }