src/FairCurve/FairCurve_DistributionOfJerk.cxx

   1 // Created on: 1996-04-01
   2 // Created by: Philippe MANGIN
   3 // Copyright (c) 1996-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 // 01-04-1996 : PMN Version originale
  18
  19 #ifndef OCCT_DEBUG
  20 #define No_Standard_RangeError
  21 #define No_Standard_OutOfRange
  22 #endif
  23
  24
  25 #include <BSplCLib.hxx>
  26 #include <FairCurve_BattenLaw.hxx>
  27 #include <FairCurve_DistributionOfJerk.hxx>
  28 #include <gp_Pnt2d.hxx>
  29 #include <gp_XY.hxx>
  30 #include <math_Matrix.hxx>
  31 #include <math_Vector.hxx>
  32
  33 FairCurve_DistributionOfJerk::FairCurve_DistributionOfJerk(const Standard_Integer BSplOrder,
  34                                                            const Handle(TColStd_HArray1OfReal)& FlatKnots,
  35                                                            const Handle(TColgp_HArray1OfPnt2d)& Poles,
  36                                                            const Standard_Integer DerivativeOrder,
  37                                                            const FairCurve_BattenLaw& Law,
  38                                                            const Standard_Integer NbValAux) :
  39                                   FairCurve_DistributionOfEnergy( BSplOrder,
  40                                                                   FlatKnots,
  41                                                                   Poles,
  42                                                                   DerivativeOrder,
  43                                                                   NbValAux) ,
  44                                    MyLaw (Law)
  45 {
  46 }
  47
  48
  49 Standard_Boolean FairCurve_DistributionOfJerk::Value(const math_Vector& TParam,
  50                                                            math_Vector& Jerk)
  51 {
  52   Standard_Boolean Ok = Standard_True;
  53   Standard_Integer ier, ii, jj, kk;
  54   gp_XY CPrim  (0., 0.), CSecn (0., 0.), CTroi(0., 0.);
  55   Standard_Integer  LastGradientIndex, FirstNonZero, LastZero;
  56
  57   // (0.0) initialisations generales
  58   Jerk.Init(0.0);
  59   math_Matrix Base(1, 5, 1, MyBSplOrder ); // On shouhaite utiliser la derive troisieme
  60                                            // Dans EvalBsplineBasis C"' <=> DerivOrder = 4
  61                                            // et il faut ajouter 1 rang dans la matrice Base => 5 rangs
  62
  63   ier  =  BSplCLib::EvalBsplineBasis(3,  MyBSplOrder,
  64                                      MyFlatKnots->Array1(), TParam(TParam.Lower()),
  65                                      FirstNonZero, Base );
  66   if (ier != 0) return Standard_False;
  67   LastZero = FirstNonZero - 1;
  68   FirstNonZero = 2*LastZero+1;
  69
  70
  71   // (0.1) evaluation de CPrim et CScn
  72   for (ii= 1; ii<= MyBSplOrder; ii++) {
  73       CPrim += Base(2, ii) * MyPoles->Value(ii+LastZero).Coord();
  74       CSecn += Base(3, ii) * MyPoles->Value(ii+LastZero).Coord();
  75       CTroi += Base(4, ii) * MyPoles->Value(ii+LastZero).Coord();
  76       }
  77
  78   // (1) Evaluation de la secousse locale = W*W
  79   Standard_Real NormeCPrim = CPrim.Modulus();
  80   Standard_Real InvNormeCPrim = 1 / NormeCPrim;
  81   Standard_Real InvNormeCPrim2 = InvNormeCPrim *InvNormeCPrim;
  82   Standard_Real ProduitC1C2 =  CPrim*CSecn;
  83   Standard_Real DeriveNormeCPrim = ProduitC1C2 * InvNormeCPrim2;
  84
  85   Standard_Real Hauteur, WVal, Mesure;
  86   Standard_Real NumRho = CPrim ^ CSecn;
  87   Standard_Real Numerateur = (CPrim ^ CTroi) - 3 * NumRho * DeriveNormeCPrim;
  88   Standard_Real Denominateur = pow ( NormeCPrim, 2.5);
  89
  90   Ok = MyLaw.Value (TParam(TParam.Lower()), Hauteur);
  91   if (!Ok) return Ok;
  92
  93   Mesure =  pow(Hauteur, 3) / 12;
  94   WVal   =  Numerateur / Denominateur;
  95   Jerk(Jerk.Lower()) = Mesure * pow(WVal, 2);
  96
  97   if (MyDerivativeOrder >= 1) {
  98   // (2) Evaluation du gradient de la secousse locale.
  99
 100       math_Vector WGrad (1, 2*MyBSplOrder+MyNbValAux),
 101                   NumGrad(1, 2*MyBSplOrder),
 102                   NGrad1(1, 2*MyBSplOrder),
 103                   NGrad2(1, 2*MyBSplOrder),
 104                   GradNormeCPrim(1, 2*MyBSplOrder),
 105                   NGDNCPrim(1, 2*MyBSplOrder),
 106                   GradDeriveNormeCPrim(1, 2*MyBSplOrder),
 107                   GradNumRho(1, 2*MyBSplOrder),
 108                   NumduGrad(1, 2*MyBSplOrder);
 109       Standard_Real Facteur;
 110       Standard_Real XPrim = CPrim.X();
 111       Standard_Real YPrim = CPrim.Y();
 112       Standard_Real XSecn = CSecn.X();
 113       Standard_Real YSecn = CSecn.Y();
 114       Standard_Real XTroi = CTroi.X();
 115       Standard_Real YTroi = CTroi.Y();
 116       Standard_Real InvDenominateur = 1 / Denominateur;
 117       Standard_Real Aux, AuxBis;
 118
 119       Facteur = 2 * Mesure * WVal;
 120       Aux = 2.5 * Numerateur * InvNormeCPrim;
 121       AuxBis = 2 * ProduitC1C2 * InvNormeCPrim;
 122       kk = Jerk.Lower() + FirstNonZero;
 123
 124
 125       jj = 1;
 126       for (ii=1; ii<=MyBSplOrder; ii++) {
 127
 128    //     (2.1) Derivation en X
 129              GradNormeCPrim(jj) =  XPrim * Base(2, ii) * InvNormeCPrim;
 130              NGDNCPrim(jj) = (XPrim * Base(3, ii) + XSecn * Base(2, ii))
 131                            -  AuxBis * GradNormeCPrim(jj);
 132              GradDeriveNormeCPrim(jj) =  NGDNCPrim(jj) * InvNormeCPrim2;
 133              GradNumRho(jj) = YSecn * Base(2, ii) - YPrim * Base(3, ii);
 134              NGrad1(jj) = YTroi * Base(2, ii) - YPrim * Base(4, ii);
 135              NGrad2(jj) =  - 3 * (  NumRho         * GradDeriveNormeCPrim(jj)
 136                                   + GradNumRho(jj) * DeriveNormeCPrim);
 137              NumGrad(jj) =  NGrad1(jj) + NGrad2(jj);
 138              NumduGrad(jj) =  NumGrad(jj) - Aux * GradNormeCPrim(jj);
 139              WGrad(jj)   = InvDenominateur * NumduGrad(jj);
 140              Jerk(kk) = Facteur *  WGrad(jj);
 141
 142              jj +=1;
 143
 144    //     (2.2) Derivation en Y
 145
 146              GradNormeCPrim(jj) = YPrim * Base(2, ii) * InvNormeCPrim;
 147              NGDNCPrim(jj) = (YPrim * Base(3, ii) + YSecn * Base(2, ii))
 148                            -  AuxBis * GradNormeCPrim(jj);
 149              GradDeriveNormeCPrim(jj) =  NGDNCPrim(jj)  * InvNormeCPrim2;
 150              GradNumRho(jj) =  - XSecn * Base(2, ii) + XPrim * Base(3, ii);
 151              NGrad1(jj) =  - XTroi * Base(2, ii) + XPrim * Base(4, ii);
 152              NGrad2(jj) =  - 3 * (  NumRho         * GradDeriveNormeCPrim(jj)
 153                                  +  GradNumRho(jj) * DeriveNormeCPrim);
 154              NumGrad(jj) =  NGrad1(jj) + NGrad2(jj);
 155              NumduGrad(jj) =  NumGrad(jj) - Aux * GradNormeCPrim(jj);
 156              WGrad(jj)   = InvDenominateur * NumduGrad(jj);
 157              Jerk(kk+1) = Facteur *  WGrad(jj);
 158
 159              jj += 1;
 160              kk += 2;
 161       }
 162       if (MyNbValAux == 1) {
 163     //    (2.3) Gestion de la variable de glissement
 164              LastGradientIndex = Jerk.Lower() + 2*MyPoles->Length() + 1;
 165              WGrad( WGrad.Upper()) = 0.0;
 166              Jerk(LastGradientIndex) = 0.0;
 167        }
 168
 169       else { LastGradientIndex = Jerk.Lower() + 2*MyPoles->Length(); }
 170
 171
 172       if (MyDerivativeOrder >= 2) {
 173
 174 // (3) Evaluation du Hessien de la tension locale ----------------------
 175
 176
 177          Standard_Real FacteurX =  (1 - Pow(XPrim * InvNormeCPrim,2)) * InvNormeCPrim;
 178          Standard_Real FacteurY =  (1 - Pow(YPrim * InvNormeCPrim,2)) * InvNormeCPrim;
 179          Standard_Real FacteurXY = - (XPrim*InvNormeCPrim)
 180                                  * (YPrim*InvNormeCPrim) * InvNormeCPrim;
 181          Standard_Real FacteurW = WVal * InvNormeCPrim;
 182
 183          Standard_Real Produit, Produit2, ProduitV, HNumRho, DSeconde, NSeconde,
 184                        HDeriveNormeCPrim, Aux1, DeriveAuxBis;
 185          Standard_Real VIntermed;
 186          Standard_Integer k1, k2, II, JJ;
 187
 188          Facteur = 2 * Mesure;
 189
 190          kk = FirstNonZero;
 191          k2 = LastGradientIndex + (kk-1)*kk/2;
 192
 193          for (ii=2; ii<= 2*MyBSplOrder; ii+=2) {
 194            II = ii/2;
 195            k1 = k2+FirstNonZero;
 196            k2 = k1+kk;
 197            kk += 2;
 198            for (jj=2; jj< ii; jj+=2) {
 199              JJ = jj/2;
 200              Produit =  Base(2, II) *  Base(2, JJ);
 201              Produit2 = Base(2, II) *  Base(3, JJ) +  Base(3, II) *  Base(2, JJ);
 202              ProduitV =   Base(2, II) *  Base(4, JJ)
 203                         - Base(4, II) *  Base(2, JJ);
 204              HNumRho  =   Base(2, II) *  Base(3, JJ)
 205                         - Base(3, II) *  Base(2, JJ);
 206
 207              // derivation en XiXj
 208              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(jj-1);
 209              DeriveAuxBis = 2 * ( (XPrim * Base(3, JJ) + XSecn * Base(2, JJ))*InvNormeCPrim
 210                                  - ProduitC1C2 * Aux1);
 211              HDeriveNormeCPrim =  ( Produit2 - DeriveAuxBis*GradNormeCPrim(ii-1)
 212                                   - AuxBis * ( Produit * InvNormeCPrim
 213                                   - XPrim * Base(2, II)*Aux1) )*InvNormeCPrim2
 214                                - 2*NGDNCPrim(ii-1) * Aux1 *InvNormeCPrim;
 215              NSeconde = - 3 * (
 216                         GradNumRho(ii-1) * GradDeriveNormeCPrim(jj-1)
 217                       + NumRho * HDeriveNormeCPrim
 218                       + GradNumRho(jj-1) * GradDeriveNormeCPrim(ii-1) );
 219
 220              DSeconde =  FacteurX * Produit;
 221              Aux = NormeCPrim * NSeconde
 222                  + NumGrad(ii-1)*GradNormeCPrim(jj-1)
 223                  - 2.5 * ( NumGrad(jj-1)*GradNormeCPrim(ii-1)
 224                          + DSeconde * Numerateur );
 225              VIntermed = InvDenominateur
 226                        * (Aux - 3.5*GradNormeCPrim(jj-1)*NumduGrad(ii-1));
 227
 228              Jerk(k1) = Facteur * ( WGrad(ii-1)*WGrad(jj-1)
 229                                   + FacteurW * VIntermed);
 230              k1++;
 231
 232              // derivation en XiYj
 233              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(jj);
 234              DeriveAuxBis = 2 * ( (YPrim * Base(3, JJ) + YSecn * Base(2, JJ))*InvNormeCPrim
 235                                  - ProduitC1C2 * Aux1);
 236              HDeriveNormeCPrim =  (- DeriveAuxBis*GradNormeCPrim(ii-1)
 237                                    + AuxBis *  XPrim * Base(2, II) * Aux1) * InvNormeCPrim2
 238                                - 2*NGDNCPrim(ii-1)*Aux1*InvNormeCPrim;
 239              NSeconde = ProduitV
 240                       - 3 * (
 241                         GradNumRho(ii-1) * GradDeriveNormeCPrim(jj)
 242                       + NumRho * HDeriveNormeCPrim
 243                       + GradNumRho(jj) * GradDeriveNormeCPrim(ii-1)
 244                       + HNumRho * DeriveNormeCPrim );
 245              DSeconde =  FacteurXY * Produit;
 246              Aux = NormeCPrim * NSeconde
 247                  + NumGrad(ii-1)*GradNormeCPrim(jj)
 248                  - 2.5 * ( NumGrad(jj)*GradNormeCPrim(ii-1)
 249                          + DSeconde * Numerateur );
 250              VIntermed = InvDenominateur
 251                        * (Aux - 3.5*GradNormeCPrim(jj)*NumduGrad(ii-1));
 252              Jerk(k1) = Facteur * ( WGrad(ii-1)*WGrad(jj)
 253                                      + FacteurW * VIntermed);
 254              k1++;
 255
 256              // derivation en YiXj
 257              // DSeconde calcule ci-dessus
 258              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(jj-1);
 259              DeriveAuxBis = 2 * ( (XPrim * Base(3, JJ) + XSecn * Base(2, JJ))*InvNormeCPrim
 260                                  - ProduitC1C2 * Aux1);
 261              HDeriveNormeCPrim =  (- DeriveAuxBis*GradNormeCPrim(ii)
 262                                   + AuxBis * YPrim * Base(2, II)*Aux1) * InvNormeCPrim2
 263                                - 2*NGDNCPrim(ii)*Aux1 *InvNormeCPrim;
 264              NSeconde = - ProduitV
 265                       - 3 * (
 266                         GradNumRho(ii) * GradDeriveNormeCPrim(jj-1)
 267                       + NumRho * HDeriveNormeCPrim
 268                       + GradNumRho(jj-1) * GradDeriveNormeCPrim(ii)
 269                       - HNumRho * DeriveNormeCPrim);
 270              Aux = NormeCPrim * NSeconde
 271                    + NumGrad(ii)*GradNormeCPrim(jj-1)
 272                    - 2.5 * ( NumGrad(jj-1)*GradNormeCPrim(ii)
 273                          + DSeconde * Numerateur );
 274              VIntermed = InvDenominateur
 275                        * (Aux - 3.5*GradNormeCPrim(jj-1)*NumduGrad(ii));
 276
 277              Jerk(k2) = Facteur * ( WGrad(ii)*WGrad(jj-1)
 278                                      + FacteurW * VIntermed);
 279              k2++;
 280
 281              // derivation en YiYj
 282              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(jj);
 283              DeriveAuxBis = 2 * ( (YPrim * Base(3, JJ) + YSecn * Base(2, JJ))*InvNormeCPrim
 284                                  - ProduitC1C2 * Aux1);
 285              HDeriveNormeCPrim =  ( Produit2 - DeriveAuxBis*GradNormeCPrim(ii)
 286                                   - AuxBis * ( Produit * InvNormeCPrim
 287                                   - YPrim * Base(2, II)*Aux1) )*InvNormeCPrim2
 288                                - 2*NGDNCPrim(ii)*Aux1 *InvNormeCPrim;
 289              NSeconde = - 3 * (
 290                         GradNumRho(ii) * GradDeriveNormeCPrim(jj)
 291                       + NumRho * HDeriveNormeCPrim
 292                       + GradNumRho(jj) * GradDeriveNormeCPrim(ii) );
 293
 294              DSeconde =  FacteurY * Produit;
 295              Aux = NSeconde * NormeCPrim
 296                  + NumGrad(ii)*GradNormeCPrim(jj)
 297                  - 2.5 * ( NumGrad(jj)*GradNormeCPrim(ii)
 298                          + DSeconde * Numerateur );
 299              VIntermed = InvDenominateur
 300                        * (Aux - 3.5*GradNormeCPrim(jj)*NumduGrad(ii));
 301              Jerk(k2) = Facteur * ( WGrad(ii)*WGrad(jj)
 302                                      + FacteurW * VIntermed);
 303              k2++;
 304              }
 305
 306          // cas ou jj = ii : remplisage en triangle
 307              Produit =  pow (Base(2, II), 2);
 308              Produit2 = 2 * Base(2, II) *  Base(3, II);
 309   //           ProduitV2 = Base
 310
 311
 312              // derivation en XiXi
 313              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(ii-1);
 314              DeriveAuxBis = 2 * ( (XPrim * Base(3, II) + XSecn * Base(2, II))*InvNormeCPrim
 315                                  - ProduitC1C2 * Aux1);
 316              HDeriveNormeCPrim =  ( Produit2 - DeriveAuxBis*GradNormeCPrim(ii-1)
 317                                   - AuxBis * ( Produit * InvNormeCPrim
 318                                   - XPrim * Base(2, II)*Aux1) )*InvNormeCPrim2
 319                                - 2*NGDNCPrim(ii-1)*Aux1 *InvNormeCPrim;
 320              NSeconde = - 3 * (
 321                               2 * GradNumRho(ii-1) * GradDeriveNormeCPrim(ii-1)
 322                               + NumRho * HDeriveNormeCPrim );
 323              DSeconde = FacteurX * Produit;
 324              Aux =  NSeconde * NormeCPrim
 325                   - 1.5 * NumGrad(ii-1)*GradNormeCPrim(ii-1)
 326                   - 2.5 * DSeconde * Numerateur;
 327              VIntermed = InvDenominateur
 328                        * (Aux - 3.5*GradNormeCPrim(ii-1)*NumduGrad(ii-1));
 329              Jerk(k1) = Facteur * ( WGrad(ii-1)*WGrad(ii-1)
 330                                      + FacteurW * VIntermed );
 331              // derivation en XiYi
 332              Aux1 =  InvNormeCPrim2 * GradNormeCPrim(ii);
 333              DeriveAuxBis = 2 * ( (YPrim * Base(3, II) + YSecn * Base(2, II))*InvNormeCPrim
 334                                  - ProduitC1C2 * Aux1);
 335              HDeriveNormeCPrim =  ( - DeriveAuxBis * GradNormeCPrim(ii-1)
 336                                     + AuxBis * XPrim * Base(2, II) * Aux1 ) * InvNormeCPrim2
 337                                - 2*NGDNCPrim(ii-1) * Aux1 *InvNormeCPrim;
 338              NSeconde = -3 * (
 339                                 GradNumRho(ii-1) * GradDeriveNormeCPrim(ii)
 340                               + NumRho * HDeriveNormeCPrim
 341                               + GradNumRho(ii) * GradDeriveNormeCPrim(ii-1) );
 342              DSeconde = FacteurXY * Produit;
 343              Aux = NSeconde * NormeCPrim
 344                  + NumGrad(ii)*GradNormeCPrim(ii-1)
 345                  - 2.5 * ( NumGrad(ii-1)*GradNormeCPrim(ii)
 346                          + DSeconde * Numerateur );
 347              VIntermed = InvDenominateur
 348                        * (Aux- 3.5*GradNormeCPrim(ii-1)*NumduGrad(ii));
 349              Jerk(k2) = Facteur * ( WGrad(ii)*WGrad(ii-1)
 350                                      + FacteurW * VIntermed);
 351              k2++;
 352
 353              // derivation en YiYi
 354              // Aux1 et DeriveAuxBis calcules au pas precedent ...
 355              HDeriveNormeCPrim =  ( Produit2 - DeriveAuxBis*GradNormeCPrim(ii)
 356                                   - AuxBis * ( Produit * InvNormeCPrim
 357                                   - YPrim * Base(2, II)*Aux1) )*InvNormeCPrim2
 358                                - 2*NGDNCPrim(ii)*Aux1 *InvNormeCPrim;
 359              NSeconde = - 3 * (
 360                               2 * GradNumRho(ii) * GradDeriveNormeCPrim(ii)
 361                               + NumRho * HDeriveNormeCPrim );
 362              DSeconde = FacteurY * Produit;
 363              Aux =  NSeconde * NormeCPrim
 364                  - 1.5 * NumGrad(ii)*GradNormeCPrim(ii)
 365                  - 2.5 * DSeconde * Numerateur;
 366              VIntermed = InvDenominateur
 367                        * (Aux - 3.5*GradNormeCPrim(ii)*NumduGrad(ii));
 368              Jerk(k2) = Facteur * ( WGrad(ii)*WGrad(ii)
 369                                      + FacteurW * VIntermed);
 370          }
 371        }
 372   }
 373
 374 // sortie standard
 375   return Ok;
 376 }
 377