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